diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index e588542..73414c5 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -5,6 +5,12 @@ public class ArmConstants { /** Constants for the shoulder motor. */ public static class ShoulderMotorConstants { + /** Gearing between the soulder motor and the shoulder joint. */ + public static final double GEARING = 51.2; + + /** If true, invert the shoulder motor. */ + public static final boolean IS_INVERTED = true; + /** Maximum voltage that can be applied to the shoulder motor. */ public static final double MAXIMUM_VOLTAGE = 2.0; } diff --git a/src/main/java/frc/robot/arm/ArmFactory.java b/src/main/java/frc/robot/arm/ArmFactory.java index 4feacfd..db14d0a 100644 --- a/src/main/java/frc/robot/arm/ArmFactory.java +++ b/src/main/java/frc/robot/arm/ArmFactory.java @@ -22,8 +22,6 @@ public static ShoulderMotorIO createShoulderMotor() { * @return an elbow motor. */ public static ElbowMotorIO createElbowMotor() { - if (Robot.isReal()) return new ElbowMotorIOSparkMax(); - - return new ElbowMotorIOSparkMax(); + return new ElbowMotorIOSim(); } } diff --git a/src/main/java/frc/robot/arm/ElbowMotorIOSim.java b/src/main/java/frc/robot/arm/ElbowMotorIOSim.java new file mode 100644 index 0000000..0a1f1e0 --- /dev/null +++ b/src/main/java/frc/robot/arm/ElbowMotorIOSim.java @@ -0,0 +1,23 @@ +package frc.robot.arm; + +/** Simulated elbow motor. */ +public class ElbowMotorIOSim implements ElbowMotorIO { + + @Override + public void configure() {} + + @Override + public void update(ElbowMotorIOValues values) {} + + @Override + public void setPosition(double positionRotations) {} + + @Override + public void runSetpoint(double positionRotations) {} + + @Override + public void setVoltage(double volts) {} + + @Override + public void stop() {} +} diff --git a/src/main/java/frc/robot/arm/ElbowMotorIOSparkMax.java b/src/main/java/frc/robot/arm/ElbowMotorIOSparkMax.java deleted file mode 100644 index c22d69c..0000000 --- a/src/main/java/frc/robot/arm/ElbowMotorIOSparkMax.java +++ /dev/null @@ -1,84 +0,0 @@ -package frc.robot.arm; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import frc.lib.Configurator; -import frc.robot.arm.ArmConstants.ElbowMotorConstants; - -/** Elbow motor using a Spark Max. */ -public class ElbowMotorIOSparkMax implements ElbowMotorIO { - - /** Hardware Spark Max. */ - private final CANSparkMax sparkMax; - - /** Feedback controller for elbow position. */ - private final ProfiledPIDController feedback = - new ProfiledPIDController(0, 0, 0, new Constraints(0, 0)); - - /** Feedforward controller for elbow position. */ - private final ArmFeedforward feedforward = new ArmFeedforward(0, 0, 0); - - /** Creates a new elbow motor using a Spark Max. */ - public ElbowMotorIOSparkMax() { - sparkMax = new CANSparkMax(33, MotorType.kBrushless); - } - - @Override - public void configure() { - Configurator.configureREV(sparkMax::restoreFactoryDefaults); - - Configurator.configureREV(() -> sparkMax.setIdleMode(IdleMode.kBrake)); - } - - @Override - public void update(ElbowMotorIOValues values) { - values.positionRotations = getPositionRotations(); - values.currentAmps = sparkMax.getOutputCurrent(); - } - - @Override - public void setPosition(double positionRotations) { - // TODO - } - - @Override - public void runSetpoint(double positionRotations) { - double measuredPositionRotations = getPositionRotations(); - - double feedbackVolts = feedback.calculate(positionRotations, measuredPositionRotations); - - double feedforwardVolts = - feedforward.calculate(measuredPositionRotations, feedback.getSetpoint().velocity); - - setVoltage(feedbackVolts + feedforwardVolts); - } - - // TODO Remove, only for characterization - @Override - public void setVoltage(double volts) { - volts = - MathUtil.clamp( - volts, -ElbowMotorConstants.MAXIMUM_VOLTAGE, ElbowMotorConstants.MAXIMUM_VOLTAGE); - - sparkMax.setVoltage(volts); - } - - @Override - public void stop() { - setVoltage(0); - } - - /** - * Gets the absolute position of the elbow in rotations. - * - * @return the absolute position of the elbow in rotations. - */ - private double getPositionRotations() { - return sparkMax.getEncoder().getPosition(); - } -} diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java index fa1fb08..962c013 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java @@ -33,6 +33,8 @@ public void configure() { Configurator.configureREV(sparkMax::restoreFactoryDefaults); Configurator.configureREV(() -> sparkMax.setIdleMode(IdleMode.kBrake)); + + sparkMax.setInverted(ShoulderMotorConstants.IS_INVERTED); } @Override @@ -43,7 +45,7 @@ public void update(ShoulderMotorIOValues values) { @Override public void setPosition(double positionRotations) { - // TODO + sparkMax.getEncoder().setPosition(positionRotations * ShoulderMotorConstants.GEARING); } @Override @@ -79,6 +81,6 @@ public void stop() { * @return the absolute position of the shoulder in rotations. */ private double getPositionRotations() { - return sparkMax.getEncoder().getPosition(); + return sparkMax.getEncoder().getPosition() / ShoulderMotorConstants.GEARING; } } diff --git a/src/main/java/frc/robot/intake/IntakeFactory.java b/src/main/java/frc/robot/intake/IntakeFactory.java index 4c827d7..f2edb56 100644 --- a/src/main/java/frc/robot/intake/IntakeFactory.java +++ b/src/main/java/frc/robot/intake/IntakeFactory.java @@ -11,7 +11,7 @@ public class IntakeFactory { * @return a roller motor. */ public static RollerMotorIO createRollerMotor() { - if (Robot.isReal()) return new RollerMotorIOSparkMax(); + //if (Robot.isReal()) return new RollerMotorIOSparkMax(); return new RollerMotorIOSim(); } diff --git a/src/main/java/frc/robot/lights/LightsFactory.java b/src/main/java/frc/robot/lights/LightsFactory.java index 0eabdff..3e1d49c 100644 --- a/src/main/java/frc/robot/lights/LightsFactory.java +++ b/src/main/java/frc/robot/lights/LightsFactory.java @@ -11,7 +11,7 @@ public class LightsFactory { * @return an LED controller. */ public static LEDControllerIO createLEDController() { - if (Robot.isReal()) return new LEDControllerIOCANdle(); + //if (Robot.isReal()) return new LEDControllerIOCANdle(); return new LEDControllerIOSim(); } diff --git a/src/main/java/frc/robot/odometry/OdometryFactory.java b/src/main/java/frc/robot/odometry/OdometryFactory.java index 1290178..5b9d840 100644 --- a/src/main/java/frc/robot/odometry/OdometryFactory.java +++ b/src/main/java/frc/robot/odometry/OdometryFactory.java @@ -13,7 +13,7 @@ public class OdometryFactory { * @return a gyroscope. */ public static GyroscopeIO createGyroscope(CAN gyroscopeCAN, Odometry odometry) { - if (Robot.isReal()) return new GyroscopeIOPigeon2(gyroscopeCAN); + //if (Robot.isReal()) return new GyroscopeIOPigeon2(gyroscopeCAN); return new GyroscopeIOSim(odometry); } diff --git a/src/main/java/frc/robot/shooter/ShooterFactory.java b/src/main/java/frc/robot/shooter/ShooterFactory.java index 2340bdf..f09f9ab 100644 --- a/src/main/java/frc/robot/shooter/ShooterFactory.java +++ b/src/main/java/frc/robot/shooter/ShooterFactory.java @@ -22,7 +22,7 @@ public static BeamBreakSensorIO createBeamBreakSensor() { * @return a serializer motor. */ public static SerializerMotorIO createSerializerMotor() { - if (Robot.isReal()) return new SerializerMotorIOSparkMax(); + //if (Robot.isReal()) return new SerializerMotorIOSparkMax(); return new SerializerMotorIOSim(); } @@ -33,7 +33,7 @@ public static SerializerMotorIO createSerializerMotor() { * @return a flywheel motor. */ public static FlywheelMotorIO createFlywheelMotor() { - if (Robot.isReal()) return new FlywheelMotorIOSparkMax(); // TODO + //if (Robot.isReal()) return new FlywheelMotorIOSparkMax(); // TODO return new FlywheelMotorIOSim(); } diff --git a/src/main/java/frc/robot/swerve/SwerveFactory.java b/src/main/java/frc/robot/swerve/SwerveFactory.java index 47a5c81..aa3ad79 100644 --- a/src/main/java/frc/robot/swerve/SwerveFactory.java +++ b/src/main/java/frc/robot/swerve/SwerveFactory.java @@ -23,8 +23,8 @@ public static SwerveModuleIO createModule(SwerveModuleConfig config) { * @return an azimuth encoder. */ public static AzimuthEncoderIO createAzimuthEncoder(SwerveModuleConfig config) { - if (Robot.isReal()) - return new AzimuthEncoderIOCANcoder(config.moduleCAN().azimuth(), config.offset()); + // if (Robot.isReal()) + // return new AzimuthEncoderIOCANcoder(config.moduleCAN().azimuth(), config.offset()); return new AzimuthEncoderIOSim(); } @@ -46,8 +46,8 @@ public static CANcoderConfiguration createAzimuthEncoderConfig() { * @return a steer motor. */ public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) { - if (Robot.isReal()) - return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(), config.moduleCAN().azimuth()); + // if (Robot.isReal()) + // return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(), config.moduleCAN().azimuth()); return new SteerMotorIOSim(); } @@ -58,7 +58,7 @@ public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) { * @return a drive motor. */ public static DriveMotorIO createDriveMotor(SwerveModuleConfig config) { - if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive()); + //if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive()); return new DriveMotorIOSim(); }