From e24a148193be1e218d05a41a1d7b4de9361756b0 Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Sat, 25 Jan 2025 21:08:11 -0800 Subject: [PATCH 1/3] fix direction of the display of arm angle changes --- .../java/competition/subsystems/elevator/ElevatorMechanism.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/competition/subsystems/elevator/ElevatorMechanism.java b/src/main/java/competition/subsystems/elevator/ElevatorMechanism.java index 812abdb..f1b6867 100644 --- a/src/main/java/competition/subsystems/elevator/ElevatorMechanism.java +++ b/src/main/java/competition/subsystems/elevator/ElevatorMechanism.java @@ -62,7 +62,7 @@ public ElevatorMechanism(/* TODO: Inject references to the Elevator + Arm + Scor public void periodic() { // update mechanism based on current elevatorHeight and armAngle elevatorLigament.setLength(elevatorLigamentBaseLengthMeters + elevatorHeight.in(Units.Meters)); - armLigament.setAngle(armLigamentBaseAngleDegrees + armAngle.in(Degrees)); + armLigament.setAngle(armLigamentBaseAngleDegrees - armAngle.in(Degrees)); coralLigament.setLength(this.coralInScorer ? coralLengthMeters : 0.0); aKitLog.record("Mech2d", mech2d); From d11ef2a9ef92e2fe01863a9c9acdf981f57dee98 Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Sat, 25 Jan 2025 21:21:29 -0800 Subject: [PATCH 2/3] fix arm sim frames of reference --- .../simulation/arm/ArmSimConstants.java | 8 +++++--- .../simulation/arm/ArmSimulator.java | 20 ++++++++++++++----- .../arm_pivot/ArmPivotSubsystem.java | 2 +- .../commands/ArmPivotMaintainerCommand.java | 2 +- 4 files changed, 22 insertions(+), 10 deletions(-) diff --git a/src/main/java/competition/simulation/arm/ArmSimConstants.java b/src/main/java/competition/simulation/arm/ArmSimConstants.java index ca925dc..4c48950 100644 --- a/src/main/java/competition/simulation/arm/ArmSimConstants.java +++ b/src/main/java/competition/simulation/arm/ArmSimConstants.java @@ -13,8 +13,10 @@ public class ArmSimConstants { public static final double armReduction = 200; public static final Mass armMass = Kilogram.of(8.0); // Kilograms public static final Distance armLength = Meters.of(0.5); - public static final Angle minAngleRads = Degrees.of(-45); - public static final Angle maxAngleRads = Degrees.of(125 - 45); - public static final Angle startingAngle = minAngleRads; + // the frame of reference for these angles is 0' right, 90' up, 180' left, 270' down + // so we have 225 as the starting angle which is 0' in arm relative terms + public static final Angle minAngleRads = Degrees.of(225 - 125); + public static final Angle maxAngleRads = Degrees.of(225); + public static final Angle startingAngle = maxAngleRads; public static final Angle armEncoderAnglePerRotation = Degrees.of(0.1); } diff --git a/src/main/java/competition/simulation/arm/ArmSimulator.java b/src/main/java/competition/simulation/arm/ArmSimulator.java index c645c18..98404dd 100644 --- a/src/main/java/competition/simulation/arm/ArmSimulator.java +++ b/src/main/java/competition/simulation/arm/ArmSimulator.java @@ -11,6 +11,7 @@ import static edu.wpi.first.units.Units.Kilograms; import static edu.wpi.first.units.Units.Rotations; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import xbot.common.controls.actuators.mock_adapters.MockCANMotorController; @@ -39,7 +40,7 @@ public ArmSimulator(ElevatorMechanism elevatorMechanism, ArmPivotSubsystem armPi ArmSimConstants.minAngleRads.in(Radians), ArmSimConstants.maxAngleRads.in(Radians), true, - 0 + ArmSimConstants.startingAngle.in(Radians) ); } @@ -48,12 +49,21 @@ public void update() { armSim.update(SimulationConstants.loopPeriodSec); // 20ms // Read out the new arm position for rendering - // TODO: the frame of reference of the simulated arm is wrong, gravity isn't being applied - // in the same way we think - var armMotorRotations = armSim.getAngleRads() / ArmSimConstants.armEncoderAnglePerRotation.in(Radians); + var armRelativeAngle = getArmAngle(); + + var armMotorRotations = armRelativeAngle.in(Radians) / ArmSimConstants.armEncoderAnglePerRotation.in(Radians); armMotor.setPosition(Rotations.of(armMotorRotations)); + + // correct for frame of reference for the arm pivot in the mechanism vs sim model - elevatorMechanism.armAngle = Radians.of(armSim.getAngleRads()).plus(ArmSimConstants.minAngleRads.times(-1)).times(-1); + elevatorMechanism.armAngle = armRelativeAngle; + } + + public Angle getArmAngle() { + // convert from the armSim frame of reference to our actual arm frame of reference where the bottom is 0' and the top is 125' + var armSimAngle = Radians.of(armSim.getAngleRads()); + + return armSimAngle.minus(ArmSimConstants.maxAngleRads).times(-1); } } diff --git a/src/main/java/competition/subsystems/arm_pivot/ArmPivotSubsystem.java b/src/main/java/competition/subsystems/arm_pivot/ArmPivotSubsystem.java index 1f72a80..4589130 100644 --- a/src/main/java/competition/subsystems/arm_pivot/ArmPivotSubsystem.java +++ b/src/main/java/competition/subsystems/arm_pivot/ArmPivotSubsystem.java @@ -17,7 +17,7 @@ @Singleton public class ArmPivotSubsystem extends BaseSetpointSubsystem { public final XCANMotorController armMotor; - Angle targetAngle; + Angle targetAngle = Degrees.of(0); ElectricalContract electricalContract; DoubleProperty degreesPerRotations; double rotationsAtZero; diff --git a/src/main/java/competition/subsystems/arm_pivot/commands/ArmPivotMaintainerCommand.java b/src/main/java/competition/subsystems/arm_pivot/commands/ArmPivotMaintainerCommand.java index cc2701e..1848a68 100644 --- a/src/main/java/competition/subsystems/arm_pivot/commands/ArmPivotMaintainerCommand.java +++ b/src/main/java/competition/subsystems/arm_pivot/commands/ArmPivotMaintainerCommand.java @@ -69,7 +69,7 @@ protected double getErrorMagnitude() { //distance from goal protected double getHumanInput() { //gamepad controls: Left joy stick up/down & Left bumper to switch between elevator/arm return MathUtils.constrainDouble( MathUtils.deadband( - oi.programmerGamepad.getLeftStickY(), + oi.programmerGamepad.getRightStickY(), oi.getOperatorGamepadTypicalDeadband(), (a) -> (a)), humanMinPower.get(), humanMaxPower.get()); From 0be5b494ea66b7895a07cd23a0e92529ea747b73 Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Sat, 25 Jan 2025 21:25:27 -0800 Subject: [PATCH 3/3] fix inversion in arm sim power direction --- src/main/java/competition/simulation/arm/ArmSimulator.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/competition/simulation/arm/ArmSimulator.java b/src/main/java/competition/simulation/arm/ArmSimulator.java index 98404dd..b31624c 100644 --- a/src/main/java/competition/simulation/arm/ArmSimulator.java +++ b/src/main/java/competition/simulation/arm/ArmSimulator.java @@ -45,7 +45,8 @@ public ArmSimulator(ElevatorMechanism elevatorMechanism, ArmPivotSubsystem armPi } public void update() { - armSim.setInput(this.armMotor.getPower() * RobotController.getBatteryVoltage()); + // invert power because the simulated arm is going "backwards" + armSim.setInput(this.armMotor.getPower() * RobotController.getBatteryVoltage() * -1.0); armSim.update(SimulationConstants.loopPeriodSec); // 20ms // Read out the new arm position for rendering