From 2addd6c3585436063d821ab3c5e7ecb16f25b044 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Mon, 15 May 2023 17:29:36 -0400 Subject: [PATCH 01/15] Started new arm code (Beak) Co-authored-by: Myles Pasetsky Co-authored-by: reyamiller --- .../stuypulse/robot/subsystems/arm/Arm.java | 142 ++++++++++-------- .../stuypulse/robot/subsystems/arm/Beak.java | 40 +++++ .../robot/subsystems/arm/BeakImpl.java | 5 + 3 files changed, 124 insertions(+), 63 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/arm/Beak.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/arm/BeakImpl.java diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 12ed3d38..6dcdb951 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -40,15 +40,15 @@ import java.util.Optional; - /** - * Double jointed arm controlled by two motion profiled PID controllers. - * - * Available control "modes": - * - setpoint control (PID+FF controllers are used) (shoulder is not allowed above maximum shoulder angle) - * - limp mode (controller output is overriden to be zero) - * - voltage override ("force" feeds a voltage to the motor) - */ + * Double jointed arm controlled by two motion profiled PID controllers. + * + * Available control "modes": + * - setpoint control (PID+FF controllers are used) (shoulder is not allowed + * above maximum shoulder angle) + * - limp mode (controller output is overriden to be zero) + * - voltage override ("force" feeds a voltage to the motor) + */ public abstract class Arm extends SubsystemBase { // Singleton @@ -122,75 +122,81 @@ public double doubleValue() { @Override public float floatValue() { - return (float)doubleValue(); + return (float) doubleValue(); } @Override public int intValue() { - return (int)doubleValue(); + return (int) doubleValue(); } @Override public long longValue() { - return (long)doubleValue(); + return (long) doubleValue(); } } protected Arm() { + // These are the setpoints for the joints relative to the "horizontal" (like the + // unit circle) -- keep both shoulderTargetDegrees = new SmartNumber("Arm/Shoulder/Target Angle (deg)", -90); wristTargetDegrees = new SmartNumber("Arm/Wrist/Target Angle (deg)", +90); + // These numbers are used for disabling/enabling wrist control while the + // shoulder is moving -- they are no longer necessary. Remove them shoulderVelocityFeedbackDebounce = new SmartNumber( - "Arm/Wrist/Feedback Enabled Debounce", - Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue()); + "Arm/Wrist/Feedback Enabled Debounce", + Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue()); shoulderVelocityFeedbackCutoff = new SmartNumber( - "Arm/Wrist/Shoulder Velocity Feedback Cutoff (deg per s)", - Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); + "Arm/Wrist/Shoulder Velocity Feedback Cutoff (deg per s)", + Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); wristEnabled = BStream.create(this::isWristFeedbackEnabled) - .filtered(new BDebounce.Both(shoulderVelocityFeedbackDebounce)); + .filtered(new BDebounce.Both(shoulderVelocityFeedbackDebounce)); shoulderMaxVelocity = new SmartNumber( - "Arm/Shoulder/Max Velocity", - Shoulder.TELEOP_MAX_VELOCITY.doubleValue()); + "Arm/Shoulder/Max Velocity", + Shoulder.TELEOP_MAX_VELOCITY.doubleValue()); shoulderMaxAcceleration = new SmartNumber( - "Arm/Shoulder/Max Acceleration", - Shoulder.TELEOP_MAX_ACCELERATION.doubleValue()); + "Arm/Shoulder/Max Acceleration", + Shoulder.TELEOP_MAX_ACCELERATION.doubleValue()); wristMaxVelocity = new SmartNumber( - "Arm/Wrist/Max Velocity", - Wrist.TELEOP_MAX_VELOCITY.doubleValue()); + "Arm/Wrist/Max Velocity", + Wrist.TELEOP_MAX_VELOCITY.doubleValue()); wristMaxAcceleration = new SmartNumber( - "Arm/Wrist/Max Acceleration", - Wrist.TELEOP_MAX_ACCELERATION.doubleValue()); - - shoulderController = new MotorFeedforward(Shoulder.Feedforward.kS, Shoulder.Feedforward.kV, Shoulder.Feedforward.kA).position() - .add(new ArmEncoderFeedforward(new GamePiecekG())) - .add(new ArmDriveFeedforward(new GamePiecekG(), SwerveDrive.getInstance()::getForwardAccelerationGs)) - .add(new PIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD)) - .setSetpointFilter( - new MotionProfile( - shoulderMaxVelocity.filtered(Math::toRadians).number(), - shoulderMaxAcceleration.filtered(Math::toRadians).number())) - .setOutputFilter(x -> { - if (isShoulderLimp()) return 0; - return shoulderVoltageOverride.orElse(x); - }) - ; + "Arm/Wrist/Max Acceleration", + Wrist.TELEOP_MAX_ACCELERATION.doubleValue()); + + shoulderController = new MotorFeedforward(Shoulder.Feedforward.kS, Shoulder.Feedforward.kV, + Shoulder.Feedforward.kA).position() + .add(new ArmEncoderFeedforward(new GamePiecekG())) + .add(new ArmDriveFeedforward(new GamePiecekG(), SwerveDrive.getInstance()::getForwardAccelerationGs)) + .add(new PIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD)) + .setSetpointFilter( + new MotionProfile( + shoulderMaxVelocity.filtered(Math::toRadians).number(), + shoulderMaxAcceleration.filtered(Math::toRadians).number())) + .setOutputFilter(x -> { + if (isShoulderLimp()) + return 0; + return shoulderVoltageOverride.orElse(x); + }); wristController = new MotorFeedforward(Wrist.Feedforward.kS, Wrist.Feedforward.kV, Wrist.Feedforward.kA).angle() - .add(new ArmEncoderAngleFeedforward(Wrist.Feedforward.kG)) - .add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD) - .setOutputFilter(x -> wristEnabled.get() ? x : 0)) - .setSetpointFilter( - new AMotionProfile( - wristMaxVelocity.filtered(Math::toRadians).number(), - wristMaxAcceleration.filtered(Math::toRadians).number())) - .setOutputFilter(x -> { - if (isWristLimp()) return 0; - return wristVoltageOverride.orElse(x); - }); + .add(new ArmEncoderAngleFeedforward(Wrist.Feedforward.kG)) + .add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD) + .setOutputFilter(x -> wristEnabled.get() ? x : 0)) + .setSetpointFilter( + new AMotionProfile( + wristMaxVelocity.filtered(Math::toRadians).number(), + wristMaxAcceleration.filtered(Math::toRadians).number())) + .setOutputFilter(x -> { + if (isWristLimp()) + return 0; + return wristVoltageOverride.orElse(x); + }); wristLimp = new SmartBoolean("Arm/Wrist/Is Limp?", false); shoulderLimp = new SmartBoolean("Arm/Shoulder/Is Limp?", false); @@ -224,7 +230,8 @@ private final boolean isShoulderLimp() { } private final boolean isWristFeedbackEnabled() { - return Math.abs(getShoulderVelocityRadiansPerSecond()) < Units.degreesToRadians(shoulderVelocityFeedbackCutoff.doubleValue()); + return Math.abs(getShoulderVelocityRadiansPerSecond()) < Units + .degreesToRadians(shoulderVelocityFeedbackCutoff.doubleValue()); } // Set kinematic constraints @@ -239,7 +246,6 @@ public final void setWristConstraints(Number velocity, Number acceleration) { wristMaxAcceleration.set(acceleration); } - // Read target State public final Rotation2d getShoulderTargetAngle() { return Rotation2d.fromDegrees(shoulderTargetDegrees.get()); @@ -305,6 +311,7 @@ public final boolean isAtTargetState(double shoulderEpsilonDegrees, double wrist // Read angle measurements public abstract Rotation2d getShoulderAngle(); + protected abstract Rotation2d getRelativeWristAngle(); public final Rotation2d getWristAngle() { @@ -316,6 +323,7 @@ public final ArmState getState() { } public abstract double getShoulderVelocityRadiansPerSecond(); + public abstract double getWristVelocityRadiansPerSecond(); // Set a voltage override @@ -329,10 +337,12 @@ public void setWristVoltage(double voltage) { // Feed a voltage to the hardware layer protected abstract void setShoulderVoltageImpl(double voltage); + protected abstract void setWristVoltageImpl(double voltage); // set coast / brake mode - public void setCoast(boolean wristCoast, boolean shoulderCoast) {} + public void setCoast(boolean wristCoast, boolean shoulderCoast) { + } // set if the ligaments are "limp" (zero voltage) public final void setLimp(boolean wristLimp, boolean shoulderLimp) { @@ -343,6 +353,7 @@ public final void setLimp(boolean wristLimp, boolean shoulderLimp) { public final void enableLimp() { setLimp(true, true); } + public final void disableLimp() { setLimp(false, false); } @@ -366,38 +377,42 @@ public final void periodic() { setShoulderTargetAngle(Rotation2d.fromDegrees(180 - Shoulder.MAX_SHOULDER_ANGLE.get())); } - // Run control loops on validated target angles shoulderController.update( - getWrappedShoulderAngle(getShoulderTargetAngle()), - getWrappedShoulderAngle(getShoulderAngle())); + getWrappedShoulderAngle(getShoulderTargetAngle()), + getWrappedShoulderAngle(getShoulderAngle())); SmartDashboard.putNumber("Arm/Shoulder/Wrapped Angle", getWrappedShoulderAngle(getShoulderAngle())); - SmartDashboard.putNumber("Arm/Shoulder/Wrapped Target Angle", getWrappedShoulderAngle(getShoulderTargetAngle())); + SmartDashboard.putNumber("Arm/Shoulder/Wrapped Target Angle", + getWrappedShoulderAngle(getShoulderTargetAngle())); wristController.update( - Angle.fromRotation2d(getWristTargetAngle()), - Angle.fromRotation2d(getWristAngle())); + Angle.fromRotation2d(getWristTargetAngle()), + Angle.fromRotation2d(getWristAngle())); setWristVoltageImpl(wristController.getOutput()); setShoulderVoltageImpl(shoulderController.getOutput()); - armVisualizer.setTargetAngles(Units.radiansToDegrees(shoulderController.getSetpoint()), wristController.getSetpoint().toDegrees()); + armVisualizer.setTargetAngles(Units.radiansToDegrees(shoulderController.getSetpoint()), + wristController.getSetpoint().toDegrees()); armVisualizer.setMeasuredAngles(getShoulderAngle().getDegrees(), getWristAngle().getDegrees()); armVisualizer.setFieldArm(Odometry.getInstance().getPose(), getState()); SmartDashboard.putNumber("Arm/Shoulder/Angle (deg)", getShoulderAngle().getDegrees()); - SmartDashboard.putNumber("Arm/Shoulder/Setpoint (deg)", Units.radiansToDegrees(shoulderController.getSetpoint())); + SmartDashboard.putNumber("Arm/Shoulder/Setpoint (deg)", + Units.radiansToDegrees(shoulderController.getSetpoint())); SmartDashboard.putNumber("Arm/Shoulder/Error (deg)", Units.radiansToDegrees(shoulderController.getError())); SmartDashboard.putNumber("Arm/Shoulder/Output (V)", shoulderController.getOutput()); - SmartDashboard.putNumber("Arm/Shoulder/Velocity (deg per s)", Units.radiansToDegrees(getShoulderVelocityRadiansPerSecond())); + SmartDashboard.putNumber("Arm/Shoulder/Velocity (deg per s)", + Units.radiansToDegrees(getShoulderVelocityRadiansPerSecond())); SmartDashboard.putNumber("Arm/Wrist/Angle (deg)", getWristAngle().getDegrees()); SmartDashboard.putNumber("Arm/Wrist/Relative Angle (deg)", getRelativeWristAngle().getDegrees()); SmartDashboard.putNumber("Arm/Wrist/Setpoint (deg)", wristController.getSetpoint().toDegrees()); SmartDashboard.putNumber("Arm/Wrist/Error (deg)", wristController.getError().toDegrees()); SmartDashboard.putNumber("Arm/Wrist/Output (V)", wristController.getOutput()); - SmartDashboard.putNumber("Arm/Wrist/Velocity (deg per s)", Units.radiansToDegrees(getWristVelocityRadiansPerSecond())); + SmartDashboard.putNumber("Arm/Wrist/Velocity (deg per s)", + Units.radiansToDegrees(getWristVelocityRadiansPerSecond())); SmartDashboard.putBoolean("Arm/Wrist/Feedback Enabled Raw", isWristFeedbackEnabled()); SmartDashboard.putBoolean("Arm/Wrist/Feedback Enabled", wristEnabled.get()); SmartDashboard.putNumber("Arm/Shoulder/kG", new GamePiecekG().doubleValue()); @@ -407,5 +422,6 @@ public final void periodic() { periodicallyCalled(); } - public void periodicallyCalled() {} + public void periodicallyCalled() { + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Beak.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Beak.java new file mode 100644 index 00000000..ca7fa414 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Beak.java @@ -0,0 +1,40 @@ +package com.stuypulse.robot.subsystems.arm; + +import com.stuypulse.robot.constants.Settings.Arm.Shoulder; +import com.stuypulse.robot.constants.Settings.Arm.Wrist; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class Beak extends SubsystemBase { + private static Beak instance; + static { + instance = new BeakImpl(); + } + + public static Beak getInstance() { + return instance; + } + + // controllers for each joint + private final Controller shoulderController; + private final AngleController wristController; + + protected Beak() { + + shoulderController = new MotorFeedforward(Shoulder.Feedforward.kS, Shoulder.Feedforward.kV, + Shoulder.Feedforward.kA) + .position() + .add(new PIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD)); + + // commented out cuz errors + // wristController = new MotorFeedforward(Wrist.Feedforward.kS, Wrist.Feedforward.kV, + // Wrist.feedforward.kA).position() + // .add(new PIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD)); + + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/BeakImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/BeakImpl.java new file mode 100644 index 00000000..96962922 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/BeakImpl.java @@ -0,0 +1,5 @@ +package com.stuypulse.robot.subsystems.arm; + +public class BeakImpl extends Beak { + +} From aeba684305aad8f85732d54a196142b3474a6fe0 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Wed, 17 May 2023 17:21:46 -0400 Subject: [PATCH 02/15] Remove unneccessary code in Arm Co-authored-by: Richie Xue Co-authored-by: bananaeater321 Co-authored-by: reyamiller Co-authored-by: Edgar694 --- .../stuypulse/robot/subsystems/arm/Arm.java | 95 ++----------------- .../robot/subsystems/arm/ArmImpl.java | 40 ++++---- 2 files changed, 24 insertions(+), 111 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 6dcdb951..ae3e7f66 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -11,11 +11,8 @@ import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.filters.MotionProfile; import com.stuypulse.robot.constants.Settings; @@ -38,8 +35,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.Optional; - /** * Double jointed arm controlled by two motion profiled PID controllers. * @@ -78,16 +73,6 @@ public static Arm getInstance() { // Mechanism2d visualizer private final ArmVisualizer armVisualizer; - // Limp mode (forces a joint to receive zero voltage) - private SmartBoolean wristLimp; - private SmartBoolean shoulderLimp; - - // Voltage overrides (used when present) - private Optional wristVoltageOverride; - private Optional shoulderVoltageOverride; - - private BStream wristEnabled; - private SmartNumber shoulderVelocityFeedbackDebounce; private SmartNumber shoulderVelocityFeedbackCutoff; @@ -152,9 +137,6 @@ protected Arm() { "Arm/Wrist/Shoulder Velocity Feedback Cutoff (deg per s)", Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); - wristEnabled = BStream.create(this::isWristFeedbackEnabled) - .filtered(new BDebounce.Both(shoulderVelocityFeedbackDebounce)); - shoulderMaxVelocity = new SmartNumber( "Arm/Shoulder/Max Velocity", Shoulder.TELEOP_MAX_VELOCITY.doubleValue()); @@ -177,32 +159,15 @@ protected Arm() { .setSetpointFilter( new MotionProfile( shoulderMaxVelocity.filtered(Math::toRadians).number(), - shoulderMaxAcceleration.filtered(Math::toRadians).number())) - .setOutputFilter(x -> { - if (isShoulderLimp()) - return 0; - return shoulderVoltageOverride.orElse(x); - }); + shoulderMaxAcceleration.filtered(Math::toRadians).number())); wristController = new MotorFeedforward(Wrist.Feedforward.kS, Wrist.Feedforward.kV, Wrist.Feedforward.kA).angle() .add(new ArmEncoderAngleFeedforward(Wrist.Feedforward.kG)) .add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD) - .setOutputFilter(x -> wristEnabled.get() ? x : 0)) - .setSetpointFilter( - new AMotionProfile( - wristMaxVelocity.filtered(Math::toRadians).number(), - wristMaxAcceleration.filtered(Math::toRadians).number())) - .setOutputFilter(x -> { - if (isWristLimp()) - return 0; - return wristVoltageOverride.orElse(x); - }); - - wristLimp = new SmartBoolean("Arm/Wrist/Is Limp?", false); - shoulderLimp = new SmartBoolean("Arm/Shoulder/Is Limp?", false); - - wristVoltageOverride = Optional.empty(); - shoulderVoltageOverride = Optional.empty(); + .setSetpointFilter( + new AMotionProfile( + wristMaxVelocity.filtered(Math::toRadians).number(), + wristMaxAcceleration.filtered(Math::toRadians).number()))); armVisualizer = new ArmVisualizer(Odometry.getInstance().getField().getObject("Field Arm")); @@ -220,15 +185,6 @@ public void disableGamePieceGravityCompensation() { } // Arm Control Overrides - - private final boolean isWristLimp() { - return wristLimp.get(); - } - - private final boolean isShoulderLimp() { - return shoulderLimp.get(); - } - private final boolean isWristFeedbackEnabled() { return Math.abs(getShoulderVelocityRadiansPerSecond()) < Units .degreesToRadians(shoulderVelocityFeedbackCutoff.doubleValue()); @@ -273,12 +229,10 @@ private static double getWrappedShoulderAngle(Rotation2d angle) { } public final void setShoulderTargetAngle(Rotation2d angle) { - shoulderVoltageOverride = Optional.empty(); shoulderTargetDegrees.set(angle.getDegrees()); } public final void setWristTargetAngle(Rotation2d angle) { - wristVoltageOverride = Optional.empty(); wristTargetDegrees.set(angle.getDegrees()); } @@ -326,48 +280,19 @@ public final ArmState getState() { public abstract double getWristVelocityRadiansPerSecond(); - // Set a voltage override - public void setShoulderVoltage(double voltage) { - shoulderVoltageOverride = Optional.of(voltage); - } - - public void setWristVoltage(double voltage) { - wristVoltageOverride = Optional.of(voltage); - } - - // Feed a voltage to the hardware layer - protected abstract void setShoulderVoltageImpl(double voltage); - - protected abstract void setWristVoltageImpl(double voltage); - // set coast / brake mode - public void setCoast(boolean wristCoast, boolean shoulderCoast) { - } - - // set if the ligaments are "limp" (zero voltage) - public final void setLimp(boolean wristLimp, boolean shoulderLimp) { - this.wristLimp.set(wristLimp); - this.shoulderLimp.set(shoulderLimp); - } - - public final void enableLimp() { - setLimp(true, true); - } - - public final void disableLimp() { - setLimp(false, false); - } + public abstract void setCoast(boolean wristCoast, boolean shoulderCoast); // Arm Visualizer public final ArmVisualizer getVisualizer() { return armVisualizer; - } + @Override public final void periodic() { // Validate shoulder and wrist target states Rotation2d shoulderTarget = getShoulderTargetAngle(); - Rotation2d wristTarget = getWristTargetAngle(); + // Rotation2d wristTarget = getWristTargetAngle(); double normalizedDeg = shoulderTarget.minus(Rotation2d.fromDegrees(-90)).getDegrees(); @@ -390,9 +315,6 @@ public final void periodic() { Angle.fromRotation2d(getWristTargetAngle()), Angle.fromRotation2d(getWristAngle())); - setWristVoltageImpl(wristController.getOutput()); - setShoulderVoltageImpl(shoulderController.getOutput()); - armVisualizer.setTargetAngles(Units.radiansToDegrees(shoulderController.getSetpoint()), wristController.getSetpoint().toDegrees()); armVisualizer.setMeasuredAngles(getShoulderAngle().getDegrees(), getWristAngle().getDegrees()); @@ -414,7 +336,6 @@ public final void periodic() { SmartDashboard.putNumber("Arm/Wrist/Velocity (deg per s)", Units.radiansToDegrees(getWristVelocityRadiansPerSecond())); SmartDashboard.putBoolean("Arm/Wrist/Feedback Enabled Raw", isWristFeedbackEnabled()); - SmartDashboard.putBoolean("Arm/Wrist/Feedback Enabled", wristEnabled.get()); SmartDashboard.putNumber("Arm/Shoulder/kG", new GamePiecekG().doubleValue()); SmartDashboard.putBoolean("Arm/Shoulder/Game Piece Compensation", pieceGravityCompensation); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index da23e36c..416d97b7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -90,17 +90,6 @@ public Rotation2d getRelativeWristAngle() { return Rotation2d.fromRotations(wristEncoder.getPosition()).minus(Wrist.ZERO_ANGLE); } - @Override - protected void setShoulderVoltageImpl(double voltage) { - shoulderLeft.setVoltage(voltage); - shoulderRight.setVoltage(voltage); - } - - @Override - protected void setWristVoltageImpl(double voltage) { - wrist.setVoltage(voltage); - } - @Override public void setCoast(boolean wristCoast, boolean shoulderCoast) { shoulderLeft.setIdleMode(shoulderCoast ? CANSparkMax.IdleMode.kCoast : CANSparkMax.IdleMode.kBrake); @@ -119,20 +108,23 @@ public double getWristVelocityRadiansPerSecond() { } // private boolean isShoulderStalling() { - // double appliedShoulderVoltage = - // Math.max( - // shoulderRight.getAppliedOutput() * shoulderRight.getBusVoltage(), - // shoulderLeft.getAppliedOutput() * shoulderLeft.getBusVoltage(), - // ); - - // return shoulderEncoder.getVelocity() < Shoulder.STALLING_VELOCITY.doubleValue() && shoulderVolts > Shoulder.STALLING_VOLTAGE.doubleValue() || - // wrist.getOutputCurrent() > Shoulder.STALLING_CURRENT.doubleValue(); + // double appliedShoulderVoltage = + // Math.max( + // shoulderRight.getAppliedOutput() * shoulderRight.getBusVoltage(), + // shoulderLeft.getAppliedOutput() * shoulderLeft.getBusVoltage(), + // ); + + // return shoulderEncoder.getVelocity() < + // Shoulder.STALLING_VELOCITY.doubleValue() && shoulderVolts > + // Shoulder.STALLING_VOLTAGE.doubleValue() || + // wrist.getOutputCurrent() > Shoulder.STALLING_CURRENT.doubleValue(); // } // private boolean isWristStalling() { - // return wristEncoder.getVelocity() < Wrist.STALLING_VELOCITY.doubleValue() && wristVolts > Wrist.STALLING_VOLTAGE.doubleValue() || - // shoulderLeft.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue() || - // shoulderRight.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue(); + // return wristEncoder.getVelocity() < Wrist.STALLING_VELOCITY.doubleValue() && + // wristVolts > Wrist.STALLING_VOLTAGE.doubleValue() || + // shoulderLeft.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue() || + // shoulderRight.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue(); // } @Override @@ -146,11 +138,11 @@ public void periodicallyCalled() { SmartDashboard.putNumber("Arm/Wrist/Current (amps)", wrist.getOutputCurrent()); // if (wristIsStalling()) { - // setWristVoltageImpl(WRIST); + // setWristVoltageImpl(WRIST); // } // if (armIsStalling()) { - // shoulderVolts = 0; + // shoulderVolts = 0; // } // runShoulder(shoulderVolts); From 3a3d048588d8ca71253848fc00c4672c49682ccd Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Thu, 18 May 2023 16:02:27 -0400 Subject: [PATCH 03/15] Remove beak --- .../stuypulse/robot/subsystems/arm/Arm.java | 1 + .../stuypulse/robot/subsystems/arm/Beak.java | 40 ------------------- .../robot/subsystems/arm/BeakImpl.java | 5 --- 3 files changed, 1 insertion(+), 45 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/arm/Beak.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/arm/BeakImpl.java diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index ae3e7f66..322c746d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -286,6 +286,7 @@ public final ArmState getState() { // Arm Visualizer public final ArmVisualizer getVisualizer() { return armVisualizer; + } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Beak.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Beak.java deleted file mode 100644 index ca7fa414..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Beak.java +++ /dev/null @@ -1,40 +0,0 @@ -package com.stuypulse.robot.subsystems.arm; - -import com.stuypulse.robot.constants.Settings.Arm.Shoulder; -import com.stuypulse.robot.constants.Settings.Arm.Wrist; -import com.stuypulse.stuylib.control.Controller; -import com.stuypulse.stuylib.control.angle.AngleController; -import com.stuypulse.stuylib.control.feedback.PIDController; -import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public abstract class Beak extends SubsystemBase { - private static Beak instance; - static { - instance = new BeakImpl(); - } - - public static Beak getInstance() { - return instance; - } - - // controllers for each joint - private final Controller shoulderController; - private final AngleController wristController; - - protected Beak() { - - shoulderController = new MotorFeedforward(Shoulder.Feedforward.kS, Shoulder.Feedforward.kV, - Shoulder.Feedforward.kA) - .position() - .add(new PIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD)); - - // commented out cuz errors - // wristController = new MotorFeedforward(Wrist.Feedforward.kS, Wrist.Feedforward.kV, - // Wrist.feedforward.kA).position() - // .add(new PIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD)); - - } - -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/BeakImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/BeakImpl.java deleted file mode 100644 index 96962922..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/BeakImpl.java +++ /dev/null @@ -1,5 +0,0 @@ -package com.stuypulse.robot.subsystems.arm; - -public class BeakImpl extends Beak { - -} From 820fc4d8f92a815a0984ef515b60e735bffc178d Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Thu, 18 May 2023 16:13:32 -0400 Subject: [PATCH 04/15] Remove operator rightBumper Remove limp in Robot.java --- src/main/java/com/stuypulse/robot/Robot.java | 2 -- src/main/java/com/stuypulse/robot/RobotContainer.java | 4 ---- 2 files changed, 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 61ca69d0..203fa7e6 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -82,7 +82,6 @@ public void autonomousInit() { robot.arm.setCoast(false, false); - robot.arm.setLimp(true, true); robot.arm.setTargetState(robot.arm.getState()); // TODO: ArmHold in auton? robot.arm.setShoulderConstraints(Shoulder.AUTON_MAX_VELOCITY, Shoulder.AUTON_MAX_ACCELERATION); robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); @@ -113,7 +112,6 @@ public void teleopInit() { SmartDashboard.putString("Match State", state.name()); robot.arm.setCoast(false, false); - robot.arm.setLimp(false, false); robot.arm.setShoulderConstraints(Shoulder.TELEOP_MAX_VELOCITY, Shoulder.TELEOP_MAX_ACCELERATION); robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); robot.arm.setShoulderVelocityFeedbackDebounce(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue()); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index df8836c7..69b9ad12 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -213,10 +213,6 @@ private void configureOperatorBindings() { operator.getBottomButton() .onTrue(new ManagerSetGamePiece(GamePiece.CONE_TIP_OUT)); - operator.getRightBumper() - .onTrue(arm.runOnce(arm::enableLimp)) - .onFalse(arm.runOnce(arm::disableLimp)); - // arm to neutral operator.getDPadRight().onTrue(new IntakeAcquire()) .onFalse(new IntakeStop()); From 5152e82d9ed642be790a7fd99abfb781596e6470 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Thu, 18 May 2023 16:20:09 -0400 Subject: [PATCH 05/15] bring back voltage overrides --- .../stuypulse/robot/subsystems/arm/Arm.java | 28 +++++++++++++++++++ .../robot/subsystems/arm/ArmImpl.java | 11 ++++++++ 2 files changed, 39 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 322c746d..329f4838 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -15,6 +15,8 @@ import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; import com.stuypulse.stuylib.streams.filters.MotionProfile; +import java.util.Optional; + import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Arm.Shoulder; import com.stuypulse.robot.constants.Settings.Arm.Wrist; @@ -66,6 +68,10 @@ public static Arm getInstance() { private final SmartNumber shoulderTargetDegrees; private final SmartNumber wristTargetDegrees; + // Voltage overrides (used when present) + private Optional wristVoltageOverride; + private Optional shoulderVoltageOverride; + // controllers for each joint private final Controller shoulderController; private final AngleController wristController; @@ -169,6 +175,9 @@ protected Arm() { wristMaxVelocity.filtered(Math::toRadians).number(), wristMaxAcceleration.filtered(Math::toRadians).number()))); + wristVoltageOverride = Optional.empty(); + shoulderVoltageOverride = Optional.empty(); + armVisualizer = new ArmVisualizer(Odometry.getInstance().getField().getObject("Field Arm")); pieceGravityCompensation = false; @@ -229,10 +238,12 @@ private static double getWrappedShoulderAngle(Rotation2d angle) { } public final void setShoulderTargetAngle(Rotation2d angle) { + shoulderVoltageOverride = Optional.empty(); shoulderTargetDegrees.set(angle.getDegrees()); } public final void setWristTargetAngle(Rotation2d angle) { + wristVoltageOverride = Optional.empty(); wristTargetDegrees.set(angle.getDegrees()); } @@ -280,6 +291,20 @@ public final ArmState getState() { public abstract double getWristVelocityRadiansPerSecond(); + // Set a voltage override + public void setShoulderVoltage(double voltage) { + shoulderVoltageOverride = Optional.of(voltage); + } + + public void setWristVoltage(double voltage) { + wristVoltageOverride = Optional.of(voltage); + } + + // Feed a voltage to the hardware layer + protected abstract void setShoulderVoltageImpl(double voltage); + + protected abstract void setWristVoltageImpl(double voltage); + // set coast / brake mode public abstract void setCoast(boolean wristCoast, boolean shoulderCoast); @@ -316,6 +341,9 @@ public final void periodic() { Angle.fromRotation2d(getWristTargetAngle()), Angle.fromRotation2d(getWristAngle())); + setWristVoltageImpl(wristController.getOutput()); + setShoulderVoltageImpl(shoulderController.getOutput()); + armVisualizer.setTargetAngles(Units.radiansToDegrees(shoulderController.getSetpoint()), wristController.getSetpoint().toDegrees()); armVisualizer.setMeasuredAngles(getShoulderAngle().getDegrees(), getWristAngle().getDegrees()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 416d97b7..8f5082a6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -90,6 +90,17 @@ public Rotation2d getRelativeWristAngle() { return Rotation2d.fromRotations(wristEncoder.getPosition()).minus(Wrist.ZERO_ANGLE); } + @Override + protected void setShoulderVoltageImpl(double voltage) { + shoulderLeft.setVoltage(voltage); + shoulderRight.setVoltage(voltage); + } + + @Override + protected void setWristVoltageImpl(double voltage) { + wrist.setVoltage(voltage); + } + @Override public void setCoast(boolean wristCoast, boolean shoulderCoast) { shoulderLeft.setIdleMode(shoulderCoast ? CANSparkMax.IdleMode.kCoast : CANSparkMax.IdleMode.kBrake); From 2c08685fe4586392389e4c73f83743dde3c27a12 Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Thu, 18 May 2023 16:21:43 -0400 Subject: [PATCH 06/15] - Remove limp in arm routine --- .../com/stuypulse/robot/commands/arm/routines/ArmRoutine.java | 2 -- src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/arm/routines/ArmRoutine.java b/src/main/java/com/stuypulse/robot/commands/arm/routines/ArmRoutine.java index 07a092c0..0989b7e5 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/routines/ArmRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/routines/ArmRoutine.java @@ -86,8 +86,6 @@ public void execute() { var targetState = trajectory.getStates().get(currentIndex); arm.setTargetState(targetState); - arm.setLimp(targetState.isWristLimp(), false); - double currentShoulderTolerance = (targetState.getShoulderTolerance().orElse(shoulderTolerance)).doubleValue(); double currentWristTolerance = (targetState.getWristTolerance().orElse(wristTolerance)).doubleValue(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 329f4838..fca5e5a0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -306,7 +306,7 @@ public void setWristVoltage(double voltage) { protected abstract void setWristVoltageImpl(double voltage); // set coast / brake mode - public abstract void setCoast(boolean wristCoast, boolean shoulderCoast); + public void setCoast(boolean wristCoast, boolean shoulderCoast) {} // Arm Visualizer public final ArmVisualizer getVisualizer() { From b622f79549e2f89a5ca90c0f53e528de50f59c06 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Thu, 18 May 2023 17:09:01 -0400 Subject: [PATCH 07/15] add back limp --- src/main/java/com/stuypulse/robot/Robot.java | 2 ++ .../stuypulse/robot/subsystems/arm/Arm.java | 32 +++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 203fa7e6..cf646f23 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -82,6 +82,7 @@ public void autonomousInit() { robot.arm.setCoast(false, false); + robot.arm.enableLimp(); robot.arm.setTargetState(robot.arm.getState()); // TODO: ArmHold in auton? robot.arm.setShoulderConstraints(Shoulder.AUTON_MAX_VELOCITY, Shoulder.AUTON_MAX_ACCELERATION); robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); @@ -112,6 +113,7 @@ public void teleopInit() { SmartDashboard.putString("Match State", state.name()); robot.arm.setCoast(false, false); + robot.arm.disableLimp(); robot.arm.setShoulderConstraints(Shoulder.TELEOP_MAX_VELOCITY, Shoulder.TELEOP_MAX_ACCELERATION); robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); robot.arm.setShoulderVelocityFeedbackDebounce(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index fca5e5a0..08b1b017 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -11,6 +11,7 @@ import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; import com.stuypulse.stuylib.streams.filters.MotionProfile; @@ -68,6 +69,10 @@ public static Arm getInstance() { private final SmartNumber shoulderTargetDegrees; private final SmartNumber wristTargetDegrees; + // Limp mode (forces a joint to receive zero voltage) + private SmartBoolean wristLimp; + private SmartBoolean shoulderLimp; + // Voltage overrides (used when present) private Optional wristVoltageOverride; private Optional shoulderVoltageOverride; @@ -178,6 +183,9 @@ protected Arm() { wristVoltageOverride = Optional.empty(); shoulderVoltageOverride = Optional.empty(); + wristLimp = new SmartBoolean("Arm/Wrist/Is Limp?", false); + shoulderLimp = new SmartBoolean("Arm/Shoulder/Is Limp?", false); + armVisualizer = new ArmVisualizer(Odometry.getInstance().getField().getObject("Field Arm")); pieceGravityCompensation = false; @@ -199,6 +207,16 @@ private final boolean isWristFeedbackEnabled() { .degreesToRadians(shoulderVelocityFeedbackCutoff.doubleValue()); } + + private final boolean isWristLimp() { + return wristLimp.get(); + } + + private final boolean isShoulderLimp() { + return shoulderLimp.get(); + } + + // Set kinematic constraints public final void setShoulderConstraints(Number velocity, Number acceleration) { @@ -308,6 +326,20 @@ public void setWristVoltage(double voltage) { // set coast / brake mode public void setCoast(boolean wristCoast, boolean shoulderCoast) {} + // set if the ligaments are "limp" (zero voltage) + public final void setLimp(boolean wristLimp, boolean shoulderLimp) { + this.wristLimp.set(wristLimp); + this.shoulderLimp.set(shoulderLimp); + } + + public final void enableLimp() { + setLimp(true, true); + } + + public final void disableLimp() { + setLimp(false, false); + } + // Arm Visualizer public final ArmVisualizer getVisualizer() { return armVisualizer; From 02015bc82c756dd7db0790255f5712c902931c4c Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Thu, 18 May 2023 17:41:06 -0400 Subject: [PATCH 08/15] fix limp --- src/main/java/com/stuypulse/robot/Robot.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 203fa7e6..d2332c64 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -82,6 +82,8 @@ public void autonomousInit() { robot.arm.setCoast(false, false); + robot.arm.setWristVoltage(0); + robot.arm.setShoulderVoltage(0); robot.arm.setTargetState(robot.arm.getState()); // TODO: ArmHold in auton? robot.arm.setShoulderConstraints(Shoulder.AUTON_MAX_VELOCITY, Shoulder.AUTON_MAX_ACCELERATION); robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); From c2e2127b947170e708ca7034b74f5e6df1235763 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Thu, 18 May 2023 20:14:44 -0400 Subject: [PATCH 09/15] replace limp --- src/main/java/com/stuypulse/robot/Robot.java | 5 --- .../stuypulse/robot/subsystems/arm/Arm.java | 33 ------------------- 2 files changed, 38 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index cda7c976..d2332c64 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -82,12 +82,8 @@ public void autonomousInit() { robot.arm.setCoast(false, false); -<<<<<<< HEAD robot.arm.setWristVoltage(0); robot.arm.setShoulderVoltage(0); -======= - robot.arm.enableLimp(); ->>>>>>> b622f79549e2f89a5ca90c0f53e528de50f59c06 robot.arm.setTargetState(robot.arm.getState()); // TODO: ArmHold in auton? robot.arm.setShoulderConstraints(Shoulder.AUTON_MAX_VELOCITY, Shoulder.AUTON_MAX_ACCELERATION); robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); @@ -118,7 +114,6 @@ public void teleopInit() { SmartDashboard.putString("Match State", state.name()); robot.arm.setCoast(false, false); - robot.arm.disableLimp(); robot.arm.setShoulderConstraints(Shoulder.TELEOP_MAX_VELOCITY, Shoulder.TELEOP_MAX_ACCELERATION); robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); robot.arm.setShoulderVelocityFeedbackDebounce(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 08b1b017..caa1d0dc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -44,7 +44,6 @@ * Available control "modes": * - setpoint control (PID+FF controllers are used) (shoulder is not allowed * above maximum shoulder angle) - * - limp mode (controller output is overriden to be zero) * - voltage override ("force" feeds a voltage to the motor) */ public abstract class Arm extends SubsystemBase { @@ -69,10 +68,6 @@ public static Arm getInstance() { private final SmartNumber shoulderTargetDegrees; private final SmartNumber wristTargetDegrees; - // Limp mode (forces a joint to receive zero voltage) - private SmartBoolean wristLimp; - private SmartBoolean shoulderLimp; - // Voltage overrides (used when present) private Optional wristVoltageOverride; private Optional shoulderVoltageOverride; @@ -183,9 +178,6 @@ protected Arm() { wristVoltageOverride = Optional.empty(); shoulderVoltageOverride = Optional.empty(); - wristLimp = new SmartBoolean("Arm/Wrist/Is Limp?", false); - shoulderLimp = new SmartBoolean("Arm/Shoulder/Is Limp?", false); - armVisualizer = new ArmVisualizer(Odometry.getInstance().getField().getObject("Field Arm")); pieceGravityCompensation = false; @@ -207,16 +199,6 @@ private final boolean isWristFeedbackEnabled() { .degreesToRadians(shoulderVelocityFeedbackCutoff.doubleValue()); } - - private final boolean isWristLimp() { - return wristLimp.get(); - } - - private final boolean isShoulderLimp() { - return shoulderLimp.get(); - } - - // Set kinematic constraints public final void setShoulderConstraints(Number velocity, Number acceleration) { @@ -326,25 +308,10 @@ public void setWristVoltage(double voltage) { // set coast / brake mode public void setCoast(boolean wristCoast, boolean shoulderCoast) {} - // set if the ligaments are "limp" (zero voltage) - public final void setLimp(boolean wristLimp, boolean shoulderLimp) { - this.wristLimp.set(wristLimp); - this.shoulderLimp.set(shoulderLimp); - } - - public final void enableLimp() { - setLimp(true, true); - } - - public final void disableLimp() { - setLimp(false, false); - } - // Arm Visualizer public final ArmVisualizer getVisualizer() { return armVisualizer; } - @Override public final void periodic() { From 815a4c6e372ef1682f4238aaa69c9c3ca460985a Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 18 May 2023 22:48:14 -0400 Subject: [PATCH 10/15] Add back limp methods, remove formatting changes --- src/main/java/com/stuypulse/robot/Robot.java | 8 +- .../com/stuypulse/robot/RobotContainer.java | 4 + .../commands/arm/routines/ArmRoutine.java | 2 + .../stuypulse/robot/constants/Settings.java | 6 - .../stuypulse/robot/subsystems/arm/Arm.java | 124 +++++++++--------- .../robot/subsystems/arm/ArmImpl.java | 29 ++-- 6 files changed, 82 insertions(+), 91 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index d2332c64..be69684c 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -82,12 +82,9 @@ public void autonomousInit() { robot.arm.setCoast(false, false); - robot.arm.setWristVoltage(0); - robot.arm.setShoulderVoltage(0); + robot.arm.setLimp(true, true); robot.arm.setTargetState(robot.arm.getState()); // TODO: ArmHold in auton? robot.arm.setShoulderConstraints(Shoulder.AUTON_MAX_VELOCITY, Shoulder.AUTON_MAX_ACCELERATION); - robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); - robot.arm.setShoulderVelocityFeedbackDebounce(Wrist.AUTON_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue()); RobotContainer.setCachedAlliance(DriverStation.getAlliance()); @@ -114,9 +111,8 @@ public void teleopInit() { SmartDashboard.putString("Match State", state.name()); robot.arm.setCoast(false, false); + robot.arm.setLimp(false, false); robot.arm.setShoulderConstraints(Shoulder.TELEOP_MAX_VELOCITY, Shoulder.TELEOP_MAX_ACCELERATION); - robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); - robot.arm.setShoulderVelocityFeedbackDebounce(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue()); new TeleopInit().schedule(); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 69b9ad12..df8836c7 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -213,6 +213,10 @@ private void configureOperatorBindings() { operator.getBottomButton() .onTrue(new ManagerSetGamePiece(GamePiece.CONE_TIP_OUT)); + operator.getRightBumper() + .onTrue(arm.runOnce(arm::enableLimp)) + .onFalse(arm.runOnce(arm::disableLimp)); + // arm to neutral operator.getDPadRight().onTrue(new IntakeAcquire()) .onFalse(new IntakeStop()); diff --git a/src/main/java/com/stuypulse/robot/commands/arm/routines/ArmRoutine.java b/src/main/java/com/stuypulse/robot/commands/arm/routines/ArmRoutine.java index 0989b7e5..07a092c0 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/routines/ArmRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/routines/ArmRoutine.java @@ -86,6 +86,8 @@ public void execute() { var targetState = trajectory.getStates().get(currentIndex); arm.setTargetState(targetState); + arm.setLimp(targetState.isWristLimp(), false); + double currentShoulderTolerance = (targetState.getShoulderTolerance().orElse(shoulderTolerance)).doubleValue(); double currentWristTolerance = (targetState.getWristTolerance().orElse(wristTolerance)).doubleValue(); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 22deba4b..29f97311 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -242,12 +242,6 @@ public interface Wrist { SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Wrist/Teleop Max Velocity (deg)", 480.0); SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Wrist/Teleop Max Acceleration (deg)", 480.0); - SmartNumber AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Auton Shoulder Velocity Feedback Cutoff (deg per s)", 5.0); - SmartNumber AUTON_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE = new SmartNumber("Arm/Wrist/Auton Feedback Enabled Debounce", 0.15); - - SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Teleop Shoulder Velocity Feedback Cutoff (deg per s)", 20.0); - SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE = new SmartNumber("Arm/Wrist/Teleop Feedback Enabled Debounce", 0.0); - SmartNumber WRIST_SAFE_ANGLE = new SmartNumber("Arm/Wrist/Safe Angle (deg)", 80); SmartNumber TOLERANCE = new SmartNumber("Arm/Wrist/Tolerance (deg)", 7.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index caa1d0dc..38748c2a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -39,13 +39,13 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; /** - * Double jointed arm controlled by two motion profiled PID controllers. - * - * Available control "modes": - * - setpoint control (PID+FF controllers are used) (shoulder is not allowed - * above maximum shoulder angle) - * - voltage override ("force" feeds a voltage to the motor) - */ + * Double jointed arm controlled by two motion profiled PID controllers. + * + * Available control "modes": + * - setpoint control (PID+FF controllers are used) (shoulder is not allowed above maximum shoulder angle) + * - limp mode (controller output is overriden to be zero) + * - voltage override ("force" feeds a voltage to the motor) + */ public abstract class Arm extends SubsystemBase { // Singleton @@ -68,10 +68,6 @@ public static Arm getInstance() { private final SmartNumber shoulderTargetDegrees; private final SmartNumber wristTargetDegrees; - // Voltage overrides (used when present) - private Optional wristVoltageOverride; - private Optional shoulderVoltageOverride; - // controllers for each joint private final Controller shoulderController; private final AngleController wristController; @@ -79,6 +75,10 @@ public static Arm getInstance() { // Mechanism2d visualizer private final ArmVisualizer armVisualizer; + // Voltage overrides (used when present) + private Optional wristVoltageOverride; + private Optional shoulderVoltageOverride; + private SmartNumber shoulderVelocityFeedbackDebounce; private SmartNumber shoulderVelocityFeedbackCutoff; @@ -133,29 +133,19 @@ protected Arm() { shoulderTargetDegrees = new SmartNumber("Arm/Shoulder/Target Angle (deg)", -90); wristTargetDegrees = new SmartNumber("Arm/Wrist/Target Angle (deg)", +90); - // These numbers are used for disabling/enabling wrist control while the - // shoulder is moving -- they are no longer necessary. Remove them - shoulderVelocityFeedbackDebounce = new SmartNumber( - "Arm/Wrist/Feedback Enabled Debounce", - Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue()); - - shoulderVelocityFeedbackCutoff = new SmartNumber( - "Arm/Wrist/Shoulder Velocity Feedback Cutoff (deg per s)", - Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue()); - shoulderMaxVelocity = new SmartNumber( - "Arm/Shoulder/Max Velocity", - Shoulder.TELEOP_MAX_VELOCITY.doubleValue()); + "Arm/Shoulder/Max Velocity", + Shoulder.TELEOP_MAX_VELOCITY.doubleValue()); shoulderMaxAcceleration = new SmartNumber( - "Arm/Shoulder/Max Acceleration", - Shoulder.TELEOP_MAX_ACCELERATION.doubleValue()); + "Arm/Shoulder/Max Acceleration", + Shoulder.TELEOP_MAX_ACCELERATION.doubleValue()); wristMaxVelocity = new SmartNumber( - "Arm/Wrist/Max Velocity", - Wrist.TELEOP_MAX_VELOCITY.doubleValue()); + "Arm/Wrist/Max Velocity", + Wrist.TELEOP_MAX_VELOCITY.doubleValue()); wristMaxAcceleration = new SmartNumber( - "Arm/Wrist/Max Acceleration", - Wrist.TELEOP_MAX_ACCELERATION.doubleValue()); + "Arm/Wrist/Max Acceleration", + Wrist.TELEOP_MAX_ACCELERATION.doubleValue()); shoulderController = new MotorFeedforward(Shoulder.Feedforward.kS, Shoulder.Feedforward.kV, Shoulder.Feedforward.kA).position() @@ -163,17 +153,19 @@ protected Arm() { .add(new ArmDriveFeedforward(new GamePiecekG(), SwerveDrive.getInstance()::getForwardAccelerationGs)) .add(new PIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD)) .setSetpointFilter( - new MotionProfile( - shoulderMaxVelocity.filtered(Math::toRadians).number(), - shoulderMaxAcceleration.filtered(Math::toRadians).number())); + new MotionProfile( + shoulderMaxVelocity.filtered(Math::toRadians).number(), + shoulderMaxAcceleration.filtered(Math::toRadians).number())) + .setOutputFilter(x -> { return shoulderVoltageOverride.orElse(x); }); wristController = new MotorFeedforward(Wrist.Feedforward.kS, Wrist.Feedforward.kV, Wrist.Feedforward.kA).angle() .add(new ArmEncoderAngleFeedforward(Wrist.Feedforward.kG)) .add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD) - .setSetpointFilter( - new AMotionProfile( - wristMaxVelocity.filtered(Math::toRadians).number(), - wristMaxAcceleration.filtered(Math::toRadians).number()))); + .setSetpointFilter( + new AMotionProfile( + wristMaxVelocity.filtered(Math::toRadians).number(), + wristMaxAcceleration.filtered(Math::toRadians).number()))) + .setOutputFilter(x -> { return wristVoltageOverride.orElse(x); }); wristVoltageOverride = Optional.empty(); shoulderVoltageOverride = Optional.empty(); @@ -193,12 +185,6 @@ public void disableGamePieceGravityCompensation() { pieceGravityCompensation = false; } - // Arm Control Overrides - private final boolean isWristFeedbackEnabled() { - return Math.abs(getShoulderVelocityRadiansPerSecond()) < Units - .degreesToRadians(shoulderVelocityFeedbackCutoff.doubleValue()); - } - // Set kinematic constraints public final void setShoulderConstraints(Number velocity, Number acceleration) { @@ -276,7 +262,6 @@ public final boolean isAtTargetState(double shoulderEpsilonDegrees, double wrist // Read angle measurements public abstract Rotation2d getShoulderAngle(); - protected abstract Rotation2d getRelativeWristAngle(); public final Rotation2d getWristAngle() { @@ -288,7 +273,6 @@ public final ArmState getState() { } public abstract double getShoulderVelocityRadiansPerSecond(); - public abstract double getWristVelocityRadiansPerSecond(); // Set a voltage override @@ -302,12 +286,33 @@ public void setWristVoltage(double voltage) { // Feed a voltage to the hardware layer protected abstract void setShoulderVoltageImpl(double voltage); - protected abstract void setWristVoltageImpl(double voltage); // set coast / brake mode public void setCoast(boolean wristCoast, boolean shoulderCoast) {} + // set if the ligaments are "limp" (zero voltage) + public final void setLimp(boolean wristLimp, boolean shoulderLimp) { + if (wristLimp) { + setWristVoltage(0); + } else { + wristVoltageOverride = Optional.empty(); + } + + if (shoulderLimp) { + setShoulderVoltage(0); + } else { + shoulderVoltageOverride = Optional.empty(); + } + } + + public final void enableLimp() { + setLimp(true, true); + } + public final void disableLimp() { + setLimp(false, false); + } + // Arm Visualizer public final ArmVisualizer getVisualizer() { return armVisualizer; @@ -317,7 +322,7 @@ public final ArmVisualizer getVisualizer() { public final void periodic() { // Validate shoulder and wrist target states Rotation2d shoulderTarget = getShoulderTargetAngle(); - // Rotation2d wristTarget = getWristTargetAngle(); + Rotation2d wristTarget = getWristTargetAngle(); double normalizedDeg = shoulderTarget.minus(Rotation2d.fromDegrees(-90)).getDegrees(); @@ -329,41 +334,35 @@ public final void periodic() { // Run control loops on validated target angles shoulderController.update( - getWrappedShoulderAngle(getShoulderTargetAngle()), - getWrappedShoulderAngle(getShoulderAngle())); + getWrappedShoulderAngle(getShoulderTargetAngle()), + getWrappedShoulderAngle(getShoulderAngle())); SmartDashboard.putNumber("Arm/Shoulder/Wrapped Angle", getWrappedShoulderAngle(getShoulderAngle())); - SmartDashboard.putNumber("Arm/Shoulder/Wrapped Target Angle", - getWrappedShoulderAngle(getShoulderTargetAngle())); + SmartDashboard.putNumber("Arm/Shoulder/Wrapped Target Angle", getWrappedShoulderAngle(getShoulderTargetAngle())); wristController.update( - Angle.fromRotation2d(getWristTargetAngle()), - Angle.fromRotation2d(getWristAngle())); + Angle.fromRotation2d(getWristTargetAngle()), + Angle.fromRotation2d(getWristAngle())); setWristVoltageImpl(wristController.getOutput()); setShoulderVoltageImpl(shoulderController.getOutput()); - armVisualizer.setTargetAngles(Units.radiansToDegrees(shoulderController.getSetpoint()), - wristController.getSetpoint().toDegrees()); + armVisualizer.setTargetAngles(Units.radiansToDegrees(shoulderController.getSetpoint()), wristController.getSetpoint().toDegrees()); armVisualizer.setMeasuredAngles(getShoulderAngle().getDegrees(), getWristAngle().getDegrees()); armVisualizer.setFieldArm(Odometry.getInstance().getPose(), getState()); SmartDashboard.putNumber("Arm/Shoulder/Angle (deg)", getShoulderAngle().getDegrees()); - SmartDashboard.putNumber("Arm/Shoulder/Setpoint (deg)", - Units.radiansToDegrees(shoulderController.getSetpoint())); + SmartDashboard.putNumber("Arm/Shoulder/Setpoint (deg)", Units.radiansToDegrees(shoulderController.getSetpoint())); SmartDashboard.putNumber("Arm/Shoulder/Error (deg)", Units.radiansToDegrees(shoulderController.getError())); SmartDashboard.putNumber("Arm/Shoulder/Output (V)", shoulderController.getOutput()); - SmartDashboard.putNumber("Arm/Shoulder/Velocity (deg per s)", - Units.radiansToDegrees(getShoulderVelocityRadiansPerSecond())); + SmartDashboard.putNumber("Arm/Shoulder/Velocity (deg per s)", Units.radiansToDegrees(getShoulderVelocityRadiansPerSecond())); SmartDashboard.putNumber("Arm/Wrist/Angle (deg)", getWristAngle().getDegrees()); SmartDashboard.putNumber("Arm/Wrist/Relative Angle (deg)", getRelativeWristAngle().getDegrees()); SmartDashboard.putNumber("Arm/Wrist/Setpoint (deg)", wristController.getSetpoint().toDegrees()); SmartDashboard.putNumber("Arm/Wrist/Error (deg)", wristController.getError().toDegrees()); SmartDashboard.putNumber("Arm/Wrist/Output (V)", wristController.getOutput()); - SmartDashboard.putNumber("Arm/Wrist/Velocity (deg per s)", - Units.radiansToDegrees(getWristVelocityRadiansPerSecond())); - SmartDashboard.putBoolean("Arm/Wrist/Feedback Enabled Raw", isWristFeedbackEnabled()); + SmartDashboard.putNumber("Arm/Wrist/Velocity (deg per s)", Units.radiansToDegrees(getWristVelocityRadiansPerSecond())); SmartDashboard.putNumber("Arm/Shoulder/kG", new GamePiecekG().doubleValue()); SmartDashboard.putBoolean("Arm/Shoulder/Game Piece Compensation", pieceGravityCompensation); @@ -371,6 +370,5 @@ public final void periodic() { periodicallyCalled(); } - public void periodicallyCalled() { - } + public void periodicallyCalled() {} } diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 8f5082a6..da23e36c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -119,23 +119,20 @@ public double getWristVelocityRadiansPerSecond() { } // private boolean isShoulderStalling() { - // double appliedShoulderVoltage = - // Math.max( - // shoulderRight.getAppliedOutput() * shoulderRight.getBusVoltage(), - // shoulderLeft.getAppliedOutput() * shoulderLeft.getBusVoltage(), - // ); - - // return shoulderEncoder.getVelocity() < - // Shoulder.STALLING_VELOCITY.doubleValue() && shoulderVolts > - // Shoulder.STALLING_VOLTAGE.doubleValue() || - // wrist.getOutputCurrent() > Shoulder.STALLING_CURRENT.doubleValue(); + // double appliedShoulderVoltage = + // Math.max( + // shoulderRight.getAppliedOutput() * shoulderRight.getBusVoltage(), + // shoulderLeft.getAppliedOutput() * shoulderLeft.getBusVoltage(), + // ); + + // return shoulderEncoder.getVelocity() < Shoulder.STALLING_VELOCITY.doubleValue() && shoulderVolts > Shoulder.STALLING_VOLTAGE.doubleValue() || + // wrist.getOutputCurrent() > Shoulder.STALLING_CURRENT.doubleValue(); // } // private boolean isWristStalling() { - // return wristEncoder.getVelocity() < Wrist.STALLING_VELOCITY.doubleValue() && - // wristVolts > Wrist.STALLING_VOLTAGE.doubleValue() || - // shoulderLeft.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue() || - // shoulderRight.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue(); + // return wristEncoder.getVelocity() < Wrist.STALLING_VELOCITY.doubleValue() && wristVolts > Wrist.STALLING_VOLTAGE.doubleValue() || + // shoulderLeft.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue() || + // shoulderRight.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue(); // } @Override @@ -149,11 +146,11 @@ public void periodicallyCalled() { SmartDashboard.putNumber("Arm/Wrist/Current (amps)", wrist.getOutputCurrent()); // if (wristIsStalling()) { - // setWristVoltageImpl(WRIST); + // setWristVoltageImpl(WRIST); // } // if (armIsStalling()) { - // shoulderVolts = 0; + // shoulderVolts = 0; // } // runShoulder(shoulderVolts); From 1e628da13d3e4aa338a92086ea1eb506c34b43b8 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 19 May 2023 19:06:43 -0400 Subject: [PATCH 11/15] Initial wrist tuning --- src/main/java/com/stuypulse/robot/constants/Settings.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 29f97311..4a833ecb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -249,16 +249,16 @@ public interface Wrist { SmartNumber INTAKE_VOLTAGE = new SmartNumber("Arm/Wrist/Intake Voltage", 0); public interface PID { - SmartNumber kP = new SmartNumber("Arm/Wrist/kP", 6.0); + SmartNumber kP = new SmartNumber("Arm/Wrist/kP", 4.0); SmartNumber kI = new SmartNumber("Arm/Wrist/kI", 0); - SmartNumber kD = new SmartNumber("Arm/Wrist/kD", 1); + SmartNumber kD = new SmartNumber("Arm/Wrist/kD", 0.2); } public interface Feedforward { SmartNumber kS = new SmartNumber("Arm/Wrist/kS", 0.0); - SmartNumber kA = new SmartNumber("Arm/Wrist/kA", 0.01); + SmartNumber kA = new SmartNumber("Arm/Wrist/kA", 0.04); SmartNumber kG = new SmartNumber("Arm/Wrist/kG", 0.0); - SmartNumber kV = new SmartNumber("Arm/Wrist/kV", 1.0); + SmartNumber kV = new SmartNumber("Arm/Wrist/kV", 1.7); } } } From 0c60fb389b063c98bf7fcccab496f373bfd65f9c Mon Sep 17 00:00:00 2001 From: Oblivion409 Date: Mon, 22 May 2023 14:21:39 -0400 Subject: [PATCH 12/15] Remove feedback cutoff values --- .../robot/commands/auton/ThreePiece.java | 14 -------------- .../robot/commands/auton/ThreePieceWLow.java | 15 --------------- .../robot/commands/auton/ThreePieceWire.java | 15 --------------- .../robot/commands/auton/TwoPieceDock.java | 2 -- .../robot/commands/auton/TwoPieceWire.java | 7 ------- .../com/stuypulse/robot/subsystems/arm/Arm.java | 11 ----------- 6 files changed, 64 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java index cc2804dd..dcfaa5c5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java @@ -233,13 +233,6 @@ public ThreePiece() { new IntakeStop() ); - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(20); - arm.setShoulderVelocityFeedbackDebounce(0.0); - }) - ); - // intake third piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), @@ -270,13 +263,6 @@ public ThreePiece() { arm.runOnce(() -> arm.setWristVoltage(0)) ); - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(5); - arm.setShoulderVelocityFeedbackDebounce(0.2); - }) - ); - // drive to grid and score third piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java index 2a0b0e1e..47cc974d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -160,7 +160,6 @@ public ThreePieceWLow() { // initial setup addCommands( - new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), new ManagerSetNodeLevel(NodeLevel.LOW), new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), new ManagerSetScoreSide(ScoreSide.BACK) @@ -197,13 +196,6 @@ public ThreePieceWLow() { arm.runOnce(() -> arm.setWristVoltage(0)) ); - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(20); - arm.setShoulderVelocityFeedbackDebounce(0.0); - }) - ); - // drive to grid and score second piece :: TODO: make custom arm setpoint for this addCommands( new ManagerSetGamePiece(GamePiece.CUBE), @@ -260,13 +252,6 @@ public ThreePieceWLow() { arm.runOnce(() -> arm.setWristVoltage(0)) ); - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(5); - arm.setShoulderVelocityFeedbackDebounce(0.2); - }) - ); - // drive to grid and score third piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java index 23689c47..6b0654d8 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java @@ -152,7 +152,6 @@ public ThreePieceWire() { // initial setup addCommands( - new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), new ManagerSetNodeLevel(NodeLevel.LOW), new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), new ManagerSetScoreSide(ScoreSide.BACK) @@ -183,13 +182,6 @@ public ThreePieceWire() { // arm.runOnce(() -> arm.setWristVoltage(0)) ); - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(20); - arm.setShoulderVelocityFeedbackDebounce(0.0); - }) - ); - // drive to grid and score second piece :: TODO: make custom arm setpoint for this addCommands( new ManagerSetGamePiece(GamePiece.CUBE), @@ -244,13 +236,6 @@ public ThreePieceWire() { arm.runOnce(() -> arm.setWristVoltage(0)) ); - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(5); - arm.setShoulderVelocityFeedbackDebounce(0.2); - }) - ); - // drive to grid and score third piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java index 92e29cdb..a220c49d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java @@ -177,8 +177,6 @@ public TwoPieceDock() { new LEDSet(LEDColor.RED), - arm.runOnce(() -> arm.setShoulderVelocityFeedbackCutoff(10.0)), - new ParallelCommandGroup( new SwerveDriveFollowTrajectory( paths.get("Score Piece")) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java index bd2619f0..d9226529 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java @@ -100,13 +100,6 @@ public TwoPieceWire() { arm.runOnce(() -> arm.setWristVoltage(0)) ); - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(20); - arm.setShoulderVelocityFeedbackDebounce(0.0); - }) - ); - // drive to grid and score second piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 38748c2a..a085c779 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -79,9 +79,6 @@ public static Arm getInstance() { private Optional wristVoltageOverride; private Optional shoulderVoltageOverride; - private SmartNumber shoulderVelocityFeedbackDebounce; - private SmartNumber shoulderVelocityFeedbackCutoff; - private boolean pieceGravityCompensation; private final SmartNumber shoulderMaxVelocity; @@ -210,14 +207,6 @@ public final ArmState getTargetState() { return new ArmState(getShoulderTargetAngle(), getWristTargetAngle()); } - public final void setShoulderVelocityFeedbackDebounce(double time) { - shoulderVelocityFeedbackDebounce.set(time); - } - - public final void setShoulderVelocityFeedbackCutoff(double velocity) { - shoulderVelocityFeedbackCutoff.set(velocity); - } - // Set target states private static double getWrappedShoulderAngle(Rotation2d angle) { return MathUtil.inputModulus(angle.getRadians(), Units.degreesToRadians(-270), Units.degreesToRadians(+90)); From 8abd5939918884c91b2fddc28235bf53bf1d11f4 Mon Sep 17 00:00:00 2001 From: Colyi Date: Mon, 22 May 2023 20:04:34 -0400 Subject: [PATCH 13/15] convert wrist to relative angles Co-authored-by: BenG49 --- .../robot/commands/auton/OnePieceDock.java | 3 +- .../commands/auton/OnePieceMobilityDock.java | 3 +- .../robot/commands/auton/ThreePiece.java | 3 +- .../robot/commands/auton/ThreePieceWLow.java | 4 +- .../robot/commands/auton/ThreePieceWire.java | 2 +- .../robot/commands/auton/TwoPieceDock.java | 2 +- .../robot/constants/ArmTrajectories.java | 48 +++++++++---------- .../stuypulse/robot/constants/Settings.java | 2 +- .../stuypulse/robot/subsystems/arm/Arm.java | 2 +- .../robot/subsystems/arm/SimArm.java | 8 +--- .../com/stuypulse/robot/util/ArmState.java | 9 +++- 11 files changed, 44 insertions(+), 42 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java index 97ea16be..0cc97168 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java @@ -12,6 +12,7 @@ import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; import com.stuypulse.robot.commands.swerve.balance.SwerveDriveBalanceBlay; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.Manager.*; import com.stuypulse.robot.util.ArmState; import com.stuypulse.robot.util.ArmTrajectory; @@ -28,7 +29,7 @@ public class OnePieceDock extends DebugSequentialCommandGroup { private class FastStow extends ArmRoutine { public FastStow() { - super(() -> new ArmState(-90, 90) + super(() -> new ArmState(-90, Settings.Arm.Wrist.WRIST_SAFE_ANGLE) .setWristTolerance(360)); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java index 8c8fc521..87f38d48 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java @@ -13,6 +13,7 @@ import com.stuypulse.robot.commands.swerve.*; import com.stuypulse.robot.commands.swerve.balance.SwerveDriveBalanceBlay; import com.stuypulse.robot.subsystems.Manager.*; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.ArmState; import com.stuypulse.robot.util.ArmTrajectory; import com.stuypulse.robot.util.DebugSequentialCommandGroup; @@ -28,7 +29,7 @@ public class OnePieceMobilityDock extends DebugSequentialCommandGroup { private class FastStow extends ArmRoutine { public FastStow() { - super(() -> new ArmState(-90, 90) + super(() -> new ArmState(-90, Settings.Arm.Wrist.WRIST_SAFE_ANGLE) .setWristTolerance(360)); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java index dcfaa5c5..597c1bf0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java @@ -106,10 +106,9 @@ public ArmIntakeSecond() { @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - dest = new ArmState( + dest = ArmState.fromWristHorizontal( -70.82, 10); - // 8.37); double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java index 47cc974d..dadf81cc 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -73,7 +73,7 @@ public ArmIntakeFirst() { @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - dest = new ArmState( + dest = ArmState.fromWristHorizontal( -70.82, 11); // 8.37); @@ -104,7 +104,7 @@ public ArmIntakeSecond() { @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - dest = new ArmState( + dest = ArmState.fromWristHorizontal( -70.82, 10); // 8.37); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java index 6b0654d8..0e4e2cb3 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java @@ -102,7 +102,7 @@ public ArmIntakeSecond() { @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - dest = new ArmState( + dest = ArmState.fromWristHorizontal( -70.82, 10); // 8.37); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java index a220c49d..501944be 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java @@ -76,7 +76,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private class FastStow extends ArmRoutine { public FastStow() { - super(() -> new ArmState(-90, 90) + super(() -> new ArmState(-90, Wrist.WRIST_SAFE_ANGLE) .setWristTolerance(360)); } diff --git a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java index 8893f924..c273bc27 100644 --- a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java +++ b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java @@ -14,45 +14,45 @@ public interface ArmTrajectories { /* Intaking */ public interface Acquire { - ArmState kCone = new ArmState( + ArmState kCone = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Acquire Cone Shoulder", -75), new SmartNumber("Arm Trajectories/Acquire Cone Wrist", 7)); - ArmState kCube = new ArmState( + ArmState kCube = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Acquire Cube Shoulder", -75), new SmartNumber("Arm Trajectories/Acquire Cube Wrist", 12)); - ArmState kHPCone = new ArmState( + ArmState kHPCone = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Acquire HP Cone Shoulder", 0), new SmartNumber("Arm Trajectories/Acquire HP Cone Wrist", 0)); - ArmState kIntermediate = new ArmState( + ArmState kIntermediate = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Acquire Intermediate Front Shoulder", -55), new SmartNumber("Arm Trajectories/Acquire Intermediate Front Wrist", 0)); - ArmState kCubeAuton = new ArmState( + ArmState kCubeAuton = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Auton Acquire Cube Front Shoulder", -70.82), new SmartNumber("Arm Trajectories/Auton Acquire Cube Front Wrist", 8.37)); - ArmState kBOOMCubeAuton = new ArmState( + ArmState kBOOMCubeAuton = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Auton BOOM Acquire Cube Front Shoulder", -70.82), new SmartNumber("Arm Trajectories/Auton BOOM Acquire Cube Front Wrist", 8.37)); - ArmState kIntermediateAuton = new ArmState( + ArmState kIntermediateAuton = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Auton Acquire Intermediate Front Shoulder", -45), new SmartNumber("Arm Trajectories/Auton Acquire Intermediate Front Wrist", 0)); } public interface Deacquire { - ArmState kFrontTrajectory = new ArmState( + ArmState kFrontTrajectory = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Deacquire Front Shoulder", -77), new SmartNumber("Arm Trajectories/Deacquire Front Wrist", 90)); - ArmState kBackTrajectory = new ArmState( + ArmState kBackTrajectory = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Deacquire Back Shoulder", -134), new SmartNumber("Arm Trajectories/Deacquire Back Wrist", -22.9)); } public interface Stow { - ArmState kTrajectory = new ArmState( + ArmState kTrajectory = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Stowed Front Shoulder", -85), new SmartNumber("Arm Trajectories/Stowed Front Wrist", 165)); } @@ -62,55 +62,55 @@ public interface Stow { public interface Ready { public interface Mid { - ArmState kConeTipUpBack = new ArmState( + ArmState kConeTipUpBack = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Shoulder", -172), new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Wrist", 136)); - ArmState kConeTipInBack = new ArmState( + ArmState kConeTipInBack = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Shoulder", -171), new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Wrist", -175)); // -4, 0 - ArmState kConeTipOutFront = new ArmState( + ArmState kConeTipOutFront = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Ready Mid Tip Out/Shoulder", -14), new SmartNumber("Arm Trajectories/Ready Mid Tip Out/Wrist", 42)); - ArmState kAutonCubeBack = new ArmState( + ArmState kAutonCubeBack = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Shoulder", -165), new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Wrist", -133)); - ArmState kCubeFront = new ArmState( + ArmState kCubeFront = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Mid Cube Front/Shoulder", -21), new SmartNumber("Arm Trajectories/Mid Cube Front/Wrist", 49)); - ArmState kCubeBack = new ArmState( + ArmState kCubeBack = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Mid Cube Back/Shoulder", -175), new SmartNumber("Arm Trajectories/Mid Cube Back/Wrist", -62)); } public interface High { - ArmState kConeTipInBack = new ArmState( + ArmState kConeTipInBack = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Ready High Tip In Back Shoulder", -185), new SmartNumber("Arm Trajectories/Ready High Tip In Back Wrist", -180)); // -175, 128 - ArmState kConeTipUpBack = new ArmState( + ArmState kConeTipUpBack = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Ready High Tip Up Back Shoulder", -179), new SmartNumber("Arm Trajectories/Ready High Tip Up Back Wrist", 136)); - ArmState kConeTipOutFront = new ArmState( + ArmState kConeTipOutFront = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Ready High Tip Out Front Shoulder", 3), new SmartNumber("Arm Trajectories/Ready High Tip Out Front Wrist", 37)); - ArmState kCubeFront = new ArmState( + ArmState kCubeFront = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/High Cube Front/Shoulder", -5), new SmartNumber("Arm Trajectories/High Cube Front/Wrist", 46)); - ArmState kCubeAutonBack = new ArmState( + ArmState kCubeAutonBack = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/High Auton Cube Back/Shoulder", -186 - 5), new SmartNumber("Arm Trajectories/High Auton Cube Back/Wrist", -138 + 5)); - ArmState kCubeBack = new ArmState( + ArmState kCubeBack = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/High Cube Back/Shoulder", -186), new SmartNumber("Arm Trajectories/High Cube Back/Wrist", -138)); } @@ -118,13 +118,13 @@ public interface High { public interface Score { public interface High { - ArmState kConeTipOutFront = new ArmState( + ArmState kConeTipOutFront = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Score High Tip Out Front Shoulder", -5), new SmartNumber("Arm Trajectories/Score High Tip Out Front Wrist", 37)); } public interface Mid { - ArmState kConeTipOutFront = new ArmState( + ArmState kConeTipOutFront = ArmState.fromWristHorizontal( new SmartNumber("Arm Trajectories/Score Mid Tip Out Front Shoulder", -28 - 5), new SmartNumber("Arm Trajectories/Score Mid Tip Out Front Wrist", 44)); } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 4a833ecb..951a4638 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -242,7 +242,7 @@ public interface Wrist { SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Wrist/Teleop Max Velocity (deg)", 480.0); SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Wrist/Teleop Max Acceleration (deg)", 480.0); - SmartNumber WRIST_SAFE_ANGLE = new SmartNumber("Arm/Wrist/Safe Angle (deg)", 80); + SmartNumber WRIST_SAFE_ANGLE = new SmartNumber("Arm/Wrist/Safe Angle (deg)", 0); SmartNumber TOLERANCE = new SmartNumber("Arm/Wrist/Tolerance (deg)", 7.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index a085c779..d276c7c0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -254,7 +254,7 @@ public final boolean isAtTargetState(double shoulderEpsilonDegrees, double wrist protected abstract Rotation2d getRelativeWristAngle(); public final Rotation2d getWristAngle() { - return getShoulderAngle().plus(getRelativeWristAngle()); + return getRelativeWristAngle(); } public final ArmState getState() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/SimArm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/SimArm.java index 966823f5..e6be1c42 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/SimArm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/SimArm.java @@ -45,17 +45,13 @@ protected void setShoulderVoltageImpl(double voltage) { } @Override -protected void setWristVoltageImpl(double voltage) { + protected void setWristVoltageImpl(double voltage) { wristVolts = voltage; } @Override public void periodicallyCalled() { - // if (!isWristFeedbackEnabled()) { - // simulation.update(0, wristVolts, Settings.DT); - // } else { - simulation.update(shoulderVolts, wristVolts, Settings.DT); - // // } + simulation.update(shoulderVolts, wristVolts, Settings.DT); } @Override diff --git a/src/main/java/com/stuypulse/robot/util/ArmState.java b/src/main/java/com/stuypulse/robot/util/ArmState.java index a2c2579c..10c870c8 100644 --- a/src/main/java/com/stuypulse/robot/util/ArmState.java +++ b/src/main/java/com/stuypulse/robot/util/ArmState.java @@ -21,9 +21,14 @@ public class ArmState { private boolean wristLimp; - public ArmState(Number shoulderDegrees, Number wristDegrees) { + // breaks smartnumber usage + public static ArmState fromWristHorizontal(Number shoulderDegrees, Number wristDegreesHorizontal) { + return new ArmState(shoulderDegrees, wristDegreesHorizontal.doubleValue() - shoulderDegrees.doubleValue()); + } + + public ArmState(Number shoulderDegrees, Number wristDegreesRelative) { this.shoulder = shoulderDegrees; - this.wrist = wristDegrees; + this.wrist = wristDegreesRelative; shoulderToleranceDegrees = Optional.empty(); wristToleranceDegrees = Optional.empty(); From bbe4a0153e2d72d1f8e20dc5dbff7e23bbbe701b Mon Sep 17 00:00:00 2001 From: BenG49 Date: Tue, 23 May 2023 00:58:19 -0400 Subject: [PATCH 14/15] Sim fixes and flip wrist safe angle by 180 --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java | 2 +- src/main/java/com/stuypulse/robot/util/ArmVisualizer.java | 6 ++++++ 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 951a4638..6cea0356 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -242,7 +242,7 @@ public interface Wrist { SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Wrist/Teleop Max Velocity (deg)", 480.0); SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Wrist/Teleop Max Acceleration (deg)", 480.0); - SmartNumber WRIST_SAFE_ANGLE = new SmartNumber("Arm/Wrist/Safe Angle (deg)", 0); + SmartNumber WRIST_SAFE_ANGLE = new SmartNumber("Arm/Wrist/Safe Angle (deg)", 180); SmartNumber TOLERANCE = new SmartNumber("Arm/Wrist/Tolerance (deg)", 7.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index d276c7c0..fd89a08c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -128,7 +128,7 @@ protected Arm() { // These are the setpoints for the joints relative to the "horizontal" (like the // unit circle) -- keep both shoulderTargetDegrees = new SmartNumber("Arm/Shoulder/Target Angle (deg)", -90); - wristTargetDegrees = new SmartNumber("Arm/Wrist/Target Angle (deg)", +90); + wristTargetDegrees = new SmartNumber("Arm/Wrist/Target Angle (deg)", +180); shoulderMaxVelocity = new SmartNumber( "Arm/Shoulder/Max Velocity", diff --git a/src/main/java/com/stuypulse/robot/util/ArmVisualizer.java b/src/main/java/com/stuypulse/robot/util/ArmVisualizer.java index 0c8f53ce..85a9cd56 100644 --- a/src/main/java/com/stuypulse/robot/util/ArmVisualizer.java +++ b/src/main/java/com/stuypulse/robot/util/ArmVisualizer.java @@ -99,6 +99,9 @@ public ArmVisualizer(FieldObject2d fieldArm) { } public void setTargetAngles(double shoulderAngle, double wristAngle) { + // convert from relative angle to horizontal angle + wristAngle = shoulderAngle + wristAngle; + targetWristRoot.setPosition(8 + (Units.metersToInches(Shoulder.LENGTH)/10)*Math.cos(Units.degreesToRadians(shoulderAngle)), 5.2 + (Units.metersToInches(Shoulder.LENGTH)/10)*Math.sin(Units.degreesToRadians(shoulderAngle))); @@ -109,6 +112,9 @@ public void setTargetAngles(double shoulderAngle, double wristAngle) { } public void setMeasuredAngles(double shoulderAngle, double wristAngle) { + // convert from relative angle to horizontal angle + wristAngle = shoulderAngle + wristAngle; + wristRoot.setPosition(8 + (Units.metersToInches(Shoulder.LENGTH)/10)*Math.cos(Units.degreesToRadians(shoulderAngle)), 5.2 + (Units.metersToInches(Shoulder.LENGTH)/10)*Math.sin(Units.degreesToRadians(shoulderAngle))); From 99e8d0b0c3bdb0b8e42afd13df2611f3ea2b8f5d Mon Sep 17 00:00:00 2001 From: Prog694 Date: Wed, 24 May 2023 17:28:03 -0400 Subject: [PATCH 15/15] Fix motion profile --- src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index fd89a08c..253070a0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -157,11 +157,11 @@ protected Arm() { wristController = new MotorFeedforward(Wrist.Feedforward.kS, Wrist.Feedforward.kV, Wrist.Feedforward.kA).angle() .add(new ArmEncoderAngleFeedforward(Wrist.Feedforward.kG)) - .add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD) + .add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD)) .setSetpointFilter( new AMotionProfile( wristMaxVelocity.filtered(Math::toRadians).number(), - wristMaxAcceleration.filtered(Math::toRadians).number()))) + wristMaxAcceleration.filtered(Math::toRadians).number())) .setOutputFilter(x -> { return wristVoltageOverride.orElse(x); }); wristVoltageOverride = Optional.empty();