From d53b71c7665ab423b587473651614be328a991d1 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Sun, 7 Dec 2025 18:02:45 +0200 Subject: [PATCH 1/9] arm calib --- src/main/java/frc/JoysticksBindings.java | 6 +++--- src/main/java/frc/Main.java | 19 ++++++++++++++++++- src/main/java/frc/RobotManager.java | 3 +++ .../java/frc/robot/subsystems/arm/Arm.java | 3 +++ .../frc/robot/subsystems/arm/ArmState.java | 1 + .../robot/subsystems/arm/ArmStateHandler.java | 10 ++++++++++ 6 files changed, 38 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 03f8dbfd2b..eee06cfe16 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/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); } diff --git a/src/main/java/frc/Main.java b/src/main/java/frc/Main.java index 3124a5a2a1..eeac813787 100644 --- a/src/main/java/frc/Main.java +++ b/src/main/java/frc/Main.java @@ -4,6 +4,7 @@ package frc; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.RobotBase; /** @@ -18,7 +19,23 @@ public final class Main { *

If you change your main robot class, change the parameter type. */ public static void main(String... args) { - RobotBase.startRobot(RobotManager::new); + + Rotation2d rot = Rotation2d.fromDegrees(-1); + Rotation2d rot2 = Rotation2d.fromDegrees(-6); + Rotation2d rot4 = Rotation2d.fromDegrees(145); + + double sumCos = rot.getCos() + rot2.getCos() + rot4.getCos(); + double sumSin = rot.getSin() + rot2.getSin() + rot4.getSin(); + + double angleRad = Math.atan2(sumSin, sumCos); + + Rotation2d rotAvg = Rotation2d.fromRadians(angleRad); + + System.out.println(rotAvg); + + + +// RobotBase.startRobot(RobotManager::new); } } diff --git a/src/main/java/frc/RobotManager.java b/src/main/java/frc/RobotManager.java index 5f52f61e41..18d26b921c 100644 --- a/src/main/java/frc/RobotManager.java +++ b/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; @@ -122,6 +123,8 @@ public void robotPeriodic() { JoysticksBindings.updateChassisDriverInputs(); robot.periodic(); AlertManager.reportAlerts(); + + ArmStateHandler.tunableNumber.periodic(); } private void createAutoReadyForConstructionChooser() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index c7b96984e7..9769d2bcbc 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -198,6 +198,9 @@ public void applyCalibrationBindings(SmartJoystick joystick) { 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 } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmState.java b/src/main/java/frc/robot/subsystems/arm/ArmState.java index 12fea6505b..14312b51e9 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmState.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmState.java @@ -5,6 +5,7 @@ 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)), diff --git a/src/main/java/frc/robot/subsystems/arm/ArmStateHandler.java b/src/main/java/frc/robot/subsystems/arm/ArmStateHandler.java index fd70570e45..c79a165733 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmStateHandler.java +++ b/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 distanceSupplier; private final Supplier arbitraryFeedForwardSupplier; + public static LoggedNetworkNumber tunableNumber = new LoggedNetworkNumber("/Tuning/MyTunableNumber", 0.0); public ArmStateHandler(Arm arm, Supplier distanceSupplier, Supplier 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(tunableNumber.get()), + state.getMaxVelocityRotation2dPerSecond(), + state.getMaxAccelerationRotation2dPerSecondSquared(), + 0 + ); case STAY_IN_PLACE -> arm.getCommandsBuilder().stayInPlace(); case L2, L3, L4, PRE_L3, PRE_L2 -> arm.getCommandsBuilder() From bf93b201cd9478cb15c6159ac3836b5a5a66b0a1 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Sun, 7 Dec 2025 18:11:47 +0200 Subject: [PATCH 2/9] arm calib --- src/main/java/frc/Main.java | 19 +------------------ 1 file changed, 1 insertion(+), 18 deletions(-) diff --git a/src/main/java/frc/Main.java b/src/main/java/frc/Main.java index eeac813787..3124a5a2a1 100644 --- a/src/main/java/frc/Main.java +++ b/src/main/java/frc/Main.java @@ -4,7 +4,6 @@ package frc; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.RobotBase; /** @@ -19,23 +18,7 @@ public final class Main { *

If you change your main robot class, change the parameter type. */ public static void main(String... args) { - - Rotation2d rot = Rotation2d.fromDegrees(-1); - Rotation2d rot2 = Rotation2d.fromDegrees(-6); - Rotation2d rot4 = Rotation2d.fromDegrees(145); - - double sumCos = rot.getCos() + rot2.getCos() + rot4.getCos(); - double sumSin = rot.getSin() + rot2.getSin() + rot4.getSin(); - - double angleRad = Math.atan2(sumSin, sumCos); - - Rotation2d rotAvg = Rotation2d.fromRadians(angleRad); - - System.out.println(rotAvg); - - - -// RobotBase.startRobot(RobotManager::new); + RobotBase.startRobot(RobotManager::new); } } From 8be12c429bd0a88826218a883c3018a2c07edfa2 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Sun, 7 Dec 2025 18:29:52 +0200 Subject: [PATCH 3/9] elevator calib --- src/main/java/frc/RobotManager.java | 2 -- .../java/frc/robot/subsystems/arm/Arm.java | 2 +- .../robot/subsystems/arm/ArmStateHandler.java | 4 ++-- .../robot/subsystems/elevator/Elevator.java | 3 ++- .../elevator/ElevatorCommandsBuilder.java | 18 ++++++++++++++++++ .../subsystems/elevator/ElevatorState.java | 1 + .../elevator/ElevatorStateHandler.java | 12 ++++++++++++ 7 files changed, 36 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/RobotManager.java b/src/main/java/frc/RobotManager.java index 18d26b921c..60d8b5da44 100644 --- a/src/main/java/frc/RobotManager.java +++ b/src/main/java/frc/RobotManager.java @@ -123,8 +123,6 @@ public void robotPeriodic() { JoysticksBindings.updateChassisDriverInputs(); robot.periodic(); AlertManager.reportAlerts(); - - ArmStateHandler.tunableNumber.periodic(); } private void createAutoReadyForConstructionChooser() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 9769d2bcbc..674886aeaa 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -198,7 +198,7 @@ public void applyCalibrationBindings(SmartJoystick joystick) { joystick.POV_LEFT.onTrue(armStateHandler.setState(ArmState.NET)); joystick.POV_DOWN.onTrue(armStateHandler.setState(ArmState.PRE_L4)); - joystick.L1.onTrue( armStateHandler.setState(ArmState.CALIBRATION)); + 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 diff --git a/src/main/java/frc/robot/subsystems/arm/ArmStateHandler.java b/src/main/java/frc/robot/subsystems/arm/ArmStateHandler.java index c79a165733..2e7d8de6e2 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmStateHandler.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmStateHandler.java @@ -15,7 +15,7 @@ public class ArmStateHandler { private ArmState currentState; private final Supplier distanceSupplier; private final Supplier arbitraryFeedForwardSupplier; - public static LoggedNetworkNumber tunableNumber = new LoggedNetworkNumber("/Tuning/MyTunableNumber", 0.0); + public static LoggedNetworkNumber tunableDegrees = new LoggedNetworkNumber("/Tuning/armDeg", 0.0); public ArmStateHandler(Arm arm, Supplier distanceSupplier, Supplier arbitraryFeedForwardSupplier) { this.arm = arm; @@ -33,7 +33,7 @@ public Command setState(ArmState state) { case CALIBRATION -> arm.getCommandsBuilder() .moveToPosition( - () -> Rotation2d.fromDegrees(tunableNumber.get()), + () -> Rotation2d.fromDegrees(tunableDegrees.get()), state.getMaxVelocityRotation2dPerSecond(), state.getMaxAccelerationRotation2dPerSecondSquared(), 0 diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 6fc2c5086f..e0a0d9b548 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/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 } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommandsBuilder.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommandsBuilder.java index dd886107d9..b85ffad29d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommandsBuilder.java +++ b/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"); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index e78302bf4b..d1fb0a012c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -4,6 +4,7 @@ public enum ElevatorState { STAY_IN_PLACE(Double.NaN), + CALIBRATION(Double.NaN), CLOSED(0.07, 6, 6), HOLD_ALGAE(0.12, 5, 5), INTAKE(0.07), diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorStateHandler.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorStateHandler.java index 17bfa13f9b..38b0681e35 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorStateHandler.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorStateHandler.java @@ -4,11 +4,13 @@ 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; public class ElevatorStateHandler { private final Elevator elevator; private ElevatorState currentState; + public static LoggedNetworkNumber tunableHeight = new LoggedNetworkNumber("/Tuning/elevatorMeters", 0.0); public ElevatorStateHandler(Elevator elevator) { this.elevator = elevator; @@ -21,6 +23,16 @@ public ElevatorState getCurrentState() { public Command setState(ElevatorState state) { if (state == ElevatorState.STAY_IN_PLACE) { return new ParallelCommandGroup(new InstantCommand(() -> currentState = state), elevator.getCommandsBuilder().stayInPlace()); + } else if (state == ElevatorState.CALIBRATION) { + return new ParallelCommandGroup( + new InstantCommand(() -> currentState = state), + elevator.getCommandsBuilder() + .setTargetPositionMeters( + () -> tunableHeight.get(), + state.getMaxVelocityMetersPerSecond(), + state.getMaxAccelerationMetersPerSecondSquared() + ) + ); } else { return new ParallelCommandGroup( new InstantCommand(() -> currentState = state), From b1374d243f80a19aa9138709fb902ba5908181c7 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 18 Dec 2025 13:00:42 +0200 Subject: [PATCH 4/9] rm elevator constants --- src/main/java/frc/robot/IDs.java | 4 ++-- .../subsystems/elevator/ElevatorConstants.java | 10 +++++----- .../robot/subsystems/elevator/ElevatorState.java | 12 ++++++------ .../robot/subsystems/elevator/calibrations.md | 16 +++++++--------- .../factory/KrakenX60ElevatorBuilder.java | 8 ++++---- 5 files changed, 24 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/IDs.java b/src/main/java/frc/robot/IDs.java index 4cfbac5a1b..108d16e6ca 100644 --- a/src/main/java/frc/robot/IDs.java +++ b/src/main/java/frc/robot/IDs.java @@ -25,9 +25,9 @@ public static class TalonFXIDs { public static final Phoenix6DeviceID SWERVE_BACK_RIGHT_DRIVE = new Phoenix6DeviceID(7, BusChain.SWERVE_CANIVORE); - public static final Phoenix6DeviceID ELEVATOR_RIGHT = new Phoenix6DeviceID(10, BusChain.SUPERSTRUCTURE_CANIVORE); + public static final Phoenix6DeviceID ELEVATOR_RIGHT = new Phoenix6DeviceID(9, BusChain.ROBORIO); - public static final Phoenix6DeviceID ELEVATOR_LEFT = new Phoenix6DeviceID(11, BusChain.SUPERSTRUCTURE_CANIVORE); + public static final Phoenix6DeviceID ELEVATOR_LEFT = new Phoenix6DeviceID(8, BusChain.ROBORIO); public static final Phoenix6DeviceID ARM = new Phoenix6DeviceID(20, BusChain.SUPERSTRUCTURE_CANIVORE); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index dbb0e7879e..b071940120 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/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; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index d1fb0a012c..8a49b92c8e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -5,16 +5,16 @@ public enum ElevatorState { STAY_IN_PLACE(Double.NaN), CALIBRATION(Double.NaN), - CLOSED(0.07, 6, 6), - HOLD_ALGAE(0.12, 5, 5), + 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), diff --git a/src/main/java/frc/robot/subsystems/elevator/calibrations.md b/src/main/java/frc/robot/subsystems/elevator/calibrations.md index f42ba4b22a..c49452bb38 100644 --- a/src/main/java/frc/robot/subsystems/elevator/calibrations.md +++ b/src/main/java/frc/robot/subsystems/elevator/calibrations.md @@ -1,18 +1,16 @@ Calibrations ---------------------------------- - [x] Rename Motor Names By Position On Robot -- [x] Motor: Id, Type -- [x] Buschain -- [x] Inverted +- [ ] Motor: Id, Type +- [ ] Buschain +- [ ] Inverted - [x] Neutral Mode -- [x] Gear Ratio +- [ ] Gear Ratio - [x] Drum Diameter - [x] Min Position -- [x] Max Position +- [ ] Max Position - [x] Current Limit -- [ ] Limit Switch Channel - [x] Enable FOC -- [x] Control: kS, kG, PID +- [ ] Control: kS, kG, kV, kA, PID - [ ] Max acceleration, Cruise velocity -- [ ] Elevator Mass -- [ ] States heights \ No newline at end of file +- [ ] States heights - L3, L2 \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/factory/KrakenX60ElevatorBuilder.java b/src/main/java/frc/robot/subsystems/elevator/factory/KrakenX60ElevatorBuilder.java index eb843c0def..e42a23d8d8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/factory/KrakenX60ElevatorBuilder.java +++ b/src/main/java/frc/robot/subsystems/elevator/factory/KrakenX60ElevatorBuilder.java @@ -89,13 +89,13 @@ private static TalonFXConfiguration mainConfiguration() { TalonFXConfiguration configuration = limitsConfiguration(); if (Robot.ROBOT_TYPE.isReal()) { // Motion Magic - configuration.Slot0.kP = 15; + configuration.Slot0.kP = 0; configuration.Slot0.kI = 0; configuration.Slot0.kD = 0; configuration.Slot0.kG = kG; configuration.Slot0.kS = 0; - configuration.Slot0.kV = 0.49307; - configuration.Slot0.kA = 0.032026; + configuration.Slot0.kV = 0; + configuration.Slot0.kA = 0; // PID configuration.Slot1.kP = 10; @@ -116,7 +116,7 @@ private static TalonFXConfiguration mainConfiguration() { configuration.Slot1.GravityType = GravityTypeValue.Elevator_Static; configuration.Slot2.GravityType = GravityTypeValue.Elevator_Static; - configuration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + configuration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; configuration.MotionMagic.MotionMagicCruiseVelocity = Elevator .convertMetersToRotations(ElevatorConstants.CRUISE_VELOCITY_METERS_PER_SECOND) From 09e8b1f0dd441eb972c2ba93008da3a149481089 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 18 Dec 2025 13:01:17 +0200 Subject: [PATCH 5/9] rm elevator constants --- src/main/java/frc/robot/IDs.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/IDs.java b/src/main/java/frc/robot/IDs.java index 108d16e6ca..5234a2bee2 100644 --- a/src/main/java/frc/robot/IDs.java +++ b/src/main/java/frc/robot/IDs.java @@ -25,9 +25,9 @@ public static class TalonFXIDs { public static final Phoenix6DeviceID SWERVE_BACK_RIGHT_DRIVE = new Phoenix6DeviceID(7, BusChain.SWERVE_CANIVORE); - public static final Phoenix6DeviceID ELEVATOR_RIGHT = new Phoenix6DeviceID(9, BusChain.ROBORIO); + public static final Phoenix6DeviceID ELEVATOR_RIGHT = new Phoenix6DeviceID(9, BusChain.SUPERSTRUCTURE_CANIVORE); - public static final Phoenix6DeviceID ELEVATOR_LEFT = new Phoenix6DeviceID(8, BusChain.ROBORIO); + public static final Phoenix6DeviceID ELEVATOR_LEFT = new Phoenix6DeviceID(8, BusChain.SUPERSTRUCTURE_CANIVORE); public static final Phoenix6DeviceID ARM = new Phoenix6DeviceID(20, BusChain.SUPERSTRUCTURE_CANIVORE); From 477dc8e450be3d93f5345771992f7178d3a1ffb6 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 18 Dec 2025 13:07:44 +0200 Subject: [PATCH 6/9] rm arm constants --- src/main/java/frc/robot/IDs.java | 2 +- .../robot/subsystems/arm/ArmConstants.java | 30 ++++------------ .../frc/robot/subsystems/arm/ArmState.java | 8 ++--- .../frc/robot/subsystems/arm/calibration.md | 35 ++++++++++--------- .../arm/factory/KrakenX60ArmBuilder.java | 12 +++---- 5 files changed, 36 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/IDs.java b/src/main/java/frc/robot/IDs.java index 5234a2bee2..2ca4a8eabd 100644 --- a/src/main/java/frc/robot/IDs.java +++ b/src/main/java/frc/robot/IDs.java @@ -49,7 +49,7 @@ public static class CANCoderIDs { public static final Phoenix6DeviceID SWERVE_BACK_RIGHT = new Phoenix6DeviceID(3, BusChain.SWERVE_CANIVORE); - public static final Phoenix6DeviceID ARM = new Phoenix6DeviceID(20, BusChain.SUPERSTRUCTURE_CANIVORE); + public static final Phoenix6DeviceID ARM = new Phoenix6DeviceID(6, BusChain.SUPERSTRUCTURE_CANIVORE); public static final Phoenix6DeviceID PIVOT = new Phoenix6DeviceID(13, BusChain.ROBORIO); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index bc2d1b64f9..05618084cc 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -10,16 +10,16 @@ public class ArmConstants { public static final Rotation2d POSITION_OFFSET = Rotation2d.fromDegrees(-16); // 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 FORWARD_SOFTWARE_LIMIT = Rotation2d.fromDegrees(0); + public static final Rotation2d MAXIMUM_POSITION = Rotation2d.fromDegrees(0); + public static final Rotation2d ELEVATOR_CLOSED_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(0); public static final Rotation2d ELEVATOR_OPEN_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(-24 + POSITION_OFFSET.getDegrees()); 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) ) ); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmState.java b/src/main/java/frc/robot/subsystems/arm/ArmState.java index 14312b51e9..bbac5d0985 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmState.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmState.java @@ -13,10 +13,10 @@ public enum ArmState { 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)), diff --git a/src/main/java/frc/robot/subsystems/arm/calibration.md b/src/main/java/frc/robot/subsystems/arm/calibration.md index e5b5a59c19..d9b721fd1c 100644 --- a/src/main/java/frc/robot/subsystems/arm/calibration.md +++ b/src/main/java/frc/robot/subsystems/arm/calibration.md @@ -3,50 +3,51 @@ Arm Calibrations ##### IDs -- [x] motor ID -- [x] motor Buschain -- [x] encoder ID -- [x] encoder Buschain +- [ ] motor ID +- [ ] motor Buschain +- [ ] encoder ID +- [ ] encoder Buschain ##### Motor config - [x] neutral mode -- [x] is inverted +- [ ] is inverted - [x] gear ratio (use rotor to sensor and not sensor to mechanism) ##### Encoder config -- [x] is inverted +- [ ] is inverted ##### Limits - [x] current limit -- [x] software forward limit -- [x] software reverse limit +- [ ] software forward limit +- [ ] software reverse limit ##### FOC - [x] enable FOC ##### Feed Forward -- [x] kS -- [x] kG +- [ ] kS +- [ ] kG ##### PID -- [x] kP -- [x] kI -- [x] kD +- [ ] kP +- [ ] kI +- [ ] kD #### Motion Magic -- [x] Cruise velocity -- [x] Max acceleration +- [ ] Cruise velocity +- [ ] Max acceleration +- [ ] States L3, L2 ##### Simulation - [x] starting position - [x] number of motors -- [ ] length -- [ ] mass +- [x] length +- [x] mass diff --git a/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java b/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java index 87e0e998c8..4789658cd9 100644 --- a/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java +++ b/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java @@ -48,11 +48,11 @@ public class KrakenX60ArmBuilder { private static final int APPLY_CONFIG_RETRIES = 5; private static final boolean ENABLE_FOC = true; - private static final boolean IS_INVERTED = true; + private static final boolean IS_INVERTED = false; 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; - public static final double kG = 0.37; + public static final double kG = 0; protected static Arm build(String logPath) { Phoenix6DynamicMotionMagicRequest positionRequest = Robot.ROBOT_TYPE.isReal() @@ -96,13 +96,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; 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; From 678dc8dd08b990351bcff66ba8c87ab0ca0c6b68 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 18 Dec 2025 13:14:19 +0200 Subject: [PATCH 7/9] reorder calib files --- .../frc/robot/subsystems/arm/calibration.md | 32 ++++++++++--------- .../robot/subsystems/elevator/calibrations.md | 7 ++-- 2 files changed, 21 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/calibration.md b/src/main/java/frc/robot/subsystems/arm/calibration.md index d9b721fd1c..330d9abc7d 100644 --- a/src/main/java/frc/robot/subsystems/arm/calibration.md +++ b/src/main/java/frc/robot/subsystems/arm/calibration.md @@ -8,29 +8,20 @@ Arm Calibrations - [ ] encoder ID - [ ] encoder Buschain -##### Motor config - -- [x] neutral mode -- [ ] is inverted -- [x] gear ratio (use rotor to sensor and not sensor to mechanism) - -##### Encoder config - -- [ ] is inverted - ##### Limits - [x] current limit - [ ] software forward limit - [ ] software reverse limit -##### FOC -- [x] enable FOC +##### Motor config -##### Feed Forward +- [x] neutral mode +- [ ] is inverted +- [x] gear ratio (use rotor to sensor and not sensor to mechanism) -- [ ] kS -- [ ] kG +##### FOC +- [x] enable FOC ##### PID @@ -38,12 +29,23 @@ Arm Calibrations - [ ] kI - [ ] kD +##### Encoder config + +- [ ] is inverted + #### Motion Magic - [ ] Cruise velocity - [ ] Max acceleration - [ ] States L3, L2 +##### Feed Forward + +- [ ] kS +- [ ] kG +- [ ] kV +- [ ] kA + ##### Simulation - [x] starting position diff --git a/src/main/java/frc/robot/subsystems/elevator/calibrations.md b/src/main/java/frc/robot/subsystems/elevator/calibrations.md index c49452bb38..58b7775d91 100644 --- a/src/main/java/frc/robot/subsystems/elevator/calibrations.md +++ b/src/main/java/frc/robot/subsystems/elevator/calibrations.md @@ -2,15 +2,16 @@ Calibrations ---------------------------------- - [x] Rename Motor Names By Position On Robot - [ ] Motor: Id, Type -- [ ] Buschain +- [ ] Control: kS, kG, kV, kA, PID - [ ] Inverted +- [ ] Max Position +- [ ] Follower Inverted - [x] Neutral Mode - [ ] Gear Ratio - [x] Drum Diameter - [x] Min Position -- [ ] Max Position - [x] Current Limit - [x] Enable FOC -- [ ] Control: kS, kG, kV, kA, PID - [ ] Max acceleration, Cruise velocity +- [ ] Buschain - [ ] States heights - L3, L2 \ No newline at end of file From 7965c09fed97129c228655d86a240f7fb1e1e0cf Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 18 Dec 2025 21:23:33 +0200 Subject: [PATCH 8/9] commit & push --- src/main/java/frc/robot/IDs.java | 2 +- .../frc/robot/subsystems/arm/ArmConstants.java | 6 +++--- .../frc/robot/subsystems/arm/calibration.md | 18 +++++++++--------- .../arm/factory/KrakenX60ArmBuilder.java | 16 +++++++--------- 4 files changed, 20 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/IDs.java b/src/main/java/frc/robot/IDs.java index 2ca4a8eabd..5234a2bee2 100644 --- a/src/main/java/frc/robot/IDs.java +++ b/src/main/java/frc/robot/IDs.java @@ -49,7 +49,7 @@ public static class CANCoderIDs { public static final Phoenix6DeviceID SWERVE_BACK_RIGHT = new Phoenix6DeviceID(3, BusChain.SWERVE_CANIVORE); - public static final Phoenix6DeviceID ARM = new Phoenix6DeviceID(6, BusChain.SUPERSTRUCTURE_CANIVORE); + public static final Phoenix6DeviceID ARM = new Phoenix6DeviceID(20, BusChain.SUPERSTRUCTURE_CANIVORE); public static final Phoenix6DeviceID PIVOT = new Phoenix6DeviceID(13, BusChain.ROBORIO); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 05618084cc..1eded80b98 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -10,10 +10,10 @@ public class ArmConstants { public static final Rotation2d POSITION_OFFSET = Rotation2d.fromDegrees(-16); // was 26 - public static final Rotation2d FORWARD_SOFTWARE_LIMIT = Rotation2d.fromDegrees(0); + public static final Rotation2d FORWARD_SOFTWARE_LIMIT = Rotation2d.fromDegrees(208); public static final Rotation2d MAXIMUM_POSITION = Rotation2d.fromDegrees(0); - public static final Rotation2d ELEVATOR_CLOSED_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(0); - public static final Rotation2d ELEVATOR_OPEN_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(-24 + POSITION_OFFSET.getDegrees()); + public static final Rotation2d ELEVATOR_CLOSED_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(-15); + public static final Rotation2d ELEVATOR_OPEN_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(-15); 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; diff --git a/src/main/java/frc/robot/subsystems/arm/calibration.md b/src/main/java/frc/robot/subsystems/arm/calibration.md index 330d9abc7d..3c73f77695 100644 --- a/src/main/java/frc/robot/subsystems/arm/calibration.md +++ b/src/main/java/frc/robot/subsystems/arm/calibration.md @@ -3,21 +3,21 @@ Arm Calibrations ##### IDs -- [ ] motor ID -- [ ] motor Buschain -- [ ] encoder ID -- [ ] encoder Buschain +- [x] motor ID +- [x] motor Buschain +- [x] encoder ID +- [x] encoder Buschain ##### Limits - [x] current limit -- [ ] software forward limit -- [ ] software reverse limit +- [x] software forward limit +- [x] software reverse limit ##### Motor config - [x] neutral mode -- [ ] is inverted +- [x] is inverted - [x] gear ratio (use rotor to sensor and not sensor to mechanism) ##### FOC @@ -31,7 +31,7 @@ Arm Calibrations ##### Encoder config -- [ ] is inverted +- [x] is inverted #### Motion Magic @@ -42,7 +42,7 @@ Arm Calibrations ##### Feed Forward - [ ] kS -- [ ] kG +- [x] kG - [ ] kV - [ ] kA diff --git a/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java b/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java index 4789658cd9..d438dd824b 100644 --- a/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java +++ b/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java @@ -48,11 +48,12 @@ public class KrakenX60ArmBuilder { private static final int APPLY_CONFIG_RETRIES = 5; private static final boolean ENABLE_FOC = true; - private static final boolean IS_INVERTED = false; + private static final boolean IS_INVERTED = true; 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; - public static final double kG = 0; + private static final Rotation2d CAN_CODER_MAGNET_OFFSET = Rotation2d.fromDegrees(98); + public static final double kG = 0.367; protected static Arm build(String logPath) { Phoenix6DynamicMotionMagicRequest positionRequest = Robot.ROBOT_TYPE.isReal() @@ -99,7 +100,7 @@ private static TalonFXConfiguration buildTalonFXConfiguration() { config.Slot0.kP = 0; config.Slot0.kI = 0; config.Slot0.kD = 0; - config.Slot0.kS = 0; + config.Slot0.kS = 0.055; config.Slot0.kG = kG; config.Slot0.kV = 0; config.Slot0.kA = 0; @@ -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; } From 681bbe53d3494cef05d33d46bb7dc4006c0eefc7 Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Thu, 18 Dec 2025 22:30:09 +0200 Subject: [PATCH 9/9] code --- src/main/java/frc/robot/IDs.java | 4 +-- .../java/frc/robot/subsystems/arm/Arm.java | 32 +++++++++---------- .../robot/subsystems/arm/ArmConstants.java | 8 ++--- .../frc/robot/subsystems/arm/calibration.md | 2 +- .../arm/factory/KrakenX60ArmBuilder.java | 8 ++--- .../calibration/sysid/SysIdCalibrator.java | 2 ++ 6 files changed, 29 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/IDs.java b/src/main/java/frc/robot/IDs.java index 5234a2bee2..4cfbac5a1b 100644 --- a/src/main/java/frc/robot/IDs.java +++ b/src/main/java/frc/robot/IDs.java @@ -25,9 +25,9 @@ public static class TalonFXIDs { public static final Phoenix6DeviceID SWERVE_BACK_RIGHT_DRIVE = new Phoenix6DeviceID(7, BusChain.SWERVE_CANIVORE); - public static final Phoenix6DeviceID ELEVATOR_RIGHT = new Phoenix6DeviceID(9, BusChain.SUPERSTRUCTURE_CANIVORE); + public static final Phoenix6DeviceID ELEVATOR_RIGHT = new Phoenix6DeviceID(10, BusChain.SUPERSTRUCTURE_CANIVORE); - public static final Phoenix6DeviceID ELEVATOR_LEFT = new Phoenix6DeviceID(8, BusChain.SUPERSTRUCTURE_CANIVORE); + public static final Phoenix6DeviceID ELEVATOR_LEFT = new Phoenix6DeviceID(11, BusChain.SUPERSTRUCTURE_CANIVORE); public static final Phoenix6DeviceID ARM = new Phoenix6DeviceID(20, BusChain.SUPERSTRUCTURE_CANIVORE); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 674886aeaa..0ac7b889cd 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -174,32 +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.L1.onTrue(armStateHandler.setState(ArmState.CALIBRATION)); - +// 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 } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 1eded80b98..4357090735 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -8,12 +8,12 @@ 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(208); + 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(-15); - public static final Rotation2d ELEVATOR_OPEN_REVERSED_SOFTWARE_LIMIT = Rotation2d.fromDegrees(-15); + 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; diff --git a/src/main/java/frc/robot/subsystems/arm/calibration.md b/src/main/java/frc/robot/subsystems/arm/calibration.md index 3c73f77695..64db735ad9 100644 --- a/src/main/java/frc/robot/subsystems/arm/calibration.md +++ b/src/main/java/frc/robot/subsystems/arm/calibration.md @@ -41,7 +41,7 @@ Arm Calibrations ##### Feed Forward -- [ ] kS +- [x] kS - [x] kG - [ ] kV - [ ] kA diff --git a/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java b/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java index d438dd824b..d7841c6654 100644 --- a/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java +++ b/src/main/java/frc/robot/subsystems/arm/factory/KrakenX60ArmBuilder.java @@ -52,8 +52,8 @@ 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(98); - public static final double kG = 0.367; + private static final Rotation2d CAN_CODER_MAGNET_OFFSET = Rotation2d.fromDegrees(86); + public static final double kG = 0.37; protected static Arm build(String logPath) { Phoenix6DynamicMotionMagicRequest positionRequest = Robot.ROBOT_TYPE.isReal() @@ -88,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() { @@ -100,7 +100,7 @@ private static TalonFXConfiguration buildTalonFXConfiguration() { config.Slot0.kP = 0; config.Slot0.kI = 0; config.Slot0.kD = 0; - config.Slot0.kS = 0.055; + config.Slot0.kS = 0.05; config.Slot0.kG = kG; config.Slot0.kV = 0; config.Slot0.kA = 0; diff --git a/src/main/java/frc/utils/calibration/sysid/SysIdCalibrator.java b/src/main/java/frc/utils/calibration/sysid/SysIdCalibrator.java index 381912e8d3..50cd9c173f 100644 --- a/src/main/java/frc/utils/calibration/sysid/SysIdCalibrator.java +++ b/src/main/java/frc/utils/calibration/sysid/SysIdCalibrator.java @@ -47,6 +47,8 @@ public SysIdCalibrator(SysIdConfigInfo sysIdConfigInfo, GBSubsystem subsystem, C * @param smartJoystick - the joystick to apply the buttons on */ public void setAllButtonsForCalibration(SmartJoystick smartJoystick) { + smartJoystick.BACK.onTrue(new InstantCommand(SignalLogger::start)); + smartJoystick.A.whileTrue(getSysIdCommand(true, SysIdRoutine.Direction.kForward)); smartJoystick.B.whileTrue(getSysIdCommand(true, SysIdRoutine.Direction.kReverse)); smartJoystick.X.whileTrue(getSysIdCommand(false, SysIdRoutine.Direction.kForward));