arm trajectory code actually works

TitaniumTitans · Dec 4, 2024 · 345bd94 · 345bd94
1 parent 0ecde99
commit 345bd94
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -45,7 +45,7 @@ private Constants() {
 
   public static final double loopPeriodSecs = Units.millisecondsToSeconds(20);
 
-  public static final Mode currentMode = Mode.SIM;
+  public static final Mode currentMode = Mode.REAL;
 
   public enum Mode {
     /** Running on a real robot. */
@@ -281,7 +281,7 @@ private ArmSetpoints() {
       throw new IllegalStateException("Static classes should not be constructed");
     }
 
-    public static final ArmPose AMP_INTERMEDIATE = new ArmPose("ArmPoses/Amp Intermediate", false, 60.0, 145.0);
+    public static final ArmPose AMP_INTERMEDIATE = new ArmPose("ArmPoses/Amp Intermediate", false, 60.0, 135.0);
 
     public static final ArmPose STOW_SETPOINT = new
         ArmPose("ArmPoses/Stow", true, 0.0, 35.0);

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
@@ -247,7 +247,7 @@ public void handleState() {
         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_reverseTimer = Math.min(m_trajTimer.get() + m_forwardTimer, traj.getFinalTime());
           m_trajTimer.restart();
         } else {
           // reverse the time by getting the difference between the trajectories length and the timer