started on wristevator sim motors.

simgui-ds.json

@@ -16,15 +16,21 @@
           "incKey": 83
         },
         {
+          "decKey": 264,
+          "incKey": 265,
           "keyRate": 0.009999999776482582
         },
         {},
         {
           "decKey": 263,
           "incKey": 262
+        },
+        {
+          "decKey": 89,
+          "incKey": 84
         }
       ],
-      "axisCount": 5,
+      "axisCount": 6,
       "buttonCount": 4,
       "buttonKeys": [
         67,
@@ -68,13 +74,6 @@
       "povCount": 0
     },
     {
-      "axisConfig": [
-        {},
-        {
-          "decKey": 265,
-          "incKey": 264
-        }
-      ],
       "axisCount": 2,
       "buttonCount": 6,
       "buttonKeys": [
@@ -94,12 +93,9 @@
     }
   ],
   "robotJoysticks": [
+    {},
     {
       "guid": "Keyboard0"
-    },
-    {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
     }
   ]
 }

src/main/java/frc/robot/Constants.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.units.AngularAccelerationUnit;
 import edu.wpi.first.units.AngularVelocityUnit;
 import edu.wpi.first.units.VoltageUnit;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
@@ -106,17 +107,46 @@ public static class IntakeConstants {
   }
 
   public static class WristevatorConstants {
-    // temporary values
-    public static final AngularVelocity maxWristSpeed = RadiansPerSecond.of(Math.PI);
-    public static final LinearVelocity maxElevatorSpeed = MetersPerSecond.of(1);
+    public static final AngularVelocity maxWristSpeed = RadiansPerSecond.of(14.039351785273068);
+    public static final AngularVelocity maxElevatorSpeed = RadiansPerSecond.of(70.19675892636535);
 
     public static final int homeSwitch = 0;
 
     public static final int leftMotorId = 10;
     public static final int rightMotorId = 11;
     public static final int wristMotorId = 12;
 
-    public static final Distance drumCircumference = Meters.of(2);
+    public static final double elevatorGearRatio = 9;
+
+    public static final Distance drumRadius = Inches.of(1.504 / 2);
+    public static final Distance drumCircumference = drumRadius.times(2 * Math.PI);
+
+    public static final Distance minElevatorHeight = Meters.of(0);
+    public static final Distance maxElevatorHeight = Meters.of(1);
+
+    public static final Distance manipulatorLength = Meters.of(0.18415);
+
+    public static final Angle minWristAngle = Radians.of(-Math.PI / 3);
+    public static final Angle maxWristAngle = Radians.of(Math.PI / 3);
+
+    public static final double wristGearRatio = 45;
+
+    // elevator feedforward for the DRUM
+    // kv is the voltage necessary to spin the drum 1 rad/s
+    // ka is the voltage necessary to accel the drum 1 rad/s^2 (lower since there are 2 motors, the
+    // torque is doubled at a voltage)
+    public static final Per<VoltageUnit, AngularVelocityUnit> elevatorkV =
+        VoltsPerRadianPerSecond.ofNative(0.170948063465262);
+    public static final Per<VoltageUnit, AngularAccelerationUnit> elevatorkA =
+        VoltsPerRadianPerSecondSquared.ofNative(0.01);
+
+    // wrist feedforward for the pivot (after the gear ratio)
+    // kv is the voltage necessary to spin the pivot 1 rad/s
+    // ka is the voltage necessary to accel the pivot 1 rad/s^2
+    public static final Per<VoltageUnit, AngularVelocityUnit> wristkV =
+        VoltsPerRadianPerSecond.ofNative(0.8547403173263101);
+    public static final Per<VoltageUnit, AngularAccelerationUnit> wristkA =
+        VoltsPerRadianPerSecondSquared.ofNative(0.01);
   }
 
   public static class SerializerConstants {

src/main/java/frc/robot/Robot.java

@@ -184,9 +184,11 @@ private void configureDefaultCommands() {
     _wristevator.setDefaultCommand(
         _wristevator.setSpeeds(
             InputStream.of(_operatorController::getRightY)
+                .deadband(0.05, 1)
                 .negate()
-                .scale(WristevatorConstants.maxElevatorSpeed.in(MetersPerSecond)),
+                .scale(WristevatorConstants.maxElevatorSpeed.in(RadiansPerSecond)),
             InputStream.of(_operatorController::getLeftY)
+                .deadband(0.05, 1)
                 .negate()
                 .scale(WristevatorConstants.maxWristSpeed.in(RadiansPerSecond))));
   }

src/main/java/frc/robot/subsystems/Wristevator.java

@@ -3,16 +3,23 @@
 import static edu.wpi.first.units.Units.*;
 
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import dev.doglog.DogLog;
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.simulation.DIOSim;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -64,7 +71,7 @@ public Distance getHeight() {
   private final MechanismRoot2d _root = _mech.getRoot("elevator mount", 1, 0.1);
 
   private final MechanismLigament2d _elevator =
-      _root.append(new MechanismLigament2d("elevator", 0, 90, 3, new Color8Bit(Color.kBlack)));
+      _root.append(new MechanismLigament2d("elevator", 0, 90, 3, new Color8Bit(Color.kCyan)));
 
   private final MechanismLigament2d _wrist =
       _elevator.append(
@@ -87,17 +94,57 @@ public Distance getHeight() {
 
   private DIOSim _homeSwitchSim;
 
+  private ElevatorSim _elevatorSim;
+  private SingleJointedArmSim _wristSim;
+
+  private double _lastSimTime;
+
+  private Notifier _simNotifier;
+
   public Wristevator() {
     if (Robot.isSimulation()) {
       _homeSwitchSim = new DIOSim(_homeSwitch);
-    } else {
-      _homeSwitchSim = null;
+
+      _elevatorSim =
+          new ElevatorSim(
+              DCMotor.getKrakenX60(2),
+              WristevatorConstants.elevatorGearRatio,
+              Units.lbsToKilograms(9.398),
+              WristevatorConstants.drumRadius.in(Meters),
+              0,
+              WristevatorConstants.maxElevatorHeight.in(Meters),
+              false,
+              0);
+
+      _wristSim =
+          new SingleJointedArmSim(
+              DCMotor.getKrakenX60(1),
+              WristevatorConstants.wristGearRatio,
+              SingleJointedArmSim.estimateMOI(
+                  WristevatorConstants.manipulatorLength.in(Meters), Units.lbsToKilograms(8.155)),
+              WristevatorConstants.manipulatorLength.in(Meters),
+              WristevatorConstants.minWristAngle.in(Radians),
+              WristevatorConstants.maxWristAngle.in(Radians),
+              false,
+              0);
+
+      startSimThread();
     }
 
     var leftMotorConfigs = new TalonFXConfiguration();
     var rightMotorConfigs = new TalonFXConfiguration();
     var wristMotorConfigs = new TalonFXConfiguration();
 
+    leftMotorConfigs.Slot0.kV = WristevatorConstants.elevatorkV.in(VoltsPerRadianPerSecond);
+    leftMotorConfigs.Slot0.kA = WristevatorConstants.elevatorkA.in(VoltsPerRadianPerSecondSquared);
+
+    leftMotorConfigs.Feedback.SensorToMechanismRatio = WristevatorConstants.elevatorGearRatio;
+
+    wristMotorConfigs.Slot0.kV = WristevatorConstants.wristkV.in(VoltsPerRadianPerSecond);
+    wristMotorConfigs.Slot0.kA = WristevatorConstants.wristkA.in(VoltsPerRadianPerSecondSquared);
+
+    wristMotorConfigs.Feedback.SensorToMechanismRatio = WristevatorConstants.wristGearRatio;
+
     CTREUtil.attempt(() -> _leftMotor.getConfigurator().apply(leftMotorConfigs), _leftMotor);
     CTREUtil.attempt(() -> _rightMotor.getConfigurator().apply(rightMotorConfigs), _rightMotor);
     CTREUtil.attempt(() -> _wristMotor.getConfigurator().apply(wristMotorConfigs), _wristMotor);
@@ -109,6 +156,64 @@ public Wristevator() {
     _rightMotor.setControl(new Follower(WristevatorConstants.leftMotorId, true));
   }
 
+  private void startSimThread() {
+    _lastSimTime = Utils.getCurrentTimeSeconds();
+
+    _simNotifier =
+        new Notifier(
+            () -> {
+              final double batteryVolts = RobotController.getBatteryVoltage();
+
+              final double currentTime = Utils.getCurrentTimeSeconds();
+              final double deltaTime = currentTime - _lastSimTime;
+
+              var leftMotorSimState = _leftMotor.getSimState();
+              var rightMotorSimState = _rightMotor.getSimState();
+              var wristMotorSimState = _wristMotor.getSimState();
+
+              leftMotorSimState.setSupplyVoltage(batteryVolts);
+              rightMotorSimState.setSupplyVoltage(batteryVolts);
+              wristMotorSimState.setSupplyVoltage(batteryVolts);
+
+              _elevatorSim.setInputVoltage(leftMotorSimState.getMotorVoltageMeasure().in(Volts));
+              _wristSim.setInputVoltage(wristMotorSimState.getMotorVoltageMeasure().in(Volts));
+
+              _elevatorSim.update(deltaTime);
+              _wristSim.update(deltaTime);
+
+              // raw rotor positions
+              leftMotorSimState.setRawRotorPosition(
+                  _elevatorSim.getPositionMeters()
+                      / WristevatorConstants.drumCircumference.in(Meters)
+                      * WristevatorConstants.elevatorGearRatio);
+              rightMotorSimState.setRawRotorPosition(
+                  -_elevatorSim.getPositionMeters()
+                      / WristevatorConstants.drumCircumference.in(Meters)
+                      * WristevatorConstants.elevatorGearRatio);
+              wristMotorSimState.setRawRotorPosition(
+                  Units.radiansToRotations(
+                      _wristSim.getAngleRads() * WristevatorConstants.wristGearRatio));
+
+              // raw rotor velocities
+              leftMotorSimState.setRotorVelocity(
+                  _elevatorSim.getVelocityMetersPerSecond()
+                      / WristevatorConstants.drumCircumference.in(Meters)
+                      * WristevatorConstants.elevatorGearRatio);
+              rightMotorSimState.setRotorVelocity(
+                  -_elevatorSim.getVelocityMetersPerSecond()
+                      / WristevatorConstants.drumCircumference.in(Meters)
+                      * WristevatorConstants.elevatorGearRatio);
+              wristMotorSimState.setRotorVelocity(
+                  Units.radiansToRotations(
+                      _wristSim.getVelocityRadPerSec() * WristevatorConstants.wristGearRatio));
+
+              _lastSimTime = currentTime;
+            });
+
+    _simNotifier.setName("Wristevator Sim Thread");
+    _simNotifier.startPeriodic(1 / Constants.simUpdateFrequency.in(Hertz));
+  }
+
   @Logged(name = "Height")
   public double getHeight() {
     return _heightGetter.refresh().getValue().in(Rotations)
@@ -132,7 +237,12 @@ public Command setSetpoint(WristevatorSetpoint setpoint) {
         .withName("Set Setpoint: " + setpoint.toString());
   }
 
-  /** Control the elevator and wrist speeds individually. */
+  /**
+   * Control the elevator and wrist speeds individually.
+   *
+   * @param elevatorSpeed The elevator drum speed in radians per second.
+   * @param wristSpeed The wrist speed in radians per second.
+   */
   public Command setSpeeds(DoubleSupplier elevatorSpeed, DoubleSupplier wristSpeed) {
     return run(() -> {
           _leftMotor.setControl(_elevatorVelocitySetter.withVelocity(elevatorSpeed.getAsDouble()));
@@ -161,5 +271,7 @@ public void close() {
     _leftMotor.close();
     _rightMotor.close();
     _wristMotor.close();
+
+    _simNotifier.close();
   }
 }