Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/Robot…

…Code2024 into sofie-arm
DeepBlueRobotics · Mar 3, 2024 · 8fe76e7 · 8fe76e7
2 parents f3e0171 + 36d1890
commit 8fe76e7
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 9 deletions.
diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
@@ -25,7 +25,7 @@ public class ArmTeleop extends Command {
   private final DoubleSupplier joystick; 
   private final Arm armSubsystem;
   private double lastTime = 0;
-  private Timer armTimer = new Timer();
+
   TrapezoidProfile.State goalState;
   /** Creates a new ArmTeleop. */
   public ArmTeleop(Arm armSubsystem, DoubleSupplier joystickSupplier) {
@@ -52,7 +52,7 @@ public void execute() {
     double goalArmRad = goalState.position + speeds * deltaT; 
     goalArmRad = MathUtil.clamp(goalArmRad, LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT);
     goalState = new TrapezoidProfile.State(goalArmRad, 0);
-    TrapezoidProfile.State setpoint = armSubsystem.calculateCustomSetPoint(armTimer.get(), armSubsystem.getCurrentArmState(), goalState);
+    TrapezoidProfile.State setpoint = armSubsystem.calculateCustomSetPoint(0.02, armSubsystem.getCurrentArmState(), goalState);
     armSubsystem.driveArm(setpoint.position);
     lastTime = currTime;
   }
@@ -71,8 +71,6 @@ public double getRequestedSpeeds() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    armTimer.stop();
-    armTimer.reset();
   }
 
   // Returns true when the command should end.

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -97,11 +97,6 @@ public Arm() {
         //armProfileTimer.start(); <-- don't neeed timer anymore
 
         setArmTarget(goalState.position);
-
-        // SmartDashboard.putNumber("Arm Max Vel", MAX_FF_VEL );
-        SmartDashboard.putNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
-        SmartDashboard.putNumber("Arm Tolerance Pos", POS_TOLERANCE_RAD);
-        SmartDashboard.putNumber("Arm Tolerance Vel", VEL_TOLERANCE_RAD_P_SEC);
     }
 
     @Override