diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 29da16b..5e0a84d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -38,8 +38,6 @@ public class Robot extends LoggedRobot { */ @Override public void robotInit() { - // Record metadata - /* Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); @@ -56,7 +54,7 @@ public void robotInit() { default: Logger.recordMetadata("GitDirty", "Unknown"); break; - } */ + } // Set up data receivers & replay source switch (Constants.currentMode) { diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java index 7f35187..fd44e7d 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -22,7 +22,7 @@ public PivotIOReal() { pivotMotor.setCANTimeout(250); pivotMotor.enableVoltageCompensation(12); - pivotMotor.setSmartCurrentLimit(80); + pivotMotor.setSmartCurrentLimit(30); pivotMotor.setIdleMode(IdleMode.kBrake); pivotMotor.burnFlash(); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..b05ab38 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Shooter extends SubsystemBase { + private ShooterIO io; + private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); + + private static Shooter instance; + + public static Shooter getInstance() { + return instance; + } + + public static Shooter initialize(ShooterIO io) { + if (instance == null) { + instance = new Shooter(io); + } + return instance; + } + + private Shooter(ShooterIO shooterIO) { + io = shooterIO; + io.updateInputs(inputs); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java new file mode 100644 index 0000000..fe15e61 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.shooter; + +import frc.robot.subsystems.shooter.ShooterIO.ShooterIOInputs; +import org.littletonrobotics.junction.AutoLog; + +public interface ShooterIO { + @AutoLog + public static class ShooterIOInputs { + public double leftMotorVelocity = 0.0; + public double rightMotorVelocity = 0.0; + } + + public default void spinForwards() {} + + public default void spinBackwards() {} + + public default void updateInputs(ShooterIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java new file mode 100644 index 0000000..2c110f0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.shooter; + +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; + +public class ShooterIOReal implements ShooterIO { + private CANSparkFlex leftShooterMotor = + new CANSparkFlex(0, MotorType.kBrushless); // Can IDs not accurate + private CANSparkFlex rightShooterMotor = + new CANSparkFlex(1, MotorType.kBrushless); // Can IDs not accurate + private RelativeEncoder leftShooterEncoder = leftShooterMotor.getEncoder(); + private RelativeEncoder rightShooterEncoder = rightShooterMotor.getEncoder(); + + public ShooterIOReal() { + rightShooterMotor.follow(leftShooterMotor, true); + + rightShooterMotor.burnFlash(); + leftShooterMotor.burnFlash(); + } + + @Override + public void spinForwards() { + leftShooterMotor.set(1.0); + } + + @Override + public void spinBackwards() { + leftShooterMotor.set(-1.0); + } + + public double getLeftvelocity() { + return leftShooterEncoder.getVelocity(); + } + + public double getRightvelocity() { + return rightShooterEncoder.getVelocity(); + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.leftMotorVelocity = getLeftvelocity(); + inputs.rightMotorVelocity = getRightvelocity(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java new file mode 100644 index 0000000..fbd83f1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.shooter; + +public class ShooterIOSim {}