merge from test project into main project

networktables.json

@@ -96,25 +96,17 @@
     }
   },
   {
-    "name": "/Preferences/Wrist Angle Offset In",
-    "type": "double",
-    "value": 0.0,
-    "properties": {
-      "persistent": true
-    }
-  },
-  {
-    "name": "/Preferences/Wrist Angle Degs offset",
+    "name": "/Preferences/Wrist Angle",
     "type": "double",
-    "value": 1.6,
+    "value": 45.0,
     "properties": {
       "persistent": true
     }
   },
   {
-    "name": "/Preferences/Wrist Angle",
+    "name": "/Preferences/Wrist Angle Offset In",
     "type": "double",
-    "value": 45.0,
+    "value": 0.0,
     "properties": {
       "persistent": true
     }

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

@@ -26,6 +26,7 @@
 import edu.wpi.first.math.util.Units;
 import frc.robot.subsystems.arm.ArmPose;
 import frc.robot.subsystems.drive.module.ModuleConstants;
+import lib.utils.ArmTrajectory;
 
 import java.util.List;
 
@@ -44,7 +45,7 @@ private Constants() {
 
   public static final double loopPeriodSecs = Units.millisecondsToSeconds(20);
 
-  public static final Mode currentMode = Mode.REAL;
+  public static final Mode currentMode = Mode.SIM;
 
   public enum Mode {
     /** Running on a real robot. */
@@ -218,6 +219,32 @@ private ArmConstants() {}
     public static final double ARM_KV = 0.0;
     public static final double ARM_KG = 0.375;
 
+    private static final ArmTrajectory.ArmTrajectoryState start = new ArmTrajectory.ArmTrajectoryState(
+        ArmSetpoints.STOW_SETPOINT.wristAngle(), 0.0, ArmSetpoints.STOW_SETPOINT.armAngle(), 0.0
+    );
+
+    private static final ArmTrajectory.ArmTrajectoryState middle = new ArmTrajectory.ArmTrajectoryState(
+        ArmSetpoints.AMP_INTERMEDIATE.wristAngle(), 0.0, ArmSetpoints.AMP_INTERMEDIATE.armAngle(), 50.0
+    );
+
+    private static final ArmTrajectory.ArmTrajectoryState end = new ArmTrajectory.ArmTrajectoryState(
+        ArmSetpoints.AMP_SETPOINT.wristAngle(), 0.0, ArmSetpoints.AMP_SETPOINT.armAngle(), 0.0
+    );
+
+    public static final ArmTrajectory AMP_TRAJECTORY = ArmTrajectory.fromCoeffs(
+        ArmTrajectory.cubic_interpolation(
+            0.0, 2.0, start, middle
+        ),
+        0.0,
+        2.0
+    ).append(ArmTrajectory.fromCoeffs(
+        ArmTrajectory.cubic_interpolation(
+            0.0, 2.0, middle, end
+        ),
+        0.0,
+        2.0
+    ));
+
     public static final GosDoubleProperty WRIST_LOWER_LIMIT =
         new GosDoubleProperty(true, "Arm/WristLowerLimit", 0);
     public static final GosDoubleProperty WRIST_UPPER_LIMIT =

src/main/java/frc/robot/RobotContainer.java

@@ -204,12 +204,13 @@ private void configureButtonBindings() {
         m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP)
             .unless(() -> !m_useAmpLineup.get())
         .finallyDo(() -> m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP).schedule()))
-        .whileFalse(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.STOW));
+        .whileFalse(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP_REVERSE));
 
     ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.75),
         () -> m_shooter.setKickerPower(0.0),
         m_shooter)
-        .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)));
+        .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)))
+        .whileFalse(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP_REVERSE));
 
     passSpinUpTrigger.whileTrue(
         m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.PASS)

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

