setarrmtarget in teleop

DeepBlueRobotics · Mar 3, 2024 · 999bb89 · 999bb89
1 parent baf30a2
commit 999bb89
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 6 deletions.
diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
@@ -52,8 +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(0.02, armSubsystem.getCurrentArmState(), goalState);
-    armSubsystem.setArmTarget(setpoint.position);
+    armSubsystem.setArmTarget(goalState.position);
     lastTime = currTime;
   }
 

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -144,10 +144,7 @@ public void periodic() {
         driveArm(goalState.position);
     }
 
-    public TrapezoidProfile.State calculateCustomSetPoint(double goalSeconds, TrapezoidProfile.State currentPoint, TrapezoidProfile.State goalState) {
-        return armProfile.calculate(goalSeconds, currentPoint, goalState);
-
-      }
+
 
   public void autoCancelArmCommand() {
         if(!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) return;