diff --git a/simgui-ds.json b/simgui-ds.json index f178b77..e664daf 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -6,18 +11,20 @@ "decKey": 65, "incKey": 68 }, - { - "decKey": 87, - "incKey": 83 - }, + {}, { "decKey": 69, "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 83, + "incKey": 87 } ], - "axisCount": 3, + "axisCount": 6, "buttonCount": 4, "buttonKeys": [ 90, @@ -90,12 +97,10 @@ } ], "robotJoysticks": [ + {}, { "guid": "78696e70757401000000000000000000", "useGamepad": true - }, - { - "useGamepad": true } ] } diff --git a/simgui.json b/simgui.json index c01ca5a..00db703 100644 --- a/simgui.json +++ b/simgui.json @@ -5,6 +5,7 @@ "/Shuffleboard/Auto/Auto Chooser": "String Chooser", "/Shuffleboard/Auto/SendableChooser[0]": "String Chooser", "/Shuffleboard/Vision/Field": "Field2d", + "/SmartDashboard/Arm Mechanism": "Mechanism2d", "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": "Field2d" }, "windows": { @@ -13,6 +14,11 @@ "visible": true } }, + "/SmartDashboard/Arm Mechanism": { + "window": { + "visible": true + } + }, "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": { "window": { "visible": true diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ee03663..f41110e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,9 +1,11 @@ package frc.robot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.lib.Telemetry; import frc.robot.arm.Arm; +import frc.robot.arm.ArmMechanism; import frc.robot.auto.Auto; import frc.robot.climber.Climber; import frc.robot.intake.Intake; @@ -56,6 +58,8 @@ public static RobotContainer getInstance() { private void initializeTelemetry() { Telemetry.initializeShuffleboards( arm, auto, climber, intake, lights, odometry, shooter, swerve, vision); + + SmartDashboard.putData("Arm Mechanism", ArmMechanism.getInstance().getMechanism()); } /** Configures operator controller bindings. */ diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index bc4fa64..c39f747 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -55,6 +55,8 @@ public static Arm getInstance() { @Override public void periodic() { shoulderMotor.update(shoulderMotorValues); + + ArmMechanism.getInstance().setState(getState()); } @Override @@ -103,14 +105,14 @@ public Command hold() { public Command driveShoulderWithJoystick(DoubleSupplier joystickSupplier) { return run(() -> shoulderMotor.setVoltage( - joystickSupplier.getAsDouble() * ShoulderMotorConstants.MAXIMUM_VOLTAGE)) + joystickSupplier.getAsDouble() * -1 * Math.abs(ShoulderMotorConstants.MAXIMUM_VOLTAGE))) .finallyDo(shoulderMotor::stop); } public Command driveElbowWithJoystick(DoubleSupplier joystickSupplier) { return run(() -> elbowMotor.setVoltage( - joystickSupplier.getAsDouble() * ElbowMotorConstants.MAXIMUM_VOLTAGE)) + joystickSupplier.getAsDouble() * -1 * Math.abs(ElbowMotorConstants.MAXIMUM_VOLTAGE))) .finallyDo(shoulderMotor::stop); } } diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index c9bb2d3..58893cd 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -1,5 +1,6 @@ package frc.robot.arm; +import edu.wpi.first.math.util.Units; import frc.lib.CAN; /** Constants for the arm subsystem. */ @@ -12,19 +13,19 @@ 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 MOTOR_IS_INVERTED = true; - - /** If true, invert the shoulder motor's encoder. */ - public static final boolean ENCODER_IS_INVERTED = true; - /** Maximum voltage that can be applied to the shoulder motor. */ public static final double MAXIMUM_VOLTAGE = 2.0; + + /** Shoulder pivot to elbow pivot distance in meters. */ + public static final double SHOULDER_TO_ELBOW_DISTANCE = Units.inchesToMeters(16.775); } /** Constants for the elbow motor. */ public static class ElbowMotorConstants { /** Maximum voltage that can be applied to the elbow motor. */ public static final double MAXIMUM_VOLTAGE = 2.0; + + /** Elbow pivot to wrist pivot distance in meters. */ + public static final double ELBOW_TO_WRIST_DISTANCE = Units.inchesToMeters(16.825); } } diff --git a/src/main/java/frc/robot/arm/ArmFactory.java b/src/main/java/frc/robot/arm/ArmFactory.java index db14d0a..17940c7 100644 --- a/src/main/java/frc/robot/arm/ArmFactory.java +++ b/src/main/java/frc/robot/arm/ArmFactory.java @@ -13,7 +13,7 @@ public class ArmFactory { public static ShoulderMotorIO createShoulderMotor() { if (Robot.isReal()) return new ShoulderMotorIOSparkMax(); - return new ShoulderMotorIOSparkMax(); + return new ShoulderMotorIOSim(); } /** diff --git a/src/main/java/frc/robot/arm/ArmMechanism.java b/src/main/java/frc/robot/arm/ArmMechanism.java new file mode 100644 index 0000000..b689e81 --- /dev/null +++ b/src/main/java/frc/robot/arm/ArmMechanism.java @@ -0,0 +1,48 @@ +package frc.robot.arm; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import frc.robot.arm.ArmConstants.ElbowMotorConstants; +import frc.robot.arm.ArmConstants.ShoulderMotorConstants; + +/** Helper class for rendering the arm using WPILib mechanisms. */ +public class ArmMechanism { + + private static ArmMechanism instance = null; + + private final Mechanism2d mechanism; + + private final MechanismRoot2d root; + + private final MechanismLigament2d shoulder, elbow; + + private final double THICKNESS = Units.inchesToMeters(2) * 100; + + private ArmMechanism() { + mechanism = new Mechanism2d(1, 1); + root = mechanism.getRoot("arm", 0.15, 0); + shoulder = root.append(new MechanismLigament2d("shoulder", ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE, 90, THICKNESS, new Color8Bit(Color.kOrange))); + elbow = shoulder.append(new MechanismLigament2d("elbow", ElbowMotorConstants.ELBOW_TO_WRIST_DISTANCE, 90, THICKNESS, new Color8Bit(Color.kGreen))); + } + + public static ArmMechanism getInstance() { + if (instance == null) { + instance = new ArmMechanism(); + } + + return instance; + } + + public Mechanism2d getMechanism() { + return mechanism; + } + + public void setState(ArmState state) { + shoulder.setAngle(state.shoulder()); + elbow.setAngle(state.elbow().minus(state.shoulder())); + } +} diff --git a/src/main/java/frc/robot/arm/ElbowMotorIOSim.java b/src/main/java/frc/robot/arm/ElbowMotorIOSim.java index 0a1f1e0..9f9ff7d 100644 --- a/src/main/java/frc/robot/arm/ElbowMotorIOSim.java +++ b/src/main/java/frc/robot/arm/ElbowMotorIOSim.java @@ -1,7 +1,7 @@ package frc.robot.arm; /** Simulated elbow motor. */ -public class ElbowMotorIOSim implements ElbowMotorIO { +public class ElbowMotorIOSim implements ElbowMotorIO { @Override public void configure() {} diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java new file mode 100644 index 0000000..d2ab1b5 --- /dev/null +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java @@ -0,0 +1,60 @@ +package frc.robot.arm; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.RobotConstants; +import frc.robot.arm.ArmConstants.ShoulderMotorConstants; + +/** Simulated shoulder motor. */ +public class ShoulderMotorIOSim implements ShoulderMotorIO { + + private final SingleJointedArmSim singleJointedArmSim; + + /** Creates a new simulated shoulder motor. */ + public ShoulderMotorIOSim() { + singleJointedArmSim = + new SingleJointedArmSim( + DCMotor.getNEO(1), + ShoulderMotorConstants.GEARING, + 0.1, + ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE, + Units.degreesToRadians(10), + Units.degreesToRadians(90), + false, + Units.degreesToRadians(90)); + } + + @Override + public void configure() { + } + + @Override + public void update(ShoulderMotorIOValues values) { + singleJointedArmSim.update(RobotConstants.PERIODIC_DURATION); + + values.positionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads()); + values.currentAmps = singleJointedArmSim.getCurrentDrawAmps(); + } + + @Override + public void setPosition(double positionRotations) { + singleJointedArmSim.setState(Units.rotationsToRadians(positionRotations), 0.0); + } + + @Override + public void runSetpoint(double positionRotations) { + //setPosition(positionRotations); + } + + // TODO Remove, only for characterization + @Override + public void setVoltage(double volts) { + singleJointedArmSim.setInputVoltage(volts); + } + + @Override + public void stop() { + setVoltage(0); + } +} diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java index c1f2b9a..2578a55 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java @@ -16,6 +16,7 @@ public class ShoulderMotorIOSparkMax implements ShoulderMotorIO { /** Feedback controller for shoulder position. */ private final PIDController feedback = new PIDController(0, 0, 0); + // private final ProfiledPIDController feedback = // new ProfiledPIDController(0, 0, 0, new Constraints(0, 0)); @@ -32,10 +33,6 @@ public void configure() { Configurator.configureREV(sparkMax::restoreFactoryDefaults); Configurator.configureREV(() -> sparkMax.setIdleMode(IdleMode.kBrake)); - - sparkMax.setInverted(ShoulderMotorConstants.MOTOR_IS_INVERTED); - - Configurator.configureREV(() -> sparkMax.getEncoder().setInverted(ShoulderMotorConstants.ENCODER_IS_INVERTED)); } @Override diff --git a/src/main/java/frc/robot/intake/IntakeFactory.java b/src/main/java/frc/robot/intake/IntakeFactory.java index f2edb56..9621fed 100644 --- a/src/main/java/frc/robot/intake/IntakeFactory.java +++ b/src/main/java/frc/robot/intake/IntakeFactory.java @@ -1,7 +1,5 @@ package frc.robot.intake; -import frc.robot.Robot; - /** Helper class for creating hardware for the intake subsystem. */ public class IntakeFactory { @@ -11,7 +9,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 3e1d49c..392494f 100644 --- a/src/main/java/frc/robot/lights/LightsFactory.java +++ b/src/main/java/frc/robot/lights/LightsFactory.java @@ -1,7 +1,5 @@ package frc.robot.lights; -import frc.robot.Robot; - /** Helper class for creating hardware for the lights subsystem. */ public class LightsFactory { @@ -11,7 +9,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 5b9d840..987f7a3 100644 --- a/src/main/java/frc/robot/odometry/OdometryFactory.java +++ b/src/main/java/frc/robot/odometry/OdometryFactory.java @@ -2,7 +2,6 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import frc.lib.CAN; -import frc.robot.Robot; /** Helper class for creating hardware for the odometry subsystem. */ public class OdometryFactory { @@ -13,7 +12,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 f09f9ab..6a28bee 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 aa3ad79..4585e94 100644 --- a/src/main/java/frc/robot/swerve/SwerveFactory.java +++ b/src/main/java/frc/robot/swerve/SwerveFactory.java @@ -47,7 +47,8 @@ public static CANcoderConfiguration createAzimuthEncoderConfig() { */ public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) { // if (Robot.isReal()) - // return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(), config.moduleCAN().azimuth()); + // return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(), + // config.moduleCAN().azimuth()); return new SteerMotorIOSim(); } @@ -58,7 +59,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(); }