diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 61ca69d0..be69684c 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -85,8 +85,6 @@ public void autonomousInit() { 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()); @@ -115,8 +113,6 @@ public void teleopInit() { 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/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 cc2804dd..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(); @@ -233,13 +232,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 +262,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..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); @@ -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..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); @@ -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..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)); } @@ -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/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 22deba4b..6cea0356 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -242,29 +242,23 @@ 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 WRIST_SAFE_ANGLE = new SmartNumber("Arm/Wrist/Safe Angle (deg)", 180); SmartNumber TOLERANCE = new SmartNumber("Arm/Wrist/Tolerance (deg)", 7.0); 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); } } } 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..253070a0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -14,10 +14,10 @@ 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 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; @@ -38,9 +38,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,19 +75,10 @@ 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; - private boolean pieceGravityCompensation; private final SmartNumber shoulderMaxVelocity; @@ -122,34 +110,25 @@ 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); - - 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()); - - wristEnabled = BStream.create(this::isWristFeedbackEnabled) - .filtered(new BDebounce.Both(shoulderVelocityFeedbackDebounce)); + wristTargetDegrees = new SmartNumber("Arm/Wrist/Target Angle (deg)", +180); shoulderMaxVelocity = new SmartNumber( "Arm/Shoulder/Max Velocity", @@ -165,35 +144,25 @@ protected Arm() { "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); - }) - ; + 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 -> { 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); - }); - - wristLimp = new SmartBoolean("Arm/Wrist/Is Limp?", false); - shoulderLimp = new SmartBoolean("Arm/Shoulder/Is Limp?", false); + .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())) + .setOutputFilter(x -> { return wristVoltageOverride.orElse(x); }); wristVoltageOverride = Optional.empty(); shoulderVoltageOverride = Optional.empty(); @@ -213,20 +182,6 @@ public void disableGamePieceGravityCompensation() { pieceGravityCompensation = false; } - // 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()); - } - // Set kinematic constraints public final void setShoulderConstraints(Number velocity, Number acceleration) { @@ -239,7 +194,6 @@ public final void setWristConstraints(Number velocity, Number acceleration) { wristMaxAcceleration.set(acceleration); } - // Read target State public final Rotation2d getShoulderTargetAngle() { return Rotation2d.fromDegrees(shoulderTargetDegrees.get()); @@ -253,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)); @@ -308,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() { @@ -336,8 +282,17 @@ 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); + if (wristLimp) { + setWristVoltage(0); + } else { + wristVoltageOverride = Optional.empty(); + } + + if (shoulderLimp) { + setShoulderVoltage(0); + } else { + shoulderVoltageOverride = Optional.empty(); + } } public final void enableLimp() { @@ -366,7 +321,6 @@ public final void periodic() { setShoulderTargetAngle(Rotation2d.fromDegrees(180 - Shoulder.MAX_SHOULDER_ANGLE.get())); } - // Run control loops on validated target angles shoulderController.update( getWrappedShoulderAngle(getShoulderTargetAngle()), @@ -398,8 +352,6 @@ public final void periodic() { 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.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/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(); 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)));