Merge pull request #57 from Team334/units-fixes

made units match ctre for control requests and slot configs

simgui-ds.json

@@ -4,6 +4,11 @@
       "visible": true
     }
   },
+  "System Joysticks": {
+    "window": {
+      "enabled": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [

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

@@ -238,15 +238,15 @@ public Angle getHeight() {
     // 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(1.3);
+        VoltsPerRadianPerSecond.ofNative(0.18);
     public static final Per<VoltageUnit, AngularAccelerationUnit> elevatorkA =
         VoltsPerRadianPerSecondSquared.ofNative(0);
 
     // 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(5);
+        VoltsPerRadianPerSecond.ofNative(0.8);
     public static final Per<VoltageUnit, AngularAccelerationUnit> wristkA =
         VoltsPerRadianPerSecondSquared.ofNative(0);
   }

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

@@ -65,15 +65,16 @@ public Intake() {
     var feedMotorConfigs = new TalonFXConfiguration();
     var actuatorMotorConfigs = new TalonFXConfiguration();
 
-    actuatorMotorConfigs.Slot0.kV = IntakeConstants.actuatorkV.in(VoltsPerRadianPerSecond);
-    actuatorMotorConfigs.Slot0.kA = IntakeConstants.actuatorkA.in(VoltsPerRadianPerSecondSquared);
+    actuatorMotorConfigs.Slot0.kV = IntakeConstants.actuatorkV.in(Volts.per(RotationsPerSecond));
+    actuatorMotorConfigs.Slot0.kA =
+        IntakeConstants.actuatorkA.in(Volts.per(RotationsPerSecondPerSecond));
 
     actuatorMotorConfigs.Feedback.SensorToMechanismRatio = IntakeConstants.actuatorGearRatio;
 
     actuatorMotorConfigs.MotionMagic.MotionMagicCruiseVelocity =
-        IntakeConstants.actuatorVelocity.in(RadiansPerSecond);
+        IntakeConstants.actuatorVelocity.in(RotationsPerSecond);
     actuatorMotorConfigs.MotionMagic.MotionMagicAcceleration =
-        IntakeConstants.actuatorAcceleration.in(RadiansPerSecondPerSecond);
+        IntakeConstants.actuatorAcceleration.in(RotationsPerSecondPerSecond);
 
     CTREUtil.attempt(() -> _feedMotor.getConfigurator().apply(feedMotorConfigs), _feedMotor);
 

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

@@ -81,7 +81,7 @@ public Manipulator(Consumer<Piece> currentPieceSetter) {
     var leftMotorConfigs = new TalonFXConfiguration();
     var rightMotorConfigs = new TalonFXConfiguration();
 
-    leftMotorConfigs.Slot0.kV = ManipulatorConstants.flywheelkV.in(VoltsPerRadianPerSecond);
+    leftMotorConfigs.Slot0.kV = ManipulatorConstants.flywheelkV.in(Volts.per(RotationsPerSecond));
 
     CTREUtil.attempt(() -> _leftMotor.getConfigurator().apply(leftMotorConfigs), _leftMotor);
     CTREUtil.attempt(() -> _rightMotor.getConfigurator().apply(rightMotorConfigs), _rightMotor);

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

@@ -122,13 +122,15 @@ public Wristevator() {
     var rightMotorConfigs = new TalonFXConfiguration();
     var wristMotorConfigs = new TalonFXConfiguration();
 
-    leftMotorConfigs.Slot0.kV = WristevatorConstants.elevatorkV.in(VoltsPerRadianPerSecond);
-    leftMotorConfigs.Slot0.kA = WristevatorConstants.elevatorkA.in(VoltsPerRadianPerSecondSquared);
+    leftMotorConfigs.Slot0.kV = WristevatorConstants.elevatorkV.in(Volts.per(RotationsPerSecond));
+    leftMotorConfigs.Slot0.kA =
+        WristevatorConstants.elevatorkA.in(Volts.per(RotationsPerSecondPerSecond));
 
     leftMotorConfigs.Feedback.SensorToMechanismRatio = WristevatorConstants.elevatorGearRatio;
 
-    wristMotorConfigs.Slot0.kV = WristevatorConstants.wristkV.in(VoltsPerRadianPerSecond);
-    wristMotorConfigs.Slot0.kA = WristevatorConstants.wristkA.in(VoltsPerRadianPerSecondSquared);
+    wristMotorConfigs.Slot0.kV = WristevatorConstants.wristkV.in(Volts.per(RotationsPerSecond));
+    wristMotorConfigs.Slot0.kA =
+        WristevatorConstants.wristkA.in(Volts.per(RotationsPerSecondPerSecond));
 
     wristMotorConfigs.Feedback.SensorToMechanismRatio = WristevatorConstants.wristGearRatio;
 
@@ -295,8 +297,8 @@ private double distance(Setpoint b) {
 
   /** Whether the wristevator is near a setpoint. */
   private boolean atSetpoint(Setpoint setpoint) {
-    return MathUtil.isNear(setpoint.getAngle().in(Radians), getAngle(), 0.15)
-        && MathUtil.isNear(setpoint.getHeight().in(Radians), getHeight(), 3);
+    return MathUtil.isNear(setpoint.getAngle().in(Radians), getAngle(), 0.2)
+        && MathUtil.isNear(setpoint.getHeight().in(Radians), getHeight(), 6);
   }
 
   /** Whether to stop lower / upper motion. */
@@ -506,17 +508,17 @@ public void periodic() {
         _wristMaxProfile.calculate(Robot.kDefaultPeriod, _wristMaxState, _wristMaxGoal);
 
     DogLog.log(
-        "Wristevator/Desired Elevator Speed",
+        "Wristevator/Elevator Reference Slope",
         Units.rotationsToRadians(_leftMotor.getClosedLoopReferenceSlope().getValueAsDouble()));
     DogLog.log(
-        "Wristevator/Desired Wrist Speed",
+        "Wristevator/Wrist Reference Slope",
         Units.rotationsToRadians(_wristMotor.getClosedLoopReferenceSlope().getValueAsDouble()));
 
     DogLog.log(
-        "Wristevator/Desired Elevator Height",
+        "Wristevator/Elevator Reference",
         Units.rotationsToRadians(_leftMotor.getClosedLoopReference().getValueAsDouble()));
     DogLog.log(
-        "Wristevator/Desired Wrist Angle",
+        "Wristevator/Wrist Reference",
         Units.rotationsToRadians(_wristMotor.getClosedLoopReference().getValueAsDouble()));
 
     DogLog.log("Wristevator/Non-Adjusted Desired Elevator Speed", _elevatorMaxState.velocity);