ArmTeleop first attempt

src/main/java/org/carlmontrobotics/RobotContainer.java

@@ -8,6 +8,7 @@
 // import org.carlmontrobotics.commands.*;
 import static org.carlmontrobotics.Constants.OI;
 
+import org.carlmontrobotics.Constants.OI;
 import org.carlmontrobotics.commands.ArmTeleop;
 import org.carlmontrobotics.commands.armAmpPos;
 import org.carlmontrobotics.commands.armGroundPos;
@@ -58,6 +59,8 @@ private void setDefaultCommands() {
     //   () -> ProcessedAxisValue(driverController, Axis.kRightX)),
     //   () -> driverController.getRawButton(OI.Driver.slowDriveButton)
     // ));
+
+    arm.setDefaultCommand(new ArmTeleop(arm, () -> inputProcessing(getStickValue(manipulatorController, Axis.kLeftY))));
   }
   private void setBindingsDriver() {
     // 4 cardinal directions on arrowpad

src/main/java/org/carlmontrobotics/commands/ArmTeleop.java

@@ -18,13 +18,15 @@
 import edu.wpi.first.wpilibj.Timer;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 
 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) {
     // Use addRequirements() here to declare subsystem dependencies.
@@ -35,23 +37,23 @@ public ArmTeleop(Arm armSubsystem, DoubleSupplier joystickSupplier) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    armSubsystem.setArmGoal(armSubsystem.getArmPos(), 0);
+    goalState = new TrapezoidProfile.State(armSubsystem.getArmPos(), armSubsystem.getArmVel());
     lastTime = Timer.getFPGATimestamp();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double value = joystick.getAsDouble();
     // use trapazoid math and controllerMoveArm method from arm subsytem to apply voltage to the motor
     double speeds = getRequestedSpeeds();
     double currTime = Timer.getFPGATimestamp();
     double deltaT = currTime - lastTime;
 
-    double goalArmRad = armSubsystem.getCurrentArmGoal().position + speeds * deltaT; // fix later
-
+    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);
+    armSubsystem.COMBINE_PID_FF_TRAPEZOID(setpoint);
     lastTime = currTime;
   }
 
@@ -68,7 +70,10 @@ public double getRequestedSpeeds() {
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    armTimer.stop();
+    armTimer.reset();
+  }
 
   // Returns true when the command should end.
   @Override

src/main/java/org/carlmontrobotics/commands/armAmpPos.java

@@ -14,7 +14,7 @@ public class armAmpPos extends Command {
   private Timer armProfileTimer = new Timer();
   public armAmpPos(Arm arm) {
     // Use addRequirements() here to declare subsystem dependencies.
-    arm = this.arm;
+    this.arm = arm;
     addRequirements(arm);
   }
 

src/main/java/org/carlmontrobotics/commands/armGroundPos.java

@@ -3,7 +3,6 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package org.carlmontrobotics.commands;
-import org.carlmontrobotics.Constants;
 import org.carlmontrobotics.subsystems.Arm;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Timer;
@@ -15,7 +14,7 @@ public class armGroundPos extends Command {
   private Timer armProfileTimer = new Timer();
   public armGroundPos(Arm arm) {
     // Use addRequirements() here to declare subsystem dependencies.
-    arm = this.arm;
+    this.arm = arm;
     addRequirements(arm);
   }
 

src/main/java/org/carlmontrobotics/commands/armSpeakerPos.java

@@ -14,7 +14,7 @@ public class armSpeakerPos extends Command {
   private Timer armProfileTimer = new Timer();
   public armSpeakerPos(Arm arm) {
     // Use addRequirements() here to declare subsystem dependencies.
-    arm = this.arm;
+    this.arm = arm;
     addRequirements(arm);
   }
 

src/main/java/org/carlmontrobotics/subsystems/Arm.java

@@ -7,6 +7,8 @@
 import java.util.function.DoubleSupplier;
 
 import org.carlmontrobotics.Constants;
+import org.carlmontrobotics.commands.ArmTeleop;
+
 import static org.carlmontrobotics.Constants.Arm.*;
 import org.carlmontrobotics.lib199.MotorConfig;
 import org.carlmontrobotics.lib199.MotorControllerFactory;
@@ -87,20 +89,16 @@ public TrapezoidProfile.State calculateSetPoint(double goalSeconds, TrapezoidPro
       return profile.calculate(goalSeconds, currentPoint, goalState[goalStateIndex]);
     }
 
-    public void driveArm(double timeToTarget,double targetPosition, double targetVelocity) {
-      targetPosition = getArmClampedGoal(targetPosition);
+    public TrapezoidProfile.State calculateCustomSetPoint(double goalSeconds, TrapezoidProfile.State currentPoint, TrapezoidProfile.State goalState) {
+      return profile.calculate(goalSeconds, currentPoint, goalState);
+    }
 
-      TrapezoidProfile.State customGoalState = new TrapezoidProfile.State(targetPosition, targetVelocity);
+    // public void driveArm(double timeToTarget, TrapezoidProfile.State goalState) {
+    //   TrapezoidProfile.State setPoint = profile.calculate(timeToTarget, getCurrentArmState(), goalState);
+    //   double armFeedVolts = armFeed.calculate(goalState.velocity, 0);
+    //   armPID.setReference(setPoint.position, CANSparkMax.ControlType.kPosition, 0, armFeedVolts);
 
-      // customGoalState.calculate(timeToTarget, getCurrentArmState(), customGoalState);
-      profile.calculate(timeToTarget, getCurrentArmState(), customGoalState);
-      //create a custom trapezoidal profile using these
-    }
-    public void driveArm(double timeToTarget, TrapezoidProfile.State goalState) {
-      profile.calculate(timeToTarget, getCurrentArmState(), goalState);
-      //u can use feedforward to find how long it would take to get there
-      //I dont wanna specify it manually :((
-    }
+    // }
 
 
     public void COMBINE_PID_FF_TRAPEZOID(TrapezoidProfile.State setPoint) {
@@ -140,12 +138,28 @@ public TrapezoidProfile.State getCurrentArmState() {
       return new TrapezoidProfile.State(getArmPos(), getArmVel());
     }
 
+     public void autoCancelArmCommand() {
+        if(!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) return;
+
+        double requestedSpeeds = ((ArmTeleop) getDefaultCommand()).getRequestedSpeeds();
+
+        if(requestedSpeeds != 0) {
+            Command currentArmCommand = getCurrentCommand();
+            if(currentArmCommand != getDefaultCommand() && currentArmCommand != null) {
+                currentArmCommand.cancel();
+            }
+        }
+    }
+
 
     @Override
     public void periodic() {
-      kP = SmartDashboard.getNumber("kP", kP);
-      kD = SmartDashboard.getNumber("kD", kD);
-      kI = SmartDashboard.getNumber("kI", kI);
+
+      autoCancelArmCommand();
+
+      // kP = SmartDashboard.getNumber("kP", kP);
+      // kD = SmartDashboard.getNumber("kD", kD);
+      // kI = SmartDashboard.getNumber("kI", kI);
 
       if (armPID.getP() != kP) {
           armPID.setP(kP);