Prep for 2/17 meeting. Finished removing TrashMotion.

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

@@ -9,15 +9,12 @@ import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.ManipulatorPivot;
-import frc.robot.subsystems.sysid.ElevatorSysID;
 import frc.robot.subsystems.ClimberPivot;
 import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Manipulator;
 
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
-
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
@@ -112,7 +109,7 @@ public class RobotContainer {
 
 		
 		manipulatorPivot.setDefaultCommand(
-			manipulatorPivot.setSpeed(
+			manipulatorPivot.runManualPivot(
 				() -> 0
 			)
 		);
@@ -305,7 +302,7 @@ public class RobotContainer {
 		// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
 		// then the elevator, then the arm again
 		} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { 
-			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
+			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
 				.andThen(manipulatorPivot.goToSetpoint(armPosition));
 		// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
 		} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
@@ -315,7 +312,7 @@ public class RobotContainer {
 			return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
 		// Catch all command that's safe regardless of arm and elevator positions
 		} else { 
-			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
+			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
 				.andThen(manipulatorPivot.goToSetpoint(armPosition));
 		}
 	}
@@ -357,13 +354,13 @@ public class RobotContainer {
 	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
 		if(!isL4){
 			return Commands.sequence(
-				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
+				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
 				elevator.goToSetpoint(() -> elevatorPosition),
 				manipulatorPivot.goToSetpoint(armPosition));
 			
 		}else{
 			return Commands.sequence(
-				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
+				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
 				elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
 				Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
 		}
@@ -379,7 +376,7 @@ public class RobotContainer {
 	 */
 	@SuppressWarnings("unused")
 	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
-		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
+		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
 			.andThen(manipulatorPivot.goToSetpoint(armPosition));
 	}
 

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -27,12 +27,12 @@ public class ElevatorConstants {
      
     public static final double kAllowedError = 0.75;
 
-    public static final double kFeedForwardS = 0;
+    public static final double kFeedForwardS = 0;   /* kG too high - kG too low / 2 */
-    public static final double kFeedForwardG = .7;//1; // calculated value  .6
+    public static final double kFeedForwardG = .7;  /* kG too high + kG too low / 2 */    // calculated value 0.6
-    public static final double kFeedForwardV = 0.12; // calculated value  .12 INCHES NOT METERS
+    public static final double kFeedForwardV = 0.12;   // calculated value 0.12
 
-    public static final double kMaxVelocity = 120.0; // 120 inches per second (COOKING) calculated max is 184 in/s
+    public static final double kMaxVelocity = 120.0;    // 120 inches per second (COOKING) calculated max is 184 in/s
-    public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    public static final double kMaxAcceleration = 240;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
 
     public static final double kCoralIntakePosition = 0;
     public static final double kL1Position = 0;

src/main/java/frc/robot/constants/ManipulatorPivotConstants.java

@@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.Volts;
 import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Seconds;
 
-import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
@@ -13,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ManipulatorPivotConstants {
-    public static final int kArmMotorID = 1;
+    public static final int kPivotMotorID = 1;
 
     public static final int kMotorCurrentMax = 40;
 
@@ -27,14 +26,16 @@ public class ManipulatorPivotConstants {
     public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
 
     public static final double kFeedForwardS = 0;
-    public static final double kFeedForwardG = 0.41; // calculated value
+    public static final double kFeedForwardG = 0.41; // calculated value 0.41
-    public static final double kFeedForwardV = 0.68; //calculated value
+    public static final double kFeedForwardV = 0.68; //calculated value 0.68
 
     public static final double kFFGravityOffset = Units.degreesToRadians(-135.0);
 
     public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
     public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
 
+    public static final double kEncoderOffset = 0;
+
     public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
     public static final double kL1Position = Units.degreesToRadians(0.0);
     public static final double kL2Position = Units.degreesToRadians(25.0);
@@ -43,17 +44,16 @@ public class ManipulatorPivotConstants {
     public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
     public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
     public static final double kProcesserPosition = Units.degreesToRadians(175.0);
+    public static final double kNetPosition = Units.degreesToRadians(175.0);
     /**The closest position to the elevator brace without hitting it */
-    public static final double kArmSafeStowPosition = Units.degreesToRadians(60.0);
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(60.0);
-    /**The forward rotation limit of the arm */
+    /**The forward rotation limit of the pivot */
     public static final double kRotationLimit = Units.degreesToRadians(175.0);
 
     public static final double kSysIDRampRate = 1;
     public static final double kSysIDStepVolts = 7;
     public static final double kSysIDTimeout = 10;
 
-    public static final SensorDirectionValue kSensorDirection = SensorDirectionValue.CounterClockwise_Positive;
-
     public static final IdleMode kIdleMode = IdleMode.kBrake;
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIG
@@ -67,20 +67,13 @@ public class ManipulatorPivotConstants {
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 
     static {
-
         motorConfig
             .smartCurrentLimit(kMotorCurrentMax)
             .idleMode(kIdleMode);
-        motorConfig.absoluteEncoder.positionConversionFactor(kPivotConversion);
+        motorConfig.absoluteEncoder
-        motorConfig.closedLoop
+            .positionConversionFactor(kPivotConversion)
-            .p(kPositionalP)
+            .inverted(false)
-            .i(kPositionalI)
+            .zeroOffset(kEncoderOffset);
-            .d(kPositionalD)
-            .velocityFF(0.0); // keep at zero for position pid
-        motorConfig.closedLoop.maxMotion
-            .maxAcceleration(kMaxAcceleration)
-            .maxVelocity(kMaxVelocity)
-            .allowedClosedLoopError(kPositionalTolerance);
     }
 
 

src/main/java/frc/robot/subsystems/Elevator.java

@@ -3,18 +3,13 @@ package frc.robot.subsystems;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.ClosedLoopSlot;
-import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -26,8 +21,6 @@ public class Elevator extends SubsystemBase {
     protected SparkMax elevatorMotor1;
     protected SparkMax elevatorMotor2;
 
-    //private SparkClosedLoopController elevatorClosedLoop;
-
     protected RelativeEncoder encoder;
 
     private DigitalInput bottomLimitSwitch;
@@ -127,6 +120,12 @@ public class Elevator extends SubsystemBase {
         });
     }
 
+    /**
+     * A command that will use the feed forward to hold up the elevator.
+     * Used for feed forward tuning.
+     * 
+     * @return Sets motor voltage based on feed forward calculation.
+     */
     public Command maintainPosition() {
         return run(() -> {
             elevatorMotor1.setVoltage(feedForward.calculate(0));
@@ -148,26 +147,14 @@ public class Elevator extends SubsystemBase {
         );
 
         return run(() -> {
-            /* 
+            pidController.reset(encoder.getPosition(), encoder.getVelocity());
-            if(pidController.atGoal()) {
-                elevatorMotor1.setVoltage(feedForward.calculate(0));
-            } else {
-*/
-                elevatorMotor1.setVoltage(
-                    pidController.calculate(
-                        encoder.getPosition(),
-                        clampedSetpoint
-                    ) + feedForward.calculate(pidController.getSetpoint().velocity)
-                );
-            
-            
-            /*elevatorClosedLoop.setReference(
-                clampedSetpoint,
-                ControlType.kMAXMotionPositionControl,
-                ClosedLoopSlot.kSlot0,
-                feedForward.calculate(0)
-            );*/
 
+            elevatorMotor1.setVoltage(
+                pidController.calculate(
+                    encoder.getPosition(),
+                    clampedSetpoint
+                ) + feedForward.calculate(pidController.getSetpoint().velocity)
+            );
         });
 
     }
@@ -191,10 +178,20 @@ public class Elevator extends SubsystemBase {
         return bottomLimitSwitch.get();
     }
 
+    /**
+     * Returns the motor's output current
+     * 
+     * @return Motor output current
+     */
     public double getMotor1() {
         return elevatorMotor1.getOutputCurrent();
     }
 
+    /**
+     * Returns the motor's output current
+     * 
+     * @return Motor output current
+     */
     public double getMotor2() {
         return elevatorMotor2.getOutputCurrent();
     }

src/main/java/frc/robot/subsystems/ManipulatorPivot.java

@@ -4,42 +4,47 @@ import com.revrobotics.spark.SparkMax;
 
 import java.util.function.DoubleSupplier;
 
-import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkAbsoluteEncoder;
-import com.revrobotics.spark.SparkBase;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ManipulatorPivotConstants;
 
 public class ManipulatorPivot extends SubsystemBase {
-    protected SparkMax armMotor;
+    protected SparkMax pivotMotor;
+
+    private SparkAbsoluteEncoder encoder;
 
     private ArmFeedforward feedForward;
 
-    private SparkClosedLoopController pivotClosedLoopController;
+    private ProfiledPIDController pidController;
-    private SparkAbsoluteEncoder absoluteEncoder;
 
     public ManipulatorPivot() {
-        armMotor = new SparkMax(
+        pivotMotor = new SparkMax(
-            ManipulatorPivotConstants.kArmMotorID, 
+            ManipulatorPivotConstants.kPivotMotorID, 
             MotorType.kBrushless
         );
 
-        armMotor.configure(
+        pivotMotor.configure(
             ManipulatorPivotConstants.motorConfig, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
         
-        pivotClosedLoopController = armMotor.getClosedLoopController();
+        encoder = pivotMotor.getAbsoluteEncoder();
-        absoluteEncoder = armMotor.getAbsoluteEncoder();
+
+        pidController = new ProfiledPIDController(
+            getEncoderPosition(), 
+            getEncoderVelocity(), 
+            getEncoderPosition(), 
+            null
+        );
         
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,
@@ -66,12 +71,18 @@ public class ManipulatorPivot extends SubsystemBase {
      * @return Is the motion safe
      */
     public boolean isMotionSafe(double motionTarget) {
-        return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition;
+        return motionTarget > ManipulatorPivotConstants.kPivotSafeStowPosition;
     }
 
-    public Command setSpeed(DoubleSupplier speed) {
+    /**
+     * Manual ManipulatorPivot command that sets the motor based on speed
+     * 
+     * @param speed The speed to set the motor
+     * @return A command that sets the motor speed
+     */
+    public Command runManualPivot(DoubleSupplier speed) {
         return run(() -> {
-            armMotor.set(speed.getAsDouble());
+            pivotMotor.set(speed.getAsDouble());
         });
     }
     
@@ -90,27 +101,34 @@ public class ManipulatorPivot extends SubsystemBase {
         );
 
         return run(() -> {
-            pivotClosedLoopController.setReference(clampedSetpoint,
+            pidController.reset(encoder.getPosition(), encoder.getVelocity());
-                SparkBase.ControlType.kMAXMotionPositionControl,
+
-                ClosedLoopSlot.kSlot1,
+            pivotMotor.setVoltage(
-                feedForward.calculate(absoluteEncoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, absoluteEncoder.getVelocity()));
+                pidController.calculate(
+                    encoder.getPosition(), 
+                    clampedSetpoint
+                ) + feedForward.calculate(
+                    encoder.getPosition(), 
+                    0
+                )
+            );
         });
     }
 
     /**
-     * Returns the CANCoder's position in radians
+     * Returns the encoder's position in radians
      * 
-     * @return CANCoder's position in radians
+     * @return Encoder's position in radians
      */
     public double getEncoderPosition() {
-        return absoluteEncoder.getPosition();
+        return encoder.getPosition();
     }
     /**
-     * Returns the CANCoder's velocity in radians per second
+     * Returns the encoder's velocity in radians per second
      * 
-     * @return CANCoder's velocity in radians per second
+     * @return Encoder's velocity in radians per second
      */
     public double getEncoderVelocity() {
-        return absoluteEncoder.getVelocity();
+        return encoder.getVelocity();
     }
 }

src/main/java/frc/robot/subsystems/sysid/ManipulatorPivotSysID.java

@@ -34,11 +34,11 @@ public class ManipulatorPivotSysID extends ManipulatorPivot {
         routine = new SysIdRoutine(
             ManipulatorPivotConstants.kSysIDConfig,
             new SysIdRoutine.Mechanism(
-                armMotor::setVoltage, 
+                pivotMotor::setVoltage, 
                 (log) -> {
                     log.motor("armMotor")
                         .voltage(appliedVoltage.mut_replace(
-                            armMotor.get() * RobotController.getBatteryVoltage(), Volts
+                            pivotMotor.get() * RobotController.getBatteryVoltage(), Volts
                         ))
                         .angularPosition(pivotPosition.mut_replace(
                             getEncoderPosition(), Radians