From f7823f437caf333410a0e6c12c9f754774c33065 Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Wed, 5 Feb 2025 21:04:20 -0500 Subject: [PATCH 01/10] works like trash --- src/main/java/frc/robot/Constants.java | 4 +- .../frc/robot/subsystems/Wristevator.java | 137 ++++++++++-------- 2 files changed, 75 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8d55099..76a8c41 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -166,8 +166,8 @@ public Angle getHeight() { /** Wristevator intermediate setpoints. */ public static enum Intermediate implements Setpoint { INFINITY(Radians.of(Integer.MAX_VALUE), Radians.of(Integer.MAX_VALUE)), - I1(Radians.of(0), Radians.of(50)), - I2(Radians.of(-1), Radians.of(30)), + I1(Radians.of(-2), Radians.of(100)), + I2(Radians.of(-2), Radians.of(100)), I3(Radians.of(1), Radians.of(50)); private final Angle _angle; diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index ac1ce03..57665e1 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -6,8 +6,8 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import dev.doglog.DogLog; @@ -68,8 +68,8 @@ public class Wristevator extends AdvancedSubsystem { private final StatusSignal _heightGetter = _leftMotor.getPosition(); private final StatusSignal _angleGetter = _wristMotor.getPosition(); - private final DynamicMotionMagicVoltage _heightSetter = new DynamicMotionMagicVoltage(0, 0, 0, 0); - private final DynamicMotionMagicVoltage _angleSetter = new DynamicMotionMagicVoltage(0, 0, 0, 0); + private final PositionVoltage _heightSetter = new PositionVoltage(0); + private final PositionVoltage _angleSetter = new PositionVoltage(0); private final StatusSignal _elevatorVelocityGetter = _leftMotor.getVelocity(); private final StatusSignal _wristVelocityGetter = _wristMotor.getVelocity(); @@ -77,28 +77,27 @@ public class Wristevator extends AdvancedSubsystem { private final VelocityVoltage _elevatorVelocitySetter = new VelocityVoltage(0); private final VelocityVoltage _wristVelocitySetter = new VelocityVoltage(0); + // elevator profiling private final Constraints _elevatorMaxConstraints = new Constraints( - WristevatorConstants.maxElevatorSpeed.in(RadiansPerSecond), - WristevatorConstants.maxElevatorAcceleration.in(RadiansPerSecondPerSecond)); + WristevatorConstants.maxElevatorSpeed.in(RotationsPerSecond), + WristevatorConstants.maxElevatorAcceleration.in(RotationsPerSecondPerSecond)); - private State _elevatorMaxState = new State(0, 0); + private TrapezoidProfile _elevatorProfile = new TrapezoidProfile(_elevatorMaxConstraints); - private State _wristMaxState = new State(0, 0); - - private final State _elevatorMaxGoal = new State(0, 0); - - private final State _wristMaxGoal = new State(0, 0); + private State _elevatorSetpoint = new State(); + private final State _elevatorGoal = new State(); + // wrist profiling private final Constraints _wristMaxConstraints = new Constraints( - WristevatorConstants.maxWristSpeed.in(RadiansPerSecond), - WristevatorConstants.maxWristAcceleration.in(RadiansPerSecondPerSecond)); + WristevatorConstants.maxWristSpeed.in(RotationsPerSecond), + WristevatorConstants.maxWristAcceleration.in(RotationsPerSecondPerSecond)); - private final TrapezoidProfile _elevatorMaxProfile = - new TrapezoidProfile(_elevatorMaxConstraints); + private TrapezoidProfile _wristProfile = new TrapezoidProfile(_wristMaxConstraints); - private final TrapezoidProfile _wristMaxProfile = new TrapezoidProfile(_wristMaxConstraints); + private State _wristSetpoint = new State(); + private final State _wristGoal = new State(); private final DigitalInput _homeSwitch = new DigitalInput(WristevatorConstants.homeSwitch); @@ -133,6 +132,8 @@ public Wristevator() { wristMotorConfigs.Feedback.SensorToMechanismRatio = WristevatorConstants.wristGearRatio; + findProfileConstraints(HOME); + CTREUtil.attempt(() -> _leftMotor.getConfigurator().apply(leftMotorConfigs), _leftMotor); CTREUtil.attempt(() -> _rightMotor.getConfigurator().apply(rightMotorConfigs), _rightMotor); CTREUtil.attempt(() -> _wristMotor.getConfigurator().apply(wristMotorConfigs), _wristMotor); @@ -296,47 +297,54 @@ private double distance(Setpoint b) { return Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2)); } - /** Whether the wristevator is near a setpoint. */ - private boolean atSetpoint(Setpoint setpoint) { - return MathUtil.isNear(setpoint.getAngle().in(Radians), getAngle(), 0.0001) - && MathUtil.isNear(setpoint.getHeight().in(Radians), getHeight(), 0.0001); + /** Whether the wristevator finished its most recent profiles. */ + private boolean finishedProfiles(Setpoint setpoint) { + System.out.println(_elevatorProfile.timeLeftUntil(setpoint.getHeight().in(Rotations))); + + return _elevatorProfile.timeLeftUntil(setpoint.getHeight().in(Rotations)) == 0.001 + && _wristProfile.timeLeftUntil(setpoint.getAngle().in(Rotations)) == 0.001; } /** Find new constraints for the motion magic control requests. */ private void findProfileConstraints(Setpoint setpoint) { - _elevatorMaxState.position = getHeight(); - _elevatorMaxState.velocity = 0; + _elevatorSetpoint.position = Units.radiansToRotations(getHeight()); + _elevatorSetpoint.velocity = 0; + + _wristSetpoint.position = Units.radiansToRotations(getAngle()); + _wristSetpoint.velocity = 0; + + _elevatorGoal.position = setpoint.getHeight().in(Rotations); + _elevatorGoal.velocity = 0; - _wristMaxState.position = getAngle(); - _wristMaxState.velocity = 0; + _wristGoal.position = setpoint.getAngle().in(Rotations); + _wristGoal.velocity = 0; - _elevatorMaxGoal.position = setpoint.getHeight().in(Radians); - _wristMaxGoal.position = setpoint.getAngle().in(Radians); + _elevatorProfile = new TrapezoidProfile(_elevatorMaxConstraints); + _wristProfile = new TrapezoidProfile(_wristMaxConstraints); - _elevatorMaxProfile.calculate(0, _elevatorMaxState, _elevatorMaxGoal); - _wristMaxProfile.calculate(0, _wristMaxState, _wristMaxGoal); + _elevatorProfile.calculate(0, _elevatorSetpoint, _elevatorGoal); + _wristProfile.calculate(0, _wristSetpoint, _wristGoal); - double elevatorTime = _elevatorMaxProfile.totalTime(); - double wristTime = _wristMaxProfile.totalTime(); + double elevatorTime = _elevatorProfile.totalTime(); + double wristTime = _wristProfile.totalTime(); - TrapezoidProfile fasterProfile = - wristTime > elevatorTime ? _elevatorMaxProfile : _wristMaxProfile; + TrapezoidProfile fasterProfile = wristTime > elevatorTime ? _elevatorProfile : _wristProfile; TrapezoidProfile slowerProfile = - fasterProfile.equals(_elevatorMaxProfile) ? _wristMaxProfile : _elevatorMaxProfile; + fasterProfile.equals(_elevatorProfile) ? _wristProfile : _elevatorProfile; double fasterDistance = - fasterProfile.equals(_elevatorMaxProfile) - ? setpoint.getHeight().in(Radians) - getHeight() - : setpoint.getAngle().in(Radians) - getAngle(); + fasterProfile.equals(_elevatorProfile) + ? setpoint.getHeight().in(Rotations) - Units.radiansToRotations(getHeight()) + : setpoint.getAngle().in(Rotations) - Units.radiansToRotations(getAngle()); // slower profile cruise velocity and acceleration double slowerVel = - slowerProfile.equals(_elevatorMaxProfile) + slowerProfile.equals(_elevatorProfile) ? _elevatorMaxConstraints.maxVelocity : _wristMaxConstraints.maxVelocity; double slowerAccel = - slowerProfile.equals(_elevatorMaxProfile) + slowerProfile.equals(_elevatorProfile) ? _elevatorMaxConstraints.maxAcceleration : _wristMaxConstraints.maxAcceleration; @@ -359,27 +367,27 @@ private void findProfileConstraints(Setpoint setpoint) { double adjustedAccel = adjustedVel / slowerAccelTime; var elevatorConstraints = - fasterProfile.equals(_elevatorMaxProfile) + fasterProfile.equals(_elevatorProfile) ? new Constraints(adjustedVel, adjustedAccel) : _elevatorMaxConstraints; var wristContraints = - fasterProfile.equals(_wristMaxProfile) + fasterProfile.equals(_wristProfile) ? new Constraints(adjustedVel, adjustedAccel) : _wristMaxConstraints; - // re-assign profile constraints to motors - _heightSetter.Velocity = Units.radiansToRotations(elevatorConstraints.maxVelocity); - _heightSetter.Acceleration = Units.radiansToRotations(elevatorConstraints.maxAcceleration); + // re-assign profile constraints to motion profiles + _elevatorProfile = new TrapezoidProfile(elevatorConstraints); + _wristProfile = new TrapezoidProfile(wristContraints); - _angleSetter.Velocity = Units.radiansToRotations(wristContraints.maxVelocity); - _angleSetter.Acceleration = Units.radiansToRotations(wristContraints.maxAcceleration); + _elevatorProfile.calculate(0, _elevatorSetpoint, _elevatorGoal); + _wristProfile.calculate(0, _wristSetpoint, _wristGoal); } /** Finds the next setpoint variable given the previous setpoint variable and the goal. */ private void findNextSetpoint(Setpoint goal) { // if we just came from manual or are in between verticies, go to an intermediate - if (_isManual || !atSetpoint(_nextSetpoint)) { + if (_isManual || !finishedProfiles(_nextSetpoint)) { Intermediate closest = Intermediate.INFINITY; // find the closest intermediate vertex @@ -417,24 +425,31 @@ public Command switchToManual() { public Command setGoal(Setpoint goal) { return run(() -> { // once the next setpoint is reached, re-find the next one - if (atSetpoint(_nextSetpoint)) { + if (finishedProfiles(_nextSetpoint)) { findNextSetpoint(goal); findProfileConstraints(_nextSetpoint); } - // log what's generated by wpilib - DogLog.log("Wristevator/Non-Adjusted Desired Elevator Speed", _elevatorMaxState.velocity); - DogLog.log("Wristevator/Non-Adjusted Desired Wrist Speed", _wristMaxState.velocity); + _elevatorSetpoint = + _elevatorProfile.calculate(Robot.kDefaultPeriod, _elevatorSetpoint, _elevatorGoal); + _wristSetpoint = + _wristProfile.calculate(Robot.kDefaultPeriod, _wristSetpoint, _wristGoal); - _elevatorMaxState = - _elevatorMaxProfile.calculate( - Robot.kDefaultPeriod, _elevatorMaxState, _elevatorMaxGoal); - _wristMaxState = - _wristMaxProfile.calculate(Robot.kDefaultPeriod, _wristMaxState, _wristMaxGoal); + DogLog.log( + "Wristevator/Elevator Profile", Units.rotationsToRadians(_elevatorSetpoint.velocity)); + DogLog.log( + "Wristevator/Wrist Profile", Units.rotationsToRadians(_wristSetpoint.velocity)); // travel to next setpoint - _leftMotor.setControl(_heightSetter.withPosition(_nextSetpoint.getHeight())); - _wristMotor.setControl(_angleSetter.withPosition(_nextSetpoint.getAngle())); + _leftMotor.setControl( + _heightSetter + .withPosition(_elevatorSetpoint.position) + .withVelocity(_elevatorSetpoint.velocity)); + + _wristMotor.setControl( + _angleSetter + .withPosition(_wristSetpoint.position) + .withVelocity(_wristSetpoint.velocity)); }) .beforeStarting( idle() @@ -449,7 +464,7 @@ public Command setGoal(Setpoint goal) { _isManual = false; })) - .until(() -> atSetpoint(goal)) + .until(() -> finishedProfiles(goal)) .withName("Set Goal"); } @@ -475,12 +490,6 @@ public Command setSpeeds(DoubleSupplier elevatorSpeed, DoubleSupplier wristSpeed public void periodic() { super.periodic(); - DogLog.log( - "Wristevator/Elevator Reference Slope", - Units.rotationsToRadians(_leftMotor.getClosedLoopReferenceSlope().getValueAsDouble())); - DogLog.log( - "Wristevator/Wrist Reference Slope", - Units.rotationsToRadians(_wristMotor.getClosedLoopReferenceSlope().getValueAsDouble())); DogLog.log( "Wristevator/Elevator Reference", Units.rotationsToRadians(_leftMotor.getClosedLoopReference().getValueAsDouble())); From 8971c8f7d76160c2c6ab0da225422af41a9fbf7e Mon Sep 17 00:00:00 2001 From: Peter Gutkovich Date: Thu, 6 Feb 2025 12:34:30 -0500 Subject: [PATCH 02/10] wrist does not work --- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/Wristevator.java | 68 +++++++++++++------ 2 files changed, 48 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e2631d9..b0c4a96 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -203,7 +203,7 @@ private void configureOperatorBindings() { _operatorController.start().onTrue(_wristevator.setGoal(HUMAN)); _operatorController.rightStick().onTrue(_wristevator.setGoal(HOME)); - _operatorController.a().onTrue(_wristevator.setGoal(L1)); + _operatorController.a().onTrue(_wristevator.dumbTest()); _operatorController .b() diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index 57665e1..493364e 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -80,8 +80,8 @@ public class Wristevator extends AdvancedSubsystem { // elevator profiling private final Constraints _elevatorMaxConstraints = new Constraints( - WristevatorConstants.maxElevatorSpeed.in(RotationsPerSecond), - WristevatorConstants.maxElevatorAcceleration.in(RotationsPerSecondPerSecond)); + WristevatorConstants.maxElevatorSpeed.in(RadiansPerSecond), + WristevatorConstants.maxElevatorAcceleration.in(RadiansPerSecondPerSecond)); private TrapezoidProfile _elevatorProfile = new TrapezoidProfile(_elevatorMaxConstraints); @@ -91,8 +91,8 @@ public class Wristevator extends AdvancedSubsystem { // wrist profiling private final Constraints _wristMaxConstraints = new Constraints( - WristevatorConstants.maxWristSpeed.in(RotationsPerSecond), - WristevatorConstants.maxWristAcceleration.in(RotationsPerSecondPerSecond)); + WristevatorConstants.maxWristSpeed.in(RadiansPerSecond), + WristevatorConstants.maxWristAcceleration.in(RadiansPerSecondPerSecond)); private TrapezoidProfile _wristProfile = new TrapezoidProfile(_wristMaxConstraints); @@ -301,22 +301,22 @@ private double distance(Setpoint b) { private boolean finishedProfiles(Setpoint setpoint) { System.out.println(_elevatorProfile.timeLeftUntil(setpoint.getHeight().in(Rotations))); - return _elevatorProfile.timeLeftUntil(setpoint.getHeight().in(Rotations)) == 0.001 - && _wristProfile.timeLeftUntil(setpoint.getAngle().in(Rotations)) == 0.001; + return _elevatorProfile.timeLeftUntil(setpoint.getHeight().in(Radians)) == 0.001 + && _wristProfile.timeLeftUntil(setpoint.getAngle().in(Radians)) == 0.001; } /** Find new constraints for the motion magic control requests. */ private void findProfileConstraints(Setpoint setpoint) { - _elevatorSetpoint.position = Units.radiansToRotations(getHeight()); + _elevatorSetpoint.position = getHeight(); _elevatorSetpoint.velocity = 0; - _wristSetpoint.position = Units.radiansToRotations(getAngle()); + _wristSetpoint.position = getAngle(); _wristSetpoint.velocity = 0; - _elevatorGoal.position = setpoint.getHeight().in(Rotations); + _elevatorGoal.position = setpoint.getHeight().in(Radians); _elevatorGoal.velocity = 0; - _wristGoal.position = setpoint.getAngle().in(Rotations); + _wristGoal.position = setpoint.getAngle().in(Radians); _wristGoal.velocity = 0; _elevatorProfile = new TrapezoidProfile(_elevatorMaxConstraints); @@ -334,8 +334,8 @@ private void findProfileConstraints(Setpoint setpoint) { double fasterDistance = fasterProfile.equals(_elevatorProfile) - ? setpoint.getHeight().in(Rotations) - Units.radiansToRotations(getHeight()) - : setpoint.getAngle().in(Rotations) - Units.radiansToRotations(getAngle()); + ? setpoint.getHeight().in(Radians) - getHeight() + : setpoint.getAngle().in(Radians) - getAngle(); // slower profile cruise velocity and acceleration double slowerVel = @@ -421,6 +421,34 @@ public Command switchToManual() { return Commands.runOnce(() -> _isManual = true).withName("Switch To Manual"); } + public Command dumbTest() { + return run(() -> { + _elevatorSetpoint = + _elevatorProfile.calculate(Robot.kDefaultPeriod, _elevatorSetpoint, _elevatorGoal); + _wristSetpoint = + _wristProfile.calculate(Robot.kDefaultPeriod, _wristSetpoint, _wristGoal); + + DogLog.log("Wristevator/Elevator Profile", _elevatorSetpoint.velocity); + DogLog.log("Wristevator/Wrist Profile", _wristSetpoint.velocity); + + // travel to next setpoint + _leftMotor.setControl( + _heightSetter + .withPosition(Units.radiansToRotations(_elevatorSetpoint.position)) + .withVelocity(Units.radiansToRotations(_elevatorSetpoint.velocity))); + + _wristMotor.setControl( + _angleSetter + .withPosition(Units.radiansToRotations(_wristSetpoint.position)) + .withVelocity(Units.radiansToRotations(_wristSetpoint.velocity))); + }) + .beforeStarting( + () -> { + // findNextSetpoint(L4); + findProfileConstraints(L4); + }); + } + /** Drives the wristevator to a goal setpoint, going to any intermediate setpoints if needed. */ public Command setGoal(Setpoint goal) { return run(() -> { @@ -435,21 +463,19 @@ public Command setGoal(Setpoint goal) { _wristSetpoint = _wristProfile.calculate(Robot.kDefaultPeriod, _wristSetpoint, _wristGoal); - DogLog.log( - "Wristevator/Elevator Profile", Units.rotationsToRadians(_elevatorSetpoint.velocity)); - DogLog.log( - "Wristevator/Wrist Profile", Units.rotationsToRadians(_wristSetpoint.velocity)); + DogLog.log("Wristevator/Elevator Profile", _elevatorSetpoint.velocity); + DogLog.log("Wristevator/Wrist Profile", _wristSetpoint.velocity); // travel to next setpoint _leftMotor.setControl( _heightSetter - .withPosition(_elevatorSetpoint.position) - .withVelocity(_elevatorSetpoint.velocity)); + .withPosition(Units.radiansToRotations(_elevatorSetpoint.position)) + .withVelocity(Units.radiansToRotations(_elevatorSetpoint.velocity))); _wristMotor.setControl( _angleSetter - .withPosition(_wristSetpoint.position) - .withVelocity(_wristSetpoint.velocity)); + .withPosition(Units.radiansToRotations(_wristSetpoint.position)) + .withVelocity(Units.radiansToRotations(_wristSetpoint.velocity))); }) .beforeStarting( idle() @@ -464,7 +490,7 @@ public Command setGoal(Setpoint goal) { _isManual = false; })) - .until(() -> finishedProfiles(goal)) + // .until(() -> finishedProfiles(goal)) .withName("Set Goal"); } From 48d21df5aa2b1eace4d61e63e6ab5ec06ea6e260 Mon Sep 17 00:00:00 2001 From: Peter Gutkovich Date: Thu, 6 Feb 2025 14:03:29 -0500 Subject: [PATCH 03/10] still just does not work --- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/Wristevator.java | 43 ++++--------------- 2 files changed, 10 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b0c4a96..e2631d9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -203,7 +203,7 @@ private void configureOperatorBindings() { _operatorController.start().onTrue(_wristevator.setGoal(HUMAN)); _operatorController.rightStick().onTrue(_wristevator.setGoal(HOME)); - _operatorController.a().onTrue(_wristevator.dumbTest()); + _operatorController.a().onTrue(_wristevator.setGoal(L1)); _operatorController .b() diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index 493364e..5c91372 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -132,8 +132,6 @@ public Wristevator() { wristMotorConfigs.Feedback.SensorToMechanismRatio = WristevatorConstants.wristGearRatio; - findProfileConstraints(HOME); - CTREUtil.attempt(() -> _leftMotor.getConfigurator().apply(leftMotorConfigs), _leftMotor); CTREUtil.attempt(() -> _rightMotor.getConfigurator().apply(rightMotorConfigs), _rightMotor); CTREUtil.attempt(() -> _wristMotor.getConfigurator().apply(wristMotorConfigs), _wristMotor); @@ -142,6 +140,8 @@ public Wristevator() { FaultLogger.register(_rightMotor); FaultLogger.register(_wristMotor); + findProfileConstraints(HOME); + _rightMotor.setControl(new Follower(WristevatorConstants.leftMotorId, true)); setDefaultCommand(idle()); @@ -272,7 +272,7 @@ public double getHeight() { return _heightGetter.refresh().getValue().in(Radians); } - @Logged(name = "Elevator Angle") + @Logged(name = "Wrist Angle") public double getAngle() { return _angleGetter.refresh().getValue().in(Radians); } @@ -299,8 +299,6 @@ private double distance(Setpoint b) { /** Whether the wristevator finished its most recent profiles. */ private boolean finishedProfiles(Setpoint setpoint) { - System.out.println(_elevatorProfile.timeLeftUntil(setpoint.getHeight().in(Rotations))); - return _elevatorProfile.timeLeftUntil(setpoint.getHeight().in(Radians)) == 0.001 && _wristProfile.timeLeftUntil(setpoint.getAngle().in(Radians)) == 0.001; } @@ -366,6 +364,9 @@ private void findProfileConstraints(Setpoint setpoint) { / (slowerAccel / slowerVel * Math.pow(slowerAccelTime, 2) + slowerCruiseTime); double adjustedAccel = adjustedVel / slowerAccelTime; + adjustedVel = Math.abs(adjustedVel); + adjustedAccel = Math.abs(adjustedAccel); + var elevatorConstraints = fasterProfile.equals(_elevatorProfile) ? new Constraints(adjustedVel, adjustedAccel) @@ -421,34 +422,6 @@ public Command switchToManual() { return Commands.runOnce(() -> _isManual = true).withName("Switch To Manual"); } - public Command dumbTest() { - return run(() -> { - _elevatorSetpoint = - _elevatorProfile.calculate(Robot.kDefaultPeriod, _elevatorSetpoint, _elevatorGoal); - _wristSetpoint = - _wristProfile.calculate(Robot.kDefaultPeriod, _wristSetpoint, _wristGoal); - - DogLog.log("Wristevator/Elevator Profile", _elevatorSetpoint.velocity); - DogLog.log("Wristevator/Wrist Profile", _wristSetpoint.velocity); - - // travel to next setpoint - _leftMotor.setControl( - _heightSetter - .withPosition(Units.radiansToRotations(_elevatorSetpoint.position)) - .withVelocity(Units.radiansToRotations(_elevatorSetpoint.velocity))); - - _wristMotor.setControl( - _angleSetter - .withPosition(Units.radiansToRotations(_wristSetpoint.position)) - .withVelocity(Units.radiansToRotations(_wristSetpoint.velocity))); - }) - .beforeStarting( - () -> { - // findNextSetpoint(L4); - findProfileConstraints(L4); - }); - } - /** Drives the wristevator to a goal setpoint, going to any intermediate setpoints if needed. */ public Command setGoal(Setpoint goal) { return run(() -> { @@ -463,6 +436,8 @@ public Command setGoal(Setpoint goal) { _wristSetpoint = _wristProfile.calculate(Robot.kDefaultPeriod, _wristSetpoint, _wristGoal); + System.out.println(_elevatorProfile.timeLeftUntil(_elevatorGoal.position)); + DogLog.log("Wristevator/Elevator Profile", _elevatorSetpoint.velocity); DogLog.log("Wristevator/Wrist Profile", _wristSetpoint.velocity); @@ -490,7 +465,7 @@ public Command setGoal(Setpoint goal) { _isManual = false; })) - // .until(() -> finishedProfiles(goal)) + .until(() -> finishedProfiles(goal)) .withName("Set Goal"); } From 1bac10517a36222c3ece81d390599edbb86795e0 Mon Sep 17 00:00:00 2001 From: Peter Gutkovich Date: Thu, 6 Feb 2025 19:03:30 -0500 Subject: [PATCH 04/10] ALMOST WORKS UPDATED TO NEXT CTRE VERSION --- .../frc/robot/subsystems/Wristevator.java | 53 ++++++++++------- ...enix6-25.2.1.json => Phoenix6-25.2.2.json} | 58 +++++++++---------- 2 files changed, 60 insertions(+), 51 deletions(-) rename vendordeps/{Phoenix6-25.2.1.json => Phoenix6-25.2.2.json} (92%) diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index ac1ce03..d0acf91 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DIOSim; import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; @@ -82,19 +83,17 @@ public class Wristevator extends AdvancedSubsystem { WristevatorConstants.maxElevatorSpeed.in(RadiansPerSecond), WristevatorConstants.maxElevatorAcceleration.in(RadiansPerSecondPerSecond)); - private State _elevatorMaxState = new State(0, 0); - - private State _wristMaxState = new State(0, 0); - - private final State _elevatorMaxGoal = new State(0, 0); - - private final State _wristMaxGoal = new State(0, 0); + private final State _elevatorMaxGoal = new State(); + private State _elevatorMaxSetpoint = new State(); private final Constraints _wristMaxConstraints = new Constraints( WristevatorConstants.maxWristSpeed.in(RadiansPerSecond), WristevatorConstants.maxWristAcceleration.in(RadiansPerSecondPerSecond)); + private final State _wristMaxGoal = new State(); + private State _wristMaxSetpoint = new State(); + private final TrapezoidProfile _elevatorMaxProfile = new TrapezoidProfile(_elevatorMaxConstraints); @@ -107,6 +106,9 @@ public class Wristevator extends AdvancedSubsystem { @Logged(name = "Is Manual") private boolean _isManual = false; + private final Timer _profileTimer = new Timer(); + private double _profileTime = 0; + private DIOSim _homeSwitchSim; private ElevatorSim _elevatorSim; @@ -271,7 +273,7 @@ public double getHeight() { return _heightGetter.refresh().getValue().in(Radians); } - @Logged(name = "Elevator Angle") + @Logged(name = "Wrist Angle") public double getAngle() { return _angleGetter.refresh().getValue().in(Radians); } @@ -298,23 +300,26 @@ private double distance(Setpoint b) { /** Whether the wristevator is near a setpoint. */ private boolean atSetpoint(Setpoint setpoint) { - return MathUtil.isNear(setpoint.getAngle().in(Radians), getAngle(), 0.0001) - && MathUtil.isNear(setpoint.getHeight().in(Radians), getHeight(), 0.0001); + System.out.println(_profileTimer.get()); + return _profileTimer.hasElapsed(_profileTime) && _nextSetpoint == setpoint; } /** Find new constraints for the motion magic control requests. */ private void findProfileConstraints(Setpoint setpoint) { - _elevatorMaxState.position = getHeight(); - _elevatorMaxState.velocity = 0; + _elevatorMaxSetpoint.position = getHeight(); + _elevatorMaxSetpoint.velocity = 0; - _wristMaxState.position = getAngle(); - _wristMaxState.velocity = 0; + _wristMaxSetpoint.position = getAngle(); + _wristMaxSetpoint.velocity = 0; _elevatorMaxGoal.position = setpoint.getHeight().in(Radians); + _elevatorMaxGoal.velocity = 0; + _wristMaxGoal.position = setpoint.getAngle().in(Radians); + _wristMaxGoal.velocity = 0; - _elevatorMaxProfile.calculate(0, _elevatorMaxState, _elevatorMaxGoal); - _wristMaxProfile.calculate(0, _wristMaxState, _wristMaxGoal); + _elevatorMaxProfile.calculate(0, _elevatorMaxSetpoint, _elevatorMaxGoal); + _wristMaxProfile.calculate(0, _wristMaxSetpoint, _wristMaxGoal); double elevatorTime = _elevatorMaxProfile.totalTime(); double wristTime = _wristMaxProfile.totalTime(); @@ -374,6 +379,9 @@ private void findProfileConstraints(Setpoint setpoint) { _angleSetter.Velocity = Units.radiansToRotations(wristContraints.maxVelocity); _angleSetter.Acceleration = Units.radiansToRotations(wristContraints.maxAcceleration); + + _profileTime = slowerProfile.totalTime(); + _profileTimer.restart(); } /** Finds the next setpoint variable given the previous setpoint variable and the goal. */ @@ -423,14 +431,15 @@ public Command setGoal(Setpoint goal) { } // log what's generated by wpilib - DogLog.log("Wristevator/Non-Adjusted Desired Elevator Speed", _elevatorMaxState.velocity); - DogLog.log("Wristevator/Non-Adjusted Desired Wrist Speed", _wristMaxState.velocity); + DogLog.log( + "Wristevator/Non-Adjusted Desired Elevator Speed", _elevatorMaxSetpoint.velocity); + DogLog.log("Wristevator/Non-Adjusted Desired Wrist Speed", _wristMaxSetpoint.velocity); - _elevatorMaxState = + _elevatorMaxSetpoint = _elevatorMaxProfile.calculate( - Robot.kDefaultPeriod, _elevatorMaxState, _elevatorMaxGoal); - _wristMaxState = - _wristMaxProfile.calculate(Robot.kDefaultPeriod, _wristMaxState, _wristMaxGoal); + Robot.kDefaultPeriod, _elevatorMaxSetpoint, _elevatorMaxGoal); + _wristMaxSetpoint = + _wristMaxProfile.calculate(Robot.kDefaultPeriod, _wristMaxSetpoint, _wristMaxGoal); // travel to next setpoint _leftMotor.setControl(_heightSetter.withPosition(_nextSetpoint.getHeight())); diff --git a/vendordeps/Phoenix6-25.2.1.json b/vendordeps/Phoenix6-25.2.2.json similarity index 92% rename from vendordeps/Phoenix6-25.2.1.json rename to vendordeps/Phoenix6-25.2.2.json index 1397da1..d617643 100644 --- a/vendordeps/Phoenix6-25.2.1.json +++ b/vendordeps/Phoenix6-25.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.2.1.json", + "fileName": "Phoenix6-25.2.2.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", + "version": "25.2.2", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.2.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, From b2844f78438bf1dc5c8395414251c7db6bf1388d Mon Sep 17 00:00:00 2001 From: reesesbowls Date: Fri, 7 Feb 2025 16:58:04 -0500 Subject: [PATCH 05/10] changed fasterdistance slowervelocity and sloweracceleration code in wristevator from a bunch of iternarys to one if else statement idk lol i was bored i have nothing to do --- .../frc/robot/subsystems/Wristevator.java | 29 +++++++++---------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index d0acf91..4742dd7 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -329,21 +329,20 @@ private void findProfileConstraints(Setpoint setpoint) { TrapezoidProfile slowerProfile = fasterProfile.equals(_elevatorMaxProfile) ? _wristMaxProfile : _elevatorMaxProfile; - double fasterDistance = - fasterProfile.equals(_elevatorMaxProfile) - ? setpoint.getHeight().in(Radians) - getHeight() - : setpoint.getAngle().in(Radians) - getAngle(); - - // slower profile cruise velocity and acceleration - double slowerVel = - slowerProfile.equals(_elevatorMaxProfile) - ? _elevatorMaxConstraints.maxVelocity - : _wristMaxConstraints.maxVelocity; - - double slowerAccel = - slowerProfile.equals(_elevatorMaxProfile) - ? _elevatorMaxConstraints.maxAcceleration - : _wristMaxConstraints.maxAcceleration; + double fasterDistance = 0; + double slowerVel = 0; + double slowerAccel = 0; + + if (fasterProfile == _elevatorMaxProfile) { + fasterDistance = setpoint.getHeight().in(Radians) - getHeight(); + slowerVel = _wristMaxConstraints.maxVelocity; + slowerAccel = _wristMaxConstraints.maxAcceleration; + } + else { + fasterDistance = setpoint.getAngle().in(Radians) - getAngle(); + slowerVel = _elevatorMaxConstraints.maxVelocity; + slowerAccel = _elevatorMaxConstraints.maxAcceleration; + } // find the acceleration and cruise times of the slower profile double slowerAccelTime = slowerVel / slowerAccel; From 84a82219bb09e8e9c532e377fd31d4da8bb3ac14 Mon Sep 17 00:00:00 2001 From: Elvis Date: Fri, 7 Feb 2025 17:42:22 -0500 Subject: [PATCH 06/10] fixed conflict --- .../frc/robot/subsystems/Wristevator.java | 62 ++++++++----------- 1 file changed, 27 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index 0e60af3..02c1c14 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -6,8 +6,8 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import dev.doglog.DogLog; @@ -69,8 +69,8 @@ public class Wristevator extends AdvancedSubsystem { private final StatusSignal _heightGetter = _leftMotor.getPosition(); private final StatusSignal _angleGetter = _wristMotor.getPosition(); - private final PositionVoltage _heightSetter = new PositionVoltage(0); - private final PositionVoltage _angleSetter = new PositionVoltage(0); + private final DynamicMotionMagicVoltage _heightSetter = new DynamicMotionMagicVoltage(0, 0, 0, 0); + private final DynamicMotionMagicVoltage _angleSetter = new DynamicMotionMagicVoltage(0, 0, 0, 0); private final StatusSignal _elevatorVelocityGetter = _leftMotor.getVelocity(); private final StatusSignal _wristVelocityGetter = _wristMotor.getVelocity(); @@ -78,7 +78,6 @@ public class Wristevator extends AdvancedSubsystem { private final VelocityVoltage _elevatorVelocitySetter = new VelocityVoltage(0); private final VelocityVoltage _wristVelocitySetter = new VelocityVoltage(0); - // elevator profiling private final Constraints _elevatorMaxConstraints = new Constraints( WristevatorConstants.maxElevatorSpeed.in(RadiansPerSecond), @@ -98,8 +97,7 @@ public class Wristevator extends AdvancedSubsystem { private final TrapezoidProfile _elevatorMaxProfile = new TrapezoidProfile(_elevatorMaxConstraints); - private State _wristSetpoint = new State(); - private final State _wristGoal = new State(); + private final TrapezoidProfile _wristMaxProfile = new TrapezoidProfile(_wristMaxConstraints); private final DigitalInput _homeSwitch = new DigitalInput(WristevatorConstants.homeSwitch); @@ -145,8 +143,6 @@ public Wristevator() { FaultLogger.register(_rightMotor); FaultLogger.register(_wristMotor); - findProfileConstraints(HOME); - _rightMotor.setControl(new Follower(WristevatorConstants.leftMotorId, true)); setDefaultCommand(idle()); @@ -325,23 +321,23 @@ private void findProfileConstraints(Setpoint setpoint) { _elevatorMaxProfile.calculate(0, _elevatorMaxSetpoint, _elevatorMaxGoal); _wristMaxProfile.calculate(0, _wristMaxSetpoint, _wristMaxGoal); - double elevatorTime = _elevatorProfile.totalTime(); - double wristTime = _wristProfile.totalTime(); + double elevatorTime = _elevatorMaxProfile.totalTime(); + double wristTime = _wristMaxProfile.totalTime(); - TrapezoidProfile fasterProfile = wristTime > elevatorTime ? _elevatorProfile : _wristProfile; + TrapezoidProfile fasterProfile = + wristTime > elevatorTime ? _elevatorMaxProfile : _wristMaxProfile; TrapezoidProfile slowerProfile = - fasterProfile.equals(_elevatorProfile) ? _wristProfile : _elevatorProfile; + fasterProfile.equals(_elevatorMaxProfile) ? _wristMaxProfile : _elevatorMaxProfile; double fasterDistance = 0; double slowerVel = 0; double slowerAccel = 0; - + if (fasterProfile == _elevatorMaxProfile) { fasterDistance = setpoint.getHeight().in(Radians) - getHeight(); slowerVel = _wristMaxConstraints.maxVelocity; slowerAccel = _wristMaxConstraints.maxAcceleration; - } - else { + } else { fasterDistance = setpoint.getAngle().in(Radians) - getAngle(); slowerVel = _elevatorMaxConstraints.maxVelocity; slowerAccel = _elevatorMaxConstraints.maxAcceleration; @@ -365,22 +361,19 @@ private void findProfileConstraints(Setpoint setpoint) { / (slowerAccel / slowerVel * Math.pow(slowerAccelTime, 2) + slowerCruiseTime); double adjustedAccel = adjustedVel / slowerAccelTime; - adjustedVel = Math.abs(adjustedVel); - adjustedAccel = Math.abs(adjustedAccel); - var elevatorConstraints = - fasterProfile.equals(_elevatorProfile) + fasterProfile.equals(_elevatorMaxProfile) ? new Constraints(adjustedVel, adjustedAccel) : _elevatorMaxConstraints; var wristContraints = - fasterProfile.equals(_wristProfile) + fasterProfile.equals(_wristMaxProfile) ? new Constraints(adjustedVel, adjustedAccel) : _wristMaxConstraints; - // re-assign profile constraints to motion profiles - _elevatorProfile = new TrapezoidProfile(elevatorConstraints); - _wristProfile = new TrapezoidProfile(wristContraints); + // re-assign profile constraints to motors + _heightSetter.Velocity = Units.radiansToRotations(elevatorConstraints.maxVelocity); + _heightSetter.Acceleration = Units.radiansToRotations(elevatorConstraints.maxAcceleration); _angleSetter.Velocity = Units.radiansToRotations(wristContraints.maxVelocity); _angleSetter.Acceleration = Units.radiansToRotations(wristContraints.maxAcceleration); @@ -392,7 +385,7 @@ private void findProfileConstraints(Setpoint setpoint) { /** Finds the next setpoint variable given the previous setpoint variable and the goal. */ private void findNextSetpoint(Setpoint goal) { // if we just came from manual or are in between verticies, go to an intermediate - if (_isManual || !finishedProfiles(_nextSetpoint)) { + if (_isManual || !atSetpoint(_nextSetpoint)) { Intermediate closest = Intermediate.INFINITY; // find the closest intermediate vertex @@ -430,7 +423,7 @@ public Command switchToManual() { public Command setGoal(Setpoint goal) { return run(() -> { // once the next setpoint is reached, re-find the next one - if (finishedProfiles(_nextSetpoint)) { + if (atSetpoint(_nextSetpoint)) { findNextSetpoint(goal); findProfileConstraints(_nextSetpoint); } @@ -447,15 +440,8 @@ public Command setGoal(Setpoint goal) { _wristMaxProfile.calculate(Robot.kDefaultPeriod, _wristMaxSetpoint, _wristMaxGoal); // travel to next setpoint - _leftMotor.setControl( - _heightSetter - .withPosition(Units.radiansToRotations(_elevatorSetpoint.position)) - .withVelocity(Units.radiansToRotations(_elevatorSetpoint.velocity))); - - _wristMotor.setControl( - _angleSetter - .withPosition(Units.radiansToRotations(_wristSetpoint.position)) - .withVelocity(Units.radiansToRotations(_wristSetpoint.velocity))); + _leftMotor.setControl(_heightSetter.withPosition(_nextSetpoint.getHeight())); + _wristMotor.setControl(_angleSetter.withPosition(_nextSetpoint.getAngle())); }) .beforeStarting( idle() @@ -470,7 +456,7 @@ public Command setGoal(Setpoint goal) { _isManual = false; })) - .until(() -> finishedProfiles(goal)) + .until(() -> atSetpoint(goal)) .withName("Set Goal"); } @@ -496,6 +482,12 @@ public Command setSpeeds(DoubleSupplier elevatorSpeed, DoubleSupplier wristSpeed public void periodic() { super.periodic(); + DogLog.log( + "Wristevator/Elevator Reference Slope", + Units.rotationsToRadians(_leftMotor.getClosedLoopReferenceSlope().getValueAsDouble())); + DogLog.log( + "Wristevator/Wrist Reference Slope", + Units.rotationsToRadians(_wristMotor.getClosedLoopReferenceSlope().getValueAsDouble())); DogLog.log( "Wristevator/Elevator Reference", Units.rotationsToRadians(_leftMotor.getClosedLoopReference().getValueAsDouble())); From 8f6fe8ee3e72acbf7ff8023dfb213b86239c7b5d Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 7 Feb 2025 20:48:26 -0500 Subject: [PATCH 07/10] works --- notes.md | 3 + src/main/java/frc/robot/Constants.java | 6 +- .../frc/robot/subsystems/Wristevator.java | 156 ++++++++++-------- 3 files changed, 97 insertions(+), 68 deletions(-) diff --git a/notes.md b/notes.md index 9e4b8bc..6827722 100644 --- a/notes.md +++ b/notes.md @@ -22,3 +22,6 @@ Noise is innaccurate measurements that occur from time to time from a sensor or ### Ambiguity When detecting pose using an AprilTag (4 corners), it is often possible to get two pose solutions from solvePnP, each with their own re-projection errors. If the re-projection errors are very close or equal, it is hard to tell which pose solution is the correct one, making the problem ambigious. In this case, the correct pose can be chosen by comparing the two to a different robot sensor, the gyro for example, and determining which one is correct. In terms of filtering, measurements with high ambiguity are typically correlated with high noise (a tag seen at a large distance would cause both ambiguity and corner noise), so those get rejected. At lower ambiguities, the measurement with better re-projection error may not necessarily be the correct measurement. Corner noise may cause for one innaccurate low re-projection error pose along with a more accurate higher re-projection error pose, so the correct one for lower ambiguities should also be determined using the gyro. + +## Wristevator Corner Cases + diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8d55099..c4ba579 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -166,9 +166,9 @@ public Angle getHeight() { /** Wristevator intermediate setpoints. */ public static enum Intermediate implements Setpoint { INFINITY(Radians.of(Integer.MAX_VALUE), Radians.of(Integer.MAX_VALUE)), - I1(Radians.of(0), Radians.of(50)), - I2(Radians.of(-1), Radians.of(30)), - I3(Radians.of(1), Radians.of(50)); + I1(Radians.of(0), Radians.of(50)); + // I2(Radians.of(-1), Radians.of(30)), + // I3(Radians.of(1), Radians.of(50)); private final Angle _angle; private final Angle _height; diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index d0acf91..476d97f 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.Constants.WristevatorConstants.Preset.*; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -10,6 +11,7 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotionMagicIsRunningValue; import dev.doglog.DogLog; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; @@ -25,7 +27,6 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DIOSim; import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; @@ -72,12 +73,24 @@ public class Wristevator extends AdvancedSubsystem { private final DynamicMotionMagicVoltage _heightSetter = new DynamicMotionMagicVoltage(0, 0, 0, 0); private final DynamicMotionMagicVoltage _angleSetter = new DynamicMotionMagicVoltage(0, 0, 0, 0); + private final StatusSignal _isMotionMagic = + _leftMotor.getMotionMagicIsRunning(); + + private final StatusSignal _elevatorReference = _leftMotor.getClosedLoopReference(); + private final StatusSignal _elevatorReferenceSlope = + _leftMotor.getClosedLoopReferenceSlope(); + + private final StatusSignal _wristReference = _wristMotor.getClosedLoopReference(); + private final StatusSignal _wristReferenceSlope = + _wristMotor.getClosedLoopReferenceSlope(); + private final StatusSignal _elevatorVelocityGetter = _leftMotor.getVelocity(); private final StatusSignal _wristVelocityGetter = _wristMotor.getVelocity(); private final VelocityVoltage _elevatorVelocitySetter = new VelocityVoltage(0); private final VelocityVoltage _wristVelocitySetter = new VelocityVoltage(0); + // elevator profile private final Constraints _elevatorMaxConstraints = new Constraints( WristevatorConstants.maxElevatorSpeed.in(RadiansPerSecond), @@ -86,6 +99,10 @@ public class Wristevator extends AdvancedSubsystem { private final State _elevatorMaxGoal = new State(); private State _elevatorMaxSetpoint = new State(); + private final TrapezoidProfile _elevatorMaxProfile = + new TrapezoidProfile(_elevatorMaxConstraints); + + // wrist profile private final Constraints _wristMaxConstraints = new Constraints( WristevatorConstants.maxWristSpeed.in(RadiansPerSecond), @@ -94,9 +111,6 @@ public class Wristevator extends AdvancedSubsystem { private final State _wristMaxGoal = new State(); private State _wristMaxSetpoint = new State(); - private final TrapezoidProfile _elevatorMaxProfile = - new TrapezoidProfile(_elevatorMaxConstraints); - private final TrapezoidProfile _wristMaxProfile = new TrapezoidProfile(_wristMaxConstraints); private final DigitalInput _homeSwitch = new DigitalInput(WristevatorConstants.homeSwitch); @@ -106,9 +120,6 @@ public class Wristevator extends AdvancedSubsystem { @Logged(name = "Is Manual") private boolean _isManual = false; - private final Timer _profileTimer = new Timer(); - private double _profileTime = 0; - private DIOSim _homeSwitchSim; private ElevatorSim _elevatorSim; @@ -145,7 +156,7 @@ public Wristevator() { _rightMotor.setControl(new Follower(WristevatorConstants.leftMotorId, true)); - setDefaultCommand(idle()); + setDefaultCommand(holdInPlace()); Translation2d[] presets = new Translation2d[Preset.values().length]; Translation2d[] intermediates = new Translation2d[Intermediate.values().length]; @@ -283,8 +294,22 @@ public boolean homeSwitch() { return _homeSwitch.get(); } - private Command idle() { - return setSpeeds(() -> 0, () -> 0).withName("Idle"); + /** Whether the wristevator is open for manual control or not. */ + public boolean isManual() { + return _isManual; + } + + /** Indicate switch to manual control. */ + public Command switchToManual() { + return Commands.runOnce(() -> _isManual = true).withName("Switch To Manual"); + } + + private Command holdInPlace() { + return run(() -> { + _leftMotor.setControl(_heightSetter); + _wristMotor.setControl(_angleSetter); + }) + .withName("Hold In Place"); } // distance between current position and supplied setpoint @@ -298,10 +323,41 @@ private double distance(Setpoint b) { return Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2)); } - /** Whether the wristevator is near a setpoint. */ - private boolean atSetpoint(Setpoint setpoint) { - System.out.println(_profileTimer.get()); - return _profileTimer.hasElapsed(_profileTime) && _nextSetpoint == setpoint; + /** Whether the wristevator profiles finished at the specified goal. */ + private boolean finishedProfiles(Setpoint setpoint) { + // only need to check one profile since they run the same amount of time + return MathUtil.isNear( + setpoint.getHeight().in(Rotations), _elevatorReference.getValueAsDouble(), 0.001) + && MathUtil.isNear(0, _elevatorReferenceSlope.getValueAsDouble(), 0.001) + && _isMotionMagic.getValue() == MotionMagicIsRunningValue.Enabled; + } + + /** Finds the next setpoint variable given the previous setpoint variable and the goal. */ + private void findNextSetpoint(Setpoint goal) { + // if we just came from manual + // or if we haven't finished the previous profiles + if (_isManual || !finishedProfiles(_nextSetpoint)) { + Intermediate closest = Intermediate.INFINITY; + + // find the closest intermediate vertex + for (Intermediate intermediate : Intermediate.values()) { + if (distance(intermediate) < distance(closest)) { + closest = intermediate; + } + } + + _nextSetpoint = closest; + + return; + } + + if (WristevatorConstants.setpointMap.containsKey(Pair.of(_nextSetpoint, goal))) { + _nextSetpoint = WristevatorConstants.setpointMap.get(Pair.of(_nextSetpoint, goal)); + + return; + } + + _nextSetpoint = goal; } /** Find new constraints for the motion magic control requests. */ @@ -379,53 +435,13 @@ private void findProfileConstraints(Setpoint setpoint) { _angleSetter.Velocity = Units.radiansToRotations(wristContraints.maxVelocity); _angleSetter.Acceleration = Units.radiansToRotations(wristContraints.maxAcceleration); - - _profileTime = slowerProfile.totalTime(); - _profileTimer.restart(); - } - - /** Finds the next setpoint variable given the previous setpoint variable and the goal. */ - private void findNextSetpoint(Setpoint goal) { - // if we just came from manual or are in between verticies, go to an intermediate - if (_isManual || !atSetpoint(_nextSetpoint)) { - Intermediate closest = Intermediate.INFINITY; - - // find the closest intermediate vertex - for (Intermediate intermediate : Intermediate.values()) { - if (distance(intermediate) < distance(closest)) { - closest = intermediate; - } - } - - _nextSetpoint = closest; - - return; - } - - if (WristevatorConstants.setpointMap.containsKey(Pair.of(_nextSetpoint, goal))) { - _nextSetpoint = WristevatorConstants.setpointMap.get(Pair.of(_nextSetpoint, goal)); - - return; - } - - _nextSetpoint = goal; - } - - /** Whether the wristevator is open for manual control or not. */ - public boolean isManual() { - return _isManual; - } - - /** Indicate switch to manual control. */ - public Command switchToManual() { - return Commands.runOnce(() -> _isManual = true).withName("Switch To Manual"); } /** Drives the wristevator to a goal setpoint, going to any intermediate setpoints if needed. */ public Command setGoal(Setpoint goal) { return run(() -> { // once the next setpoint is reached, re-find the next one - if (atSetpoint(_nextSetpoint)) { + if (finishedProfiles(_nextSetpoint)) { findNextSetpoint(goal); findProfileConstraints(_nextSetpoint); } @@ -441,12 +457,12 @@ public Command setGoal(Setpoint goal) { _wristMaxSetpoint = _wristMaxProfile.calculate(Robot.kDefaultPeriod, _wristMaxSetpoint, _wristMaxGoal); - // travel to next setpoint + // move towards the next setpoint _leftMotor.setControl(_heightSetter.withPosition(_nextSetpoint.getHeight())); _wristMotor.setControl(_angleSetter.withPosition(_nextSetpoint.getAngle())); }) .beforeStarting( - idle() + setSpeeds(() -> 0, () -> 0) .until( () -> MathUtil.isNear(0, getElevatorVelocity(), 0.01) @@ -458,7 +474,7 @@ public Command setGoal(Setpoint goal) { _isManual = false; })) - .until(() -> atSetpoint(goal)) + .until(() -> finishedProfiles(goal)) .withName("Set Goal"); } @@ -484,18 +500,28 @@ public Command setSpeeds(DoubleSupplier elevatorSpeed, DoubleSupplier wristSpeed public void periodic() { super.periodic(); + BaseStatusSignal.refreshAll( + _isMotionMagic, + _elevatorReference, + _elevatorReferenceSlope, + _wristReference, + _wristReferenceSlope); + DogLog.log( - "Wristevator/Elevator Reference Slope", - Units.rotationsToRadians(_leftMotor.getClosedLoopReferenceSlope().getValueAsDouble())); - DogLog.log( - "Wristevator/Wrist Reference Slope", - Units.rotationsToRadians(_wristMotor.getClosedLoopReferenceSlope().getValueAsDouble())); + "Wristevator/Is Motion Magic", + _isMotionMagic.getValue() == MotionMagicIsRunningValue.Enabled); DogLog.log( "Wristevator/Elevator Reference", - Units.rotationsToRadians(_leftMotor.getClosedLoopReference().getValueAsDouble())); + Units.rotationsToRadians(_elevatorReference.getValueAsDouble())); DogLog.log( "Wristevator/Wrist Reference", - Units.rotationsToRadians(_wristMotor.getClosedLoopReference().getValueAsDouble())); + Units.rotationsToRadians(_wristReference.getValueAsDouble())); + DogLog.log( + "Wristevator/Elevator Reference Slope", + Units.rotationsToRadians(_elevatorReferenceSlope.getValueAsDouble())); + DogLog.log( + "Wristevator/Wrist Reference Slope", + Units.rotationsToRadians(_wristReferenceSlope.getValueAsDouble())); DogLog.log("Wristevator/Next Setpoint", _nextSetpoint.toString()); From a6798fadf0251e069be6de9059a44915e000838a Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 7 Feb 2025 22:22:49 -0500 Subject: [PATCH 08/10] finally works after so long --- notes.md | 3 -- src/main/java/frc/robot/Constants.java | 8 +++--- .../frc/robot/subsystems/Wristevator.java | 28 ++++++++++++------- 3 files changed, 22 insertions(+), 17 deletions(-) diff --git a/notes.md b/notes.md index 6827722..9e4b8bc 100644 --- a/notes.md +++ b/notes.md @@ -22,6 +22,3 @@ Noise is innaccurate measurements that occur from time to time from a sensor or ### Ambiguity When detecting pose using an AprilTag (4 corners), it is often possible to get two pose solutions from solvePnP, each with their own re-projection errors. If the re-projection errors are very close or equal, it is hard to tell which pose solution is the correct one, making the problem ambigious. In this case, the correct pose can be chosen by comparing the two to a different robot sensor, the gyro for example, and determining which one is correct. In terms of filtering, measurements with high ambiguity are typically correlated with high noise (a tag seen at a large distance would cause both ambiguity and corner noise), so those get rejected. At lower ambiguities, the measurement with better re-projection error may not necessarily be the correct measurement. Corner noise may cause for one innaccurate low re-projection error pose along with a more accurate higher re-projection error pose, so the correct one for lower ambiguities should also be determined using the gyro. - -## Wristevator Corner Cases - diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 76a8c41..6587df0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,6 +5,8 @@ package frc.robot; import static edu.wpi.first.units.Units.*; +// import static frc.robot.Constants.WristevatorConstants.Preset.*; +// import static frc.robot.Constants.WristevatorConstants.Intermediate.*; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -166,9 +168,7 @@ public Angle getHeight() { /** Wristevator intermediate setpoints. */ public static enum Intermediate implements Setpoint { INFINITY(Radians.of(Integer.MAX_VALUE), Radians.of(Integer.MAX_VALUE)), - I1(Radians.of(-2), Radians.of(100)), - I2(Radians.of(-2), Radians.of(100)), - I3(Radians.of(1), Radians.of(50)); + I1(Radians.of(0), Radians.of(50)); private final Angle _angle; private final Angle _height; @@ -189,7 +189,7 @@ public Angle getHeight() { } } - public static final HashMap, Preset> setpointMap = new HashMap<>(); + public static final HashMap, Setpoint> setpointMap = new HashMap<>(); static { // TODO: actually find setpoint map and put it here diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index 04f9ca2..374fcec 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -70,8 +70,10 @@ public class Wristevator extends AdvancedSubsystem { private final StatusSignal _heightGetter = _leftMotor.getPosition(); private final StatusSignal _angleGetter = _wristMotor.getPosition(); - private final DynamicMotionMagicVoltage _heightSetter = new DynamicMotionMagicVoltage(0, 0, 0, 0); - private final DynamicMotionMagicVoltage _angleSetter = new DynamicMotionMagicVoltage(0, 0, 0, 0); + private final DynamicMotionMagicVoltage _heightSetter = + new DynamicMotionMagicVoltage(HOME.getHeight().in(Rotations), 0, 0, 0); + private final DynamicMotionMagicVoltage _angleSetter = + new DynamicMotionMagicVoltage(HOME.getAngle().in(Rotations), 0, 0, 0); private final StatusSignal _isMotionMagic = _leftMotor.getMotionMagicIsRunning(); @@ -308,6 +310,8 @@ private Command holdInPlace() { return run(() -> { _leftMotor.setControl(_heightSetter); _wristMotor.setControl(_angleSetter); + + refreshProfileReferences(); }) .withName("Hold In Place"); } @@ -323,13 +327,20 @@ private double distance(Setpoint b) { return Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2)); } + // refresh the references of the talonfx profiles + private void refreshProfileReferences() { + if (_isMotionMagic.getValue() == MotionMagicIsRunningValue.Disabled) return; + + BaseStatusSignal.refreshAll( + _elevatorReference, _elevatorReferenceSlope, _wristReference, _wristReferenceSlope); + } + /** Whether the wristevator profiles finished at the specified goal. */ private boolean finishedProfiles(Setpoint setpoint) { // only need to check one profile since they run the same amount of time return MathUtil.isNear( setpoint.getHeight().in(Rotations), _elevatorReference.getValueAsDouble(), 0.001) - && MathUtil.isNear(0, _elevatorReferenceSlope.getValueAsDouble(), 0.001) - && _isMotionMagic.getValue() == MotionMagicIsRunningValue.Enabled; + && MathUtil.isNear(0, _elevatorReferenceSlope.getValueAsDouble(), 0.001); } /** Finds the next setpoint variable given the previous setpoint variable and the goal. */ @@ -458,6 +469,8 @@ public Command setGoal(Setpoint goal) { // move towards the next setpoint _leftMotor.setControl(_heightSetter.withPosition(_nextSetpoint.getHeight())); _wristMotor.setControl(_angleSetter.withPosition(_nextSetpoint.getAngle())); + + refreshProfileReferences(); }) .beforeStarting( setSpeeds(() -> 0, () -> 0) @@ -498,12 +511,7 @@ public Command setSpeeds(DoubleSupplier elevatorSpeed, DoubleSupplier wristSpeed public void periodic() { super.periodic(); - BaseStatusSignal.refreshAll( - _isMotionMagic, - _elevatorReference, - _elevatorReferenceSlope, - _wristReference, - _wristReferenceSlope); + _isMotionMagic.refresh(); DogLog.log( "Wristevator/Is Motion Magic", From ff39c729596ac4eb6c62863b5dd2c34c6239e96c Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 7 Feb 2025 22:30:40 -0500 Subject: [PATCH 09/10] dumb spotless thing --- src/main/java/frc/robot/subsystems/Wristevator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index 374fcec..ef97e78 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -39,7 +39,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.AdvancedSubsystem; -import frc.lib.CTREUtil; +import frc.lib.CTREUtil; // something import frc.lib.FaultLogger; import frc.robot.Constants; import frc.robot.Constants.WristevatorConstants; From 6c2d2a8326acda6cbc6ea84694e9d79934c7d920 Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 7 Feb 2025 22:30:59 -0500 Subject: [PATCH 10/10] dumb spotless thing --- src/main/java/frc/robot/subsystems/Wristevator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index ef97e78..374fcec 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -39,7 +39,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.AdvancedSubsystem; -import frc.lib.CTREUtil; // something +import frc.lib.CTREUtil; import frc.lib.FaultLogger; import frc.robot.Constants; import frc.robot.Constants.WristevatorConstants;