Calib states

src/main/java/frc/JoysticksBindings.java

@@ -32,7 +32,7 @@ public class JoysticksBindings {
 	private static final SmartJoystick THIRD_JOYSTICK = new SmartJoystick(JoystickPorts.THIRD);
 	private static final SmartJoystick FOURTH_JOYSTICK = new SmartJoystick(JoystickPorts.FOURTH);
 //	private static final SmartJoystick FIFTH_JOYSTICK = new SmartJoystick(JoystickPorts.FIFTH);
-//	private static final SmartJoystick SIXTH_JOYSTICK = new SmartJoystick(JoystickPorts.SIXTH);
+	private static final SmartJoystick SIXTH_JOYSTICK = new SmartJoystick(JoystickPorts.SIXTH);
 
 
 	private static final ChassisPowers chassisDriverInputs = new ChassisPowers();
@@ -263,9 +263,9 @@ private static void fifthJoystickButtons(Robot robot) {
 	}
 
 	private static void sixthJoystickButtons(Robot robot) {
-//		SmartJoystick usedJoystick = SIXTH_JOYSTICK;
+		SmartJoystick usedJoystick = SIXTH_JOYSTICK;
 
-//		robot.getArm().applyCalibrationBindings(usedJoystick);
+		robot.getArm().applyCalibrationBindings(usedJoystick);
 //		robot.getEndEffector().applyCalibrationsBindings(usedJoystick);
 	}
 

src/main/java/frc/RobotManager.java

@@ -14,6 +14,7 @@
 import frc.robot.autonomous.AutonomousConstants;
 import frc.robot.led.LEDConstants;
 import frc.robot.led.LEDState;
+import frc.robot.subsystems.arm.ArmStateHandler;
 import frc.robot.subsystems.climb.lifter.LifterConstants;
 import frc.utils.DriverStationUtil;
 import frc.utils.alerts.AlertManager;

src/main/java/frc/robot/subsystems/arm/Arm.java

@@ -174,29 +174,32 @@ public boolean isBehindPosition(Rotation2d position) {
 	}
 
 	public void applyCalibrationBindings(SmartJoystick joystick) {
-		joystick.A.onTrue(new InstantCommand(() -> commandsBuilder.setIsSubsystemRunningIndependently(true)));
-		joystick.B.onTrue(new InstantCommand(() -> commandsBuilder.setIsSubsystemRunningIndependently(false)));
+		joystick.R1.onTrue(new InstantCommand(() -> commandsBuilder.setIsSubsystemRunningIndependently(true)));
+//		joystick.B.onTrue(new InstantCommand(() -> commandsBuilder.setIsSubsystemRunningIndependently(false)));
 
 		// Calibrate kG using phoenix tuner by setting the voltage
 
 		// Check limits
-		joystick.R1.whileTrue(
-			commandsBuilder.setPower(
-				() -> joystick.getAxisValue(Axis.LEFT_Y) * ArmConstants.CALIBRATION_MAX_POWER
-					+ (getKgVoltage() / BatteryUtil.getCurrentVoltage())
-			)
-		);
+//		joystick.R1.whileTrue(
+//			commandsBuilder.setPower(
+//				() -> joystick.getAxisValue(Axis.LEFT_Y) * ArmConstants.CALIBRATION_MAX_POWER
+//					+ (getKgVoltage() / BatteryUtil.getCurrentVoltage())
+//			)
+//		);
 
 		// Calibrate feed forward using sys id:
 		sysIdCalibrator.setAllButtonsForCalibration(joystick);
 
-		ArmStateHandler armStateHandler = new ArmStateHandler(this, () -> 0.0, () -> 0.0);
+//		ArmStateHandler armStateHandler = new ArmStateHandler(this, () -> 0.0, () -> 0.0);
 
 		// Calibrate PID using phoenix tuner and these bindings:
-		joystick.POV_UP.onTrue(armStateHandler.setState(ArmState.CLOSED));
-		joystick.POV_RIGHT.onTrue(armStateHandler.setState(ArmState.CLIMB));
-		joystick.POV_LEFT.onTrue(armStateHandler.setState(ArmState.NET));
-		joystick.POV_DOWN.onTrue(armStateHandler.setState(ArmState.PRE_L4));
+//		joystick.POV_UP.onTrue(armStateHandler.setState(ArmState.CLOSED));
+//		joystick.POV_RIGHT.onTrue(armStateHandler.setState(ArmState.CLIMB));
+//		joystick.POV_LEFT.onTrue(armStateHandler.setState(ArmState.NET));
+//		joystick.POV_DOWN.onTrue(armStateHandler.setState(ArmState.PRE_L4));
+//
+//		joystick.L1.onTrue(armStateHandler.setState(ArmState.CALIBRATION));
+//
 
 		// Calibrate max acceleration and cruise velocity by the equations: max acceleration = (12 + Ks)/2kA, cruise velocity =(12 + Ks)/kV
 	}

src/main/java/frc/robot/subsystems/arm/ArmConstants.java

@@ -8,18 +8,18 @@
 
 public class ArmConstants {
 
-	public static final Rotation2d POSITION_OFFSET = Rotation2d.fromDegrees(-16); // was 26
+	public static final Rotation2d POSITION_OFFSET = Rotation2d.fromDegrees(-10); // was 26
 
-	public static final Rotation2d FORWARD_SOFTWARE_LIMIT = Rotation2d.fromDegrees(231 + POSITION_OFFSET.getDegrees());
-	public static final Rotation2d MAXIMUM_POSITION = Rotation2d.fromDegrees(246 + POSITION_OFFSET.getDegrees());
-	public static final Rotation2d ELEVATOR_CLOSED_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(-8 + POSITION_OFFSET.getDegrees());
-	public static final Rotation2d ELEVATOR_OPEN_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(-24 + POSITION_OFFSET.getDegrees());
+	public static final Rotation2d FORWARD_SOFTWARE_LIMIT = Rotation2d.fromDegrees(195);
+	public static final Rotation2d MAXIMUM_POSITION = Rotation2d.fromDegrees(0);
+	public static final Rotation2d ELEVATOR_CLOSED_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(-20);
+	public static final Rotation2d ELEVATOR_OPEN_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(-20);
 	public static final double ELEVATOR_HEIGHT_METERS_TO_CHANGE_SOFT_LIMIT = 0.3;
 	public static final double LENGTH_METERS = 0.3;
 	public static final double MASS_KG = 5;
 
-	public static final Rotation2d CRUISE_VELOCITY_ANGLES_PER_SECOND = Rotation2d.fromRotations(3);
-	public static final Rotation2d ACCELERATION_ANGLES_PER_SECOND_SQUARED = Rotation2d.fromRotations(3);
+	public static final Rotation2d CRUISE_VELOCITY_ANGLES_PER_SECOND = Rotation2d.fromRotations(1);
+	public static final Rotation2d ACCELERATION_ANGLES_PER_SECOND_SQUARED = Rotation2d.fromRotations(1);
 
 	public static final double DEFAULT_ARBITRARY_FEED_FORWARD = 0;
 	public static final double CALIBRATION_MAX_POWER = 0.2;
@@ -46,15 +46,7 @@ public class ArmConstants {
 		InterpolationMap.interpolatorForRotation2d(),
 		Map.of(
 			0.0,
-			Rotation2d.fromDegrees(0),
-			0.48,
-			Rotation2d.fromDegrees(0),
-			0.59,
-			Rotation2d.fromDegrees(1.75),
-			0.65,
-			Rotation2d.fromDegrees(4.5),
-			0.70,
-			Rotation2d.fromDegrees(7)
+			Rotation2d.fromDegrees(0)
 		)
 	);
 
@@ -63,15 +55,7 @@ public class ArmConstants {
 		InterpolationMap.interpolatorForRotation2d(),
 		Map.of(
 			0.0,
-			Rotation2d.fromDegrees(0),
-			0.48,
-			Rotation2d.fromDegrees(0),
-			0.59,
-			Rotation2d.fromDegrees(3.75),
-			0.65,
-			Rotation2d.fromDegrees(6.5),
-			0.70,
-			Rotation2d.fromDegrees(9.5)
+			Rotation2d.fromDegrees(0)
 		)
 	);
 

src/main/java/frc/robot/subsystems/arm/ArmState.java

@@ -5,17 +5,18 @@
 public enum ArmState {
 
 	STAY_IN_PLACE(Rotation2d.fromDegrees(Double.NaN)),
+	CALIBRATION(Rotation2d.fromDegrees(Double.NaN)),
 	CLOSED(Rotation2d.fromDegrees(213)),
 	START_GAME(Rotation2d.fromDegrees(233)),
 	MID_WAY_CLOSE(Rotation2d.fromDegrees(90), Rotation2d.fromRotations(2), Rotation2d.fromRotations(2)),
 	INTAKE(Rotation2d.fromDegrees(213)),
 	ALGAE_OUTTAKE(Rotation2d.fromDegrees(211)),
 	PRE_L1(Rotation2d.fromDegrees(216)),
 	L1(Rotation2d.fromDegrees(216)),
-	PRE_L2(Rotation2d.fromDegrees(14)),
-	L2(Rotation2d.fromDegrees(14)),
-	PRE_L3(Rotation2d.fromDegrees(36)),
-	L3(Rotation2d.fromDegrees(36)),
+	PRE_L2(Rotation2d.fromDegrees(10)),
+	L2(Rotation2d.fromDegrees(10)),
+	PRE_L3(Rotation2d.fromDegrees(12)),
+	L3(Rotation2d.fromDegrees(12)),
 	PRE_L4(Rotation2d.fromDegrees(81), Rotation2d.fromRotations(3), Rotation2d.fromRotations(1.5)),
 	L4(Rotation2d.fromDegrees(-3), Rotation2d.fromRotations(3), Rotation2d.fromRotations(1.5)),
 	LOW_ALGAE_REMOVE(Rotation2d.fromDegrees(-4)),

src/main/java/frc/robot/subsystems/arm/ArmStateHandler.java

@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.statemachine.superstructure.TargetChecks;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 
 import java.util.function.Supplier;
 
@@ -14,6 +15,7 @@ public class ArmStateHandler {
 	private ArmState currentState;
 	private final Supplier<Double> distanceSupplier;
 	private final Supplier<Double> arbitraryFeedForwardSupplier;
+	public static LoggedNetworkNumber tunableDegrees = new LoggedNetworkNumber("/Tuning/armDeg", 0.0);
 
 	public ArmStateHandler(Arm arm, Supplier<Double> distanceSupplier, Supplier<Double> arbitraryFeedForwardSupplier) {
 		this.arm = arm;
@@ -28,6 +30,14 @@ public ArmState getCurrentState() {
 
 	public Command setState(ArmState state) {
 		return new ParallelCommandGroup(new InstantCommand(() -> currentState = state), switch (state) {
+			case CALIBRATION ->
+				arm.getCommandsBuilder()
+					.moveToPosition(
+						() -> Rotation2d.fromDegrees(tunableDegrees.get()),
+						state.getMaxVelocityRotation2dPerSecond(),
+						state.getMaxAccelerationRotation2dPerSecondSquared(),
+						0
+					);
 			case STAY_IN_PLACE -> arm.getCommandsBuilder().stayInPlace();
 			case L2, L3, L4, PRE_L3, PRE_L2 ->
 				arm.getCommandsBuilder()

src/main/java/frc/robot/subsystems/arm/calibration.md

@@ -8,45 +8,48 @@ Arm Calibrations
 - [x] encoder ID 
 - [x] encoder Buschain
 
-##### Motor config
-
-- [x] neutral mode
-- [x] is inverted
-- [x] gear ratio (use rotor to sensor and not sensor to mechanism)
-
-##### Encoder config
-
-- [x] is inverted
-
 ##### Limits 
 
 - [x] current limit
 - [x] software forward limit
 - [x] software reverse limit
 
+##### Motor config
+
+- [x] neutral mode
+- [x] is inverted
+- [x] gear ratio (use rotor to sensor and not sensor to mechanism)
+
 ##### FOC
 - [x] enable FOC
 
-##### Feed Forward 
+##### PID 
 
-- [x] kS
-- [x] kG
+- [ ] kP  
+- [ ] kI   
+- [ ] kD
 
-##### PID 
+##### Encoder config
 
-- [x] kP  
-- [x] kI   
-- [x] kD
+- [x] is inverted
 
 #### Motion Magic
 
-- [x] Cruise velocity
-- [x] Max acceleration
+- [ ] Cruise velocity
+- [ ] Max acceleration
+- [ ] States L3, L2
+
+##### Feed Forward
+
+- [x] kS
+- [x] kG
+- [ ] kV
+- [ ] kA
 
 ##### Simulation
 
 - [x] starting position
 - [x] number of motors
-- [ ] length
-- [ ] mass
+- [x] length
+- [x] mass
 

src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java

@@ -52,6 +52,7 @@ public class KrakenX60ArmBuilder {
 	private static final Rotation2d STARTING_POSITION = Rotation2d.fromDegrees(17);
 	private static final int NUMBER_OF_MOTORS = 1;
 	private static final double GEAR_RATIO = 450.0 / 7.0;
+	private static final Rotation2d CAN_CODER_MAGNET_OFFSET = Rotation2d.fromDegrees(86);
 	public static final double kG = 0.37;
 
 	protected static Arm build(String logPath) {
@@ -87,7 +88,7 @@ protected static Arm build(String logPath) {
 
 
 	public static SysIdRoutine.Config buildSysidConfig() {
-		return new SysIdRoutine.Config(Volts.of(1).per(Second), Volts.of(7), null, state -> SignalLogger.writeString("state", state.toString()));
+		return new SysIdRoutine.Config(Volts.of(0.5).per(Second), Volts.of(2), null, state -> SignalLogger.writeString("state", state.toString()));
 	}
 
 	private static TalonFXConfiguration buildTalonFXConfiguration() {
@@ -96,13 +97,13 @@ private static TalonFXConfiguration buildTalonFXConfiguration() {
 		switch (Robot.ROBOT_TYPE) {
 			case REAL -> {
 				// Motion magic
-				config.Slot0.kP = 28;
+				config.Slot0.kP = 0;
 				config.Slot0.kI = 0;
 				config.Slot0.kD = 0;
-				config.Slot0.kS = 0.065;
+				config.Slot0.kS = 0.05;
 				config.Slot0.kG = kG;
-				config.Slot0.kV = 9.0000095367432;
-				config.Slot0.kA = 0.5209;
+				config.Slot0.kV = 0;
+				config.Slot0.kA = 0;
 
 				// PID
 				config.Slot1.kP = 80;
@@ -141,7 +142,7 @@ private static TalonFXConfiguration buildTalonFXConfiguration() {
 
 		config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ArmConstants.FORWARD_SOFTWARE_LIMIT.getRotations();
 		config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
-		config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ArmConstants.ELEVATOR_OPEN_REVERSED_SOFTWARE_LIMIT.getRotations();
+		config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ArmConstants.ELEVATOR_CLOSED_REVERSED_SOFTWARE_LIMIT.getRotations();
 		config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
 
 		config.Feedback.RotorToSensorRatio = GEAR_RATIO;
@@ -194,13 +195,10 @@ private static IAngleEncoder buildRealEncoder(String logPath) {
 	}
 
 	private static CANcoderConfiguration buildEncoderConfig(CANCoderEncoder canCoderEncoder) {
-		MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs();
-		canCoderEncoder.getDevice().getConfigurator().refresh(magnetSensorConfigs);
-
 		CANcoderConfiguration configuration = new CANcoderConfiguration();
 		configuration.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
-		configuration.MagnetSensor.MagnetOffset = magnetSensorConfigs.MagnetOffset;
-		configuration.MagnetSensor.AbsoluteSensorDiscontinuityPoint = ArmConstants.MAXIMUM_POSITION.getRotations();
+		configuration.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.8;
+		configuration.MagnetSensor.MagnetOffset = CAN_CODER_MAGNET_OFFSET.getRotations();
 
 		return configuration;
 	}

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

@@ -232,7 +232,8 @@ public void applyCalibrationBindings(SmartJoystick joystick) {
 		joystick.POV_DOWN.onTrue(elevatorStateHandler.setState(ElevatorState.CLOSED));
 		joystick.POV_LEFT.onTrue(elevatorStateHandler.setState(ElevatorState.NET));
 		joystick.POV_RIGHT.onTrue(elevatorStateHandler.setState(ElevatorState.PRE_L4));
-		joystick.POV_UP.onTrue(elevatorStateHandler.setState(ElevatorState.L3));
+
+		joystick.POV_UP.onTrue(elevatorStateHandler.setState(ElevatorState.CALIBRATION));
 
 		// Calibrate max acceleration and cruse velocity by the equations: max acceleration = (12 + Ks)/2kA cruise velocity = (12 + Ks)/kV
 	}

src/main/java/frc/robot/subsystems/elevator/ElevatorCommandsBuilder.java

@@ -56,6 +56,24 @@ public Command setTargetPositionMeters(
 			);
 	}
 
+	public Command setTargetPositionMeters(
+		DoubleSupplier targetPositionMeters,
+		double maxVelocityMetersPerSecond,
+		double maxAccelerationMetersPerSecondSquared
+	) {
+		return elevator.asSubsystemCommand(
+			new RunCommand(
+				() -> elevator.setTargetPositionMeters(
+					targetPositionMeters.getAsDouble(),
+					maxVelocityMetersPerSecond,
+					maxAccelerationMetersPerSecondSquared
+				),
+				elevator
+			),
+			"Set Target Position To by supplier"
+		);
+	}
+
 	public Command stayInPlace() {
 		return elevator.asSubsystemCommand(new InitExecuteCommand(elevator::stayInPlace, () -> {}, elevator), "Stay in place");
 	}

src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java

@@ -7,16 +7,16 @@ public class ElevatorConstants {
 	public static final double DRUM_RADIUS_METERS = DRUM_DIAMETER_METERS / 2;
 	public static final double MASS_KG = 9.5154;
 	public static final double MINIMUM_HEIGHT_METERS = 0;
-	public static final double MAXIMUM_HEIGHT_METERS = 1.2;
+	public static final double MAXIMUM_HEIGHT_METERS = 0.8;
 	public static final double FIRST_STAGE_MAXIMUM_HEIGHT_METERS = 0.7;
 
-	public final static double GEAR_RATIO = (63.0 / 17.0);
+	public final static double GEAR_RATIO = (56.0 / 17.0);
 
 	public static final double REVERSE_SOFT_LIMIT_VALUE_METERS = 0.01;
 
-	public static final double FORWARD_SOFT_LIMIT_VALUE_METERS = 1.2;
+	public static final double FORWARD_SOFT_LIMIT_VALUE_METERS = 0.79;
 
-	public static final double CRUISE_VELOCITY_METERS_PER_SECOND = 2.5;
-	public static final double ACCELERATION_METERS_PER_SECOND_SQUARED = 2.5;
+	public static final double CRUISE_VELOCITY_METERS_PER_SECOND = 1;
+	public static final double ACCELERATION_METERS_PER_SECOND_SQUARED = 1;
 
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorState.java

@@ -4,16 +4,17 @@
 public enum ElevatorState {
 
 	STAY_IN_PLACE(Double.NaN),
-	CLOSED(0.07, 6, 6),
-	HOLD_ALGAE(0.12, 5, 5),
+	CALIBRATION(Double.NaN),
+	CLOSED(0.07),
+	HOLD_ALGAE(0.12),
 	INTAKE(0.07),
 	ALGAE_OUTTAKE(0.02),
 	PRE_L1(0),
 	L1(0.02),
-	PRE_L2(0.02),
-	L2(0.02),
-	PRE_L3(0.19),
-	L3(0.19),
+	PRE_L2(0.06),
+	L2(0.06),
+	PRE_L3(0.1),
+	L3(0.1),
 	WHILE_DRIVE_L4(0.4, 4, 4),
 	PRE_L4(1.18, 4, 4),
 	L4(1.18, 4, 4),