Fix forward kinematics and profiles

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

@@ -1,7 +1,7 @@
 package frc.robot;
 
 public final class Constants {
-  public static final Mode kCurrentMode = Mode.kReal;
+  public static final Mode kCurrentMode = Mode.kSim;
   public static final RobotType kRobot = RobotType.kPractice;
   public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim);
   public static final String kDrivetrainCanBus = "CANivore1";

src/main/java/frc/robot/arm/Arm.java

@@ -69,13 +69,13 @@ public class Arm extends SubsystemBase {
         break;
       case kSim:
         // We should not need kS in simulation
-        elbowkP.initDefault(3.0);
+        elbowkP.initDefault(5.0);
         elbowkD.initDefault(0.0);
         elbowCruiseVelocityRadPerSec.initDefault(3.0);
         elbowMaxAccelerationRadPerSecSq.initDefault(2.0);
-        wristkP.initDefault(6.0);
+        wristkP.initDefault(20.0);
         wristkD.initDefault(0.0);
-        wristCruiseVelocityRadPerSec.initDefault(3.0);
+        wristCruiseVelocityRadPerSec.initDefault(5.0);
         wristMaxAccelerationRadPerSecSq.initDefault(2.0);
         break;
       default:
@@ -147,7 +147,9 @@ private void setSetpoint(Vector<N2> setpointAnglesRad) {
     Logger.recordOutput("Arm/Goal Elbow Angle", elbowAngle);
     Logger.recordOutput("Arm/Goal Wrist Angle", wristAngle);
     m_setpointVisualizer.update(elbowAngle, wristAngle);
+    m_elbowController.reset(m_elbowPositionRad);
     m_elbowController.setGoal(elbowAngle);
+    m_wristController.reset(m_wristPositionRad);
     m_wristController.setGoal(wristAngle);
     m_profileInitialAngles = VecBuilder.fill(m_elbowPositionRad, m_wristPositionRad);
   }
@@ -239,6 +241,20 @@ class ArmKinematics {
 
     public Translation2d forward(Vector<N2> angles) {
 
+      double elbowAngle = angles.get(0, 0);
+      double wristAngle = angles.get(1, 0);
+
+      return new Translation2d(
+          ArmModel.kOrigin.getX()
+              + ArmModel.kElbowLengthMeters * Math.cos(elbowAngle)
+              + ArmModel.kWristLengthMeters * Math.cos(elbowAngle + (wristAngle - elbowAngle)),
+          ArmModel.kOrigin.getY()
+              + ArmModel.kElbowLengthMeters * Math.sin(elbowAngle)
+              + ArmModel.kWristLengthMeters * Math.sin(elbowAngle + (wristAngle - elbowAngle)));
+    }
+
+    public Translation2d forwardWristRelativeToElbow(Vector<N2> angles) {
+
       return new Translation2d(
           ArmModel.kOrigin.getX()
               + ArmModel.kElbowLengthMeters * Math.cos(angles.get(0, 0))
@@ -248,6 +264,7 @@ public Translation2d forward(Vector<N2> angles) {
               + ArmModel.kWristLengthMeters * Math.sin(angles.get(0, 0) + angles.get(1, 0)));
     }
 
+    // This is still broken
     public Optional<Vector<N2>> inverse(Translation2d position) {
       Translation2d relativePosition = position.minus(ArmModel.kOrigin);
 
@@ -276,7 +293,8 @@ public Optional<Vector<N2>> inverse(Translation2d position) {
 
       // Invert shoulder angle if invalid
       Translation2d testPosition =
-          forward(VecBuilder.fill(elbowAngle, wristAngle)).minus(ArmModel.kOrigin);
+          forwardWristRelativeToElbow(VecBuilder.fill(elbowAngle, wristAngle))
+              .minus(ArmModel.kOrigin);
       if (testPosition.getDistance(relativePosition) > 1e-3) {
         elbowAngle += Math.PI;
       }

src/main/java/frc/robot/arm/ArmIOSim.java

@@ -1,5 +1,6 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.numbers.N4;
@@ -42,10 +43,10 @@ public void updateInputs(ArmIOInputs inputs) {
   }
 
   public void setElbowVoltage(double volts) {
-    m_elbowAppliedVolts = volts;
+    m_elbowAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
   }
 
   public void setWristVoltage(double volts) {
-    m_wristAppliedVolts = volts;
+    m_wristAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
   }
 }

src/main/java/frc/robot/arm/ArmModel.java

@@ -25,8 +25,8 @@ public class ArmModel {
   private final double kElbowMassKg = Units.lbsToKilograms(7.0);
   private final double kElbowMoiKgMetersSq =
       SingleJointedArmSim.estimateMOI(kElbowLengthMeters, kElbowMassKg);
-  private final double elbowkG = 0.53;
-  private final double elbowkV = 0.27;
+  private final double elbowkG = 0.31;
+  private final double elbowkV = 0.44;
 
   public static final double kWristGearboxReduction = 15.0;
   public static final double kWristChainReduction = 36.0 / 22.0;
@@ -38,8 +38,8 @@ public class ArmModel {
   private final double kWristMassKg = Units.lbsToKilograms(11.5);
   private final double kWristMoiKgMetersSq =
       SingleJointedArmSim.estimateMOI(kWristLengthMeters, kWristMassKg);
-  private final double wristkG = 0.8;
-  private final double wristkV = 0.2;
+  private final double wristkG = 0.41;
+  private final double wristkV = 0.44;
 
   private final ArmFeedforward m_elbowFeedForward, m_wristFeedForward;
   private final SingleJointedArmSim m_elbowSim, m_wristSim;
@@ -89,7 +89,7 @@ public Vector<N2> feedforward(Vector<N2> position, Vector<N2> velocity) {
    */
   public Vector<N4> simulate(Vector<N4> state, Vector<N2> voltage, double dt) {
     m_elbowSim.setState(state.get(0, 0), state.get(2, 0));
-    m_wristSim.setState(state.get(0, 0) + state.get(1, 0), state.get(3, 0));
+    m_wristSim.setState(state.get(1, 0), state.get(3, 0));
     m_elbowSim.setInputVoltage(voltage.get(0, 0));
     m_wristSim.setInputVoltage(voltage.get(1, 0));
     m_elbowSim.update(dt);

src/main/java/frc/robot/arm/ArmSetpoints.java

@@ -5,9 +5,9 @@
 import edu.wpi.first.math.util.Units;
 
 public enum ArmSetpoints {
-  kStowed(0.005, 0.609, 0.8, 0.0),
+  kStowed(new Translation2d(0.005, 0.609), 0.0, 0.0),
   kFun(-0.125, 0.679, 0.8, 0.0),
-  kTrap(19.8, 30, 0.0, 0.6);
+  kTrap(19.8, 30, 0.0, 0.0);
 
   private final Translation2d m_setpoint;
   private final double m_elbowDelay;