@@ -48,7 +48,7 @@ public void setArmVoltage(double voltage) {
   public void setArmAngle(double degrees, double velocity) {
     double setpointRots = Units.degreesToRotations(degrees);
     m_armAppliedOutput = m_armController.calculate(Units.radiansToRotations(m_armSim.getAngleRads()), setpointRots);
-    m_armAppliedOutput = MathUtil.clamp(m_armAppliedOutput, -12.0, 12.0) * velocity;
+    m_armAppliedOutput = MathUtil.clamp(m_armAppliedOutput, -12.0, 12.0);// * velocity;
     m_armSim.setInputVoltage(m_armAppliedOutput);
   }
 
@@ -63,7 +63,7 @@ public void setWristAngle(double degrees, double velocity) {
     double setpointRots = Units.degreesToRotations(degrees);
     m_wristAppliedOutput =
         m_wristController.calculate(Units.radiansToRotations(m_wristSim.getAngleRads()), setpointRots);
-    m_wristAppliedOutput = MathUtil.clamp(m_wristAppliedOutput, -12.0, 12.0) * velocity;
+    m_wristAppliedOutput = MathUtil.clamp(m_wristAppliedOutput, -12.0, 12.0);// * velocity;
     m_wristSim.setInputVoltage(m_wristAppliedOutput);
   }
 

src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java

@@ -4,12 +4,14 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ArmConstants;
 import frc.robot.Constants.ArmSetpoints;
 import lib.utils.AimbotUtils;
+import lib.utils.ArmTrajectory;
 import org.littletonrobotics.junction.Logger;
 
 import java.util.function.BooleanSupplier;
@@ -24,12 +26,13 @@ public enum ArmState {
     AUTO_AIM,
     ANTI_DEFENSE,
     AMP,
+    AMP_REVERSE,
     PREPARE_TRAP,
     SCORE_TRAP,
     PASS,
     DISABLED,
     BACKUP_SHOT,
-    MANUAL_CONTROL
+    MANUAL_CONTROL,
   }
 
   private final ArmIO m_io;
@@ -40,6 +43,10 @@ public enum ArmState {
   private double m_wristVelocityMult = 0;
   private boolean m_disabledBrakeMode = true;
 
+  private final Timer m_trajTimer;
+  private double m_reverseTimer;
+  private ArmTrajectory.ArmTrajectoryState armState;
+
   private ArmState m_desiredState = ArmState.STOW;
   private ArmState m_currentState = ArmState.DISABLED;
 
@@ -70,6 +77,9 @@ public ArmSubsystem(ArmIO io, Supplier<Pose2d> supplier, BooleanSupplier climber
     m_climberLock = climberLock;
     m_climberHeightSupplier = climberHeight;
 
+    m_trajTimer = new Timer();
+    armState = null;
+
     m_poseVisualizer = new ArmVisualizer("Current Arm Pose", Color.kFirstBlue);
     m_setpointVisualizer = new ArmVisualizer("Current Arm Setpoint", Color.kFirstRed);
   }
@@ -117,6 +127,9 @@ public void periodic() {
     Logger.recordOutput("Arm/Arm Setpoint", m_desiredArmPoseDegs);
     Logger.recordOutput("Arm/Wrist Setpoint", m_desiredWristPoseDegs);
 
+    Logger.recordOutput("Arm/At Setpoint", armAtSetpoint());
+    Logger.recordOutput("Arm/Traj Timer", m_trajTimer.get());
+
 //    Logger.recordOutput("Arm/Arm Velocity Multiplier");
 //    Logger.recordOutput("Arm/Wrist Velocity Multiplier");
 
@@ -194,17 +207,52 @@ public void handleState() {
         m_desiredWristPoseDegs = ArmSetpoints.INTAKE_SETPOINT.wristAngle();
       }
       case AMP -> {
-        if (Math.abs(m_inputs.wristPositionDegs - m_desiredWristPoseDegs) > 5.0) {
-          m_armVelocityMult = 0.5;
-        } else {
-          m_armVelocityMult = 1.0;
+        ArmTrajectory traj = ArmConstants.AMP_TRAJECTORY;
+
+        // arm state should be null by the time the trajectory ends
+        if (m_currentState != ArmState.AMP && m_currentState != ArmState.AMP_REVERSE) {
+          m_trajTimer.restart();
+          m_currentState = ArmState.AMP;
         }
-        m_wristVelocityMult = 1.0;
 
-        m_currentState = ArmState.AMP;
+        armState = traj.sample(m_trajTimer.get());
+        m_desiredArmPoseDegs = armState.armPositionDegs();
+        m_desiredWristPoseDegs = armState.wristPositionDegs();
 
-        m_desiredArmPoseDegs = ArmSetpoints.AMP_SETPOINT.armAngle();
-        m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristAngle();
+        m_armVelocityMult = armState.armVelocityDegsPerSec();
+        m_wristVelocityMult = armState.wristVelocityDegsPerSec();
+      }
+      case AMP_REVERSE -> {
+        ArmTrajectory traj = ArmConstants.AMP_TRAJECTORY;
+        // if the arm is at the setpoint, then the arm is up at amp
+        if (m_reverseTimer == 0.0) {
+          // restart the timer and find how long the last trajectory ran
+          m_trajTimer.stop();
+          m_reverseTimer = Math.min(m_trajTimer.get(), traj.getFinalTime());
+          m_trajTimer.restart();
+        } else {
+          // reverse the time by getting the difference between the trajectories length and the timer
+          double time = m_reverseTimer - m_trajTimer.get();
+
+          if (time <= 0.0) {
+            m_reverseTimer = 0.0;
+            m_trajTimer.stop();
+            m_trajTimer.reset();
+            armState = null;
+            m_desiredState = ArmState.STOW;
+            handleState();
+          }
+
+          // get the current state in the trajectory
+          armState =
+              traj.sample(time);
+
+          m_desiredArmPoseDegs = armState.armPositionDegs();
+          m_armVelocityMult = armState.armVelocityDegsPerSec();
+
+          m_desiredWristPoseDegs = armState.wristPositionDegs();
+          m_wristVelocityMult = armState.wristVelocityDegsPerSec();
+        }
       }
       case PREPARE_TRAP -> {
         m_desiredArmPoseDegs = ArmSetpoints.TRAP_PREPARE.armAngle();