Feature/arm sim fixes

src/main/java/competition/simulation/arm/ArmSimConstants.java

@@ -13,8 +13,10 @@ public class ArmSimConstants {
     public static final double armReduction = 200;
     public static final Mass armMass = Kilogram.of(8.0); // Kilograms
     public static final Distance armLength = Meters.of(0.5);
-    public static final Angle minAngleRads = Degrees.of(-45);
-    public static final Angle maxAngleRads = Degrees.of(125 - 45);
-    public static final Angle startingAngle = minAngleRads;
+    // the frame of reference for these angles is 0' right, 90' up, 180' left, 270' down
+    // so we have 225 as the starting angle which is 0' in arm relative terms
+    public static final Angle minAngleRads = Degrees.of(225 - 125);
+    public static final Angle maxAngleRads = Degrees.of(225);
+    public static final Angle startingAngle = maxAngleRads;
     public static final Angle armEncoderAnglePerRotation = Degrees.of(0.1);
 }

src/main/java/competition/simulation/arm/ArmSimulator.java

@@ -11,6 +11,7 @@
 import static edu.wpi.first.units.Units.Kilograms;
 import static edu.wpi.first.units.Units.Rotations;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import xbot.common.controls.actuators.mock_adapters.MockCANMotorController;
@@ -39,21 +40,31 @@ public ArmSimulator(ElevatorMechanism elevatorMechanism, ArmPivotSubsystem armPi
                 ArmSimConstants.minAngleRads.in(Radians),
                 ArmSimConstants.maxAngleRads.in(Radians),
                 true,
-                0
+                ArmSimConstants.startingAngle.in(Radians)
         );
     }
 
     public void update() {
-        armSim.setInput(this.armMotor.getPower() * RobotController.getBatteryVoltage());
+        // invert power because the simulated arm is going "backwards"
+        armSim.setInput(this.armMotor.getPower() * RobotController.getBatteryVoltage() * -1.0);
         armSim.update(SimulationConstants.loopPeriodSec); // 20ms
 
         // Read out the new arm position for rendering
-        // TODO: the frame of reference of the simulated arm is wrong, gravity isn't being applied
-        // in the same way we think
-        var armMotorRotations = armSim.getAngleRads() / ArmSimConstants.armEncoderAnglePerRotation.in(Radians);
+        var armRelativeAngle = getArmAngle();
 
+
+        var armMotorRotations = armRelativeAngle.in(Radians) / ArmSimConstants.armEncoderAnglePerRotation.in(Radians);
         armMotor.setPosition(Rotations.of(armMotorRotations));
+
+
         // correct for frame of reference for the arm pivot in the mechanism vs sim model
-        elevatorMechanism.armAngle = Radians.of(armSim.getAngleRads()).plus(ArmSimConstants.minAngleRads.times(-1)).times(-1);
+        elevatorMechanism.armAngle = armRelativeAngle;
+    }
+
+    public Angle getArmAngle() {
+        // convert from the armSim frame of reference to our actual arm frame of reference where the bottom is 0' and the top is 125'
+        var armSimAngle = Radians.of(armSim.getAngleRads());
+
+        return armSimAngle.minus(ArmSimConstants.maxAngleRads).times(-1);
     }
 }

src/main/java/competition/subsystems/arm_pivot/ArmPivotSubsystem.java

@@ -17,7 +17,7 @@
 @Singleton
 public class ArmPivotSubsystem extends BaseSetpointSubsystem<Angle> {
     public final XCANMotorController armMotor;
-    Angle targetAngle;
+    Angle targetAngle = Degrees.of(0);
     ElectricalContract electricalContract;
     DoubleProperty degreesPerRotations;
     double rotationsAtZero;

src/main/java/competition/subsystems/arm_pivot/commands/ArmPivotMaintainerCommand.java

@@ -69,7 +69,7 @@ protected double getErrorMagnitude() { //distance from goal
     protected double getHumanInput() { //gamepad controls: Left joy stick up/down & Left bumper to switch between elevator/arm
        return MathUtils.constrainDouble(
                MathUtils.deadband(
-                       oi.programmerGamepad.getLeftStickY(),
+                       oi.programmerGamepad.getRightStickY(),
                        oi.getOperatorGamepadTypicalDeadband(),
                        (a) -> (a)),
                humanMinPower.get(), humanMaxPower.get());

src/main/java/competition/subsystems/elevator/ElevatorMechanism.java

@@ -62,7 +62,7 @@ public ElevatorMechanism(/* TODO: Inject references to the Elevator + Arm + Scor
     public void periodic() {
         // update mechanism based on current elevatorHeight and armAngle
         elevatorLigament.setLength(elevatorLigamentBaseLengthMeters + elevatorHeight.in(Units.Meters));
-        armLigament.setAngle(armLigamentBaseAngleDegrees + armAngle.in(Degrees));
+        armLigament.setAngle(armLigamentBaseAngleDegrees - armAngle.in(Degrees));
         coralLigament.setLength(this.coralInScorer ? coralLengthMeters : 0.0);
         aKitLog.record("Mech2d", mech2d);