From 865e3715cae97cec6b0dc2a241d719aa382148e7 Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Tue, 5 Mar 2024 14:56:35 -0500 Subject: [PATCH 1/2] move feedforward into Arm.java --- src/main/java/frc/robot/arm/Arm.java | 14 +++++++------- src/main/java/frc/robot/arm/ArmModel.java | 19 ------------------- 2 files changed, 7 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index fd76f1a..70ee8f2 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose3d; @@ -28,12 +29,13 @@ public class Arm extends SubsystemBase { private final double kSDeadband = 0.05; private final ArmIO m_io; private final ArmIOInputsAutoLogged m_inputs = new ArmIOInputsAutoLogged(); - private final ArmModel m_armModel = new ArmModel(); private final ArmVisualizer m_goalVisualizer, m_setpointVisualizer, m_measuredVisualizer; private ProfiledPIDController m_elbowController = new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0)); private ProfiledPIDController m_wristController = new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0)); + private final ArmFeedforward m_elbowFeedForward = new ArmFeedforward(0.0, 0.15, 2.31); + private final ArmFeedforward m_wristFeedForward = new ArmFeedforward(0.0, 0.51, 0.8); private static final LoggedTunableNumber elbowkP = new LoggedTunableNumber("Arm/Elbow/kP"); private static final LoggedTunableNumber elbowkD = new LoggedTunableNumber("Arm/Elbow/kD"); @@ -190,12 +192,10 @@ private void runSetpoint(double elbowDelay, double wristDelay) { } else wristFeedbackVolts = m_deadbandkS.apply(m_wristController.calculate(m_inputs.wristPositionRad), wristkS.get()); - final Vector feedforwardVolts = - m_armModel.feedforward( - VecBuilder.fill(elbowNextPosition, wristNextPosition), - VecBuilder.fill(elbowNextVelocity, wristNextVelocity)); - double elbowFeedForwardVolts = feedforwardVolts.get(0, 0); - double wristFeedForwardVolts = feedforwardVolts.get(1, 0); + double elbowFeedForwardVolts = + m_elbowFeedForward.calculate(elbowNextPosition, elbowNextVelocity); + double wristFeedForwardVolts = + m_wristFeedForward.calculate(wristNextPosition, wristNextVelocity); Logger.recordOutput("Arm/Elbow Feed Forward", elbowFeedForwardVolts); Logger.recordOutput("Arm/Wrist Feed Forward", wristFeedForwardVolts); m_io.setElbowVoltage(elbowFeedbackVolts + elbowFeedForwardVolts); diff --git a/src/main/java/frc/robot/arm/ArmModel.java b/src/main/java/frc/robot/arm/ArmModel.java index 1ace565..6fdc289 100644 --- a/src/main/java/frc/robot/arm/ArmModel.java +++ b/src/main/java/frc/robot/arm/ArmModel.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.numbers.N4; @@ -25,8 +24,6 @@ public class ArmModel { private final double kElbowMassKg = Units.lbsToKilograms(7.0); private final double kElbowMoiKgMetersSq = SingleJointedArmSim.estimateMOI(kElbowLengthMeters, kElbowMassKg); - private final double elbowkG = 0.15; - private final double elbowkV = 2.31; public static final double kWristGearboxReduction = 3.0 * 5.0; public static final double kWristChainReduction = 36.0 / 22.0; @@ -38,15 +35,9 @@ public class ArmModel { private final double kWristMassKg = Units.lbsToKilograms(11.5); private final double kWristMoiKgMetersSq = SingleJointedArmSim.estimateMOI(kWristLengthMeters, kWristMassKg); - private final double wristkG = 0.51; - private final double wristkV = 0.8; - - private final ArmFeedforward m_elbowFeedForward, m_wristFeedForward; private final SingleJointedArmSim m_elbowSim, m_wristSim; public ArmModel() { - m_elbowFeedForward = new ArmFeedforward(0.0, elbowkG, elbowkV, 0.0); - m_wristFeedForward = new ArmFeedforward(0.0, wristkG, wristkV, 0.0); m_elbowSim = new SingleJointedArmSim( kElbowGearbox, @@ -69,16 +60,6 @@ public ArmModel() { 0.0); } - public Vector feedforward(Vector position) { - return feedforward(position, VecBuilder.fill(0.0, 0.0)); - } - - public Vector feedforward(Vector position, Vector velocity) { - return VecBuilder.fill( - m_elbowFeedForward.calculate(position.get(0, 0), velocity.get(0, 0)), - m_wristFeedForward.calculate(position.get(0, 0), velocity.get(1, 0))); - } - /** * Adjusts the simulated state of the arm based on applied voltages. * From 53474e0627ba83e57542268a36a02a0682172afe Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Tue, 5 Mar 2024 16:32:48 -0500 Subject: [PATCH 2/2] refactor arm code, add sysid, and fix gyro zero --- src/main/java/frc/robot/Robot.java | 20 +++++++ src/main/java/frc/robot/arm/Arm.java | 59 +++++++++++++++++++ src/main/java/frc/robot/arm/ArmModel.java | 4 -- .../java/frc/robot/arm/ArmVisualizer.java | 17 ++---- .../java/frc/robot/drivetrain/Drivetrain.java | 8 ++- src/main/java/frc/robot/shooter/Shooter.java | 4 +- 6 files changed, 94 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 53b26ba..ec59f16 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -148,6 +148,26 @@ public Robot() { "Shooter SysId (Dynamic Forward)", m_shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); m_autoChooser.addOption( "Shooter SysId (Dynamic Reverse)", m_shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + m_autoChooser.addOption( + "Elbow SysId (Quasistatic Forward)", + m_arm.sysIdElbowQuasistatic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Elbow SysId (Quasistatic Reverse)", + m_arm.sysIdElbowQuasistatic(SysIdRoutine.Direction.kReverse)); + m_autoChooser.addOption( + "Elbow SysId (Dynamic Forward)", m_arm.sysIdElbowDynamic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Elbow SysId (Dynamic Reverse)", m_arm.sysIdElbowDynamic(SysIdRoutine.Direction.kReverse)); + m_autoChooser.addOption( + "Wrist SysId (Quasistatic Forward)", + m_arm.sysIdWristQuasistatic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Wrist SysId (Quasistatic Reverse)", + m_arm.sysIdWristQuasistatic(SysIdRoutine.Direction.kReverse)); + m_autoChooser.addOption( + "Wrist SysId (Dynamic Forward)", m_arm.sysIdWristDynamic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Wrist SysId (Dynamic Reverse)", m_arm.sysIdWristDynamic(SysIdRoutine.Direction.kReverse)); m_autoChooser.addOption("3 Piece Centerline", autos.centerlineTwoPiece()); m_drivetrain.setDefaultCommand( m_drivetrain.joystickDrive( diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 70ee8f2..f9a8bb7 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -1,5 +1,8 @@ package frc.robot.arm; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.controller.ArmFeedforward; @@ -15,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.LoggedTunableNumber; import frc.robot.Constants; import java.util.Set; @@ -62,6 +66,7 @@ public class Arm extends SubsystemBase { private Vector m_profileInitialAngles; public Supplier wristPoseSupplier; + private final SysIdRoutine m_sysIdRoutineElbow, m_sysIdRoutineWrist; static { switch (Constants.kCurrentMode) { @@ -104,6 +109,24 @@ public Arm(ArmIO io) { m_elbowController.setTolerance(Units.degreesToRadians(0.5)); m_wristController.setTolerance(Units.degreesToRadians(0.5)); wristPoseSupplier = () -> m_measuredVisualizer.getWristPose(); + m_sysIdRoutineElbow = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(0.25).per(Seconds.of(1.0)), + Volts.of(2.0), + null, + state -> Logger.recordOutput("Arm/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + voltage -> m_io.setElbowVoltage(voltage.in(Volts)), null, this)); + m_sysIdRoutineWrist = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(0.25).per(Seconds.of(1.0)), + Volts.of(2.0), + null, + state -> Logger.recordOutput("Arm/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + voltage -> m_io.setWristVoltage(voltage.in(Volts)), null, this)); setDefaultCommand(idle()); } @@ -254,6 +277,42 @@ public Command idleCoast() { .withName("Idle Coast"); } + public Command sysIdElbowQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineElbow + .quasistatic(direction) + .until( + () -> + m_inputs.elbowPositionRad > (Math.PI / 2) + || m_inputs.elbowPositionRad < ArmSetpoints.kStowed.elbowAngle); + } + + public Command sysIdElbowDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineElbow + .dynamic(direction) + .until( + () -> + m_inputs.elbowPositionRad > (Math.PI / 2) + || m_inputs.elbowPositionRad < ArmSetpoints.kStowed.elbowAngle); + } + + public Command sysIdWristQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineWrist + .quasistatic(direction) + .until( + () -> + m_inputs.elbowPositionRad < 0.0 + || m_inputs.wristPositionRad > ArmSetpoints.kStowed.wristAngle); + } + + public Command sysIdWristDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineWrist + .dynamic(direction) + .until( + () -> + m_inputs.elbowPositionRad < 0.0 + || m_inputs.wristPositionRad > ArmSetpoints.kStowed.wristAngle); + } + private boolean getJointsAtGoal() { return m_elbowController.atGoal() && m_wristController.atGoal(); } diff --git a/src/main/java/frc/robot/arm/ArmModel.java b/src/main/java/frc/robot/arm/ArmModel.java index 6fdc289..95e4a0f 100644 --- a/src/main/java/frc/robot/arm/ArmModel.java +++ b/src/main/java/frc/robot/arm/ArmModel.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.numbers.N4; import edu.wpi.first.math.system.plant.DCMotor; @@ -11,9 +10,6 @@ public class ArmModel { - public static final Translation2d origin = - new Translation2d(Units.inchesToMeters(-9.2), Units.inchesToMeters(20)); - public static final double kElbowGearboxReduction = 3.0 * 5.0 * 5.0; public static final double kElbowChainReduction = 36.0 / 22.0; public static final double kElbowFinalReduction = kElbowGearboxReduction * kElbowChainReduction; diff --git a/src/main/java/frc/robot/arm/ArmVisualizer.java b/src/main/java/frc/robot/arm/ArmVisualizer.java index 62eefdc..c2814f3 100644 --- a/src/main/java/frc/robot/arm/ArmVisualizer.java +++ b/src/main/java/frc/robot/arm/ArmVisualizer.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; @@ -16,6 +17,8 @@ public class ArmVisualizer { // Value from CAD, angle from horizontal to shoulder. private final double kShoulderAngleDegrees = 103.985388; + private final Translation2d origin = + new Translation2d(Units.inchesToMeters(-9.2), Units.inchesToMeters(20)); private final String m_logKey; private final Mechanism2d m_mechanism; private final MechanismRoot2d m_mechanismRoot; @@ -25,15 +28,11 @@ public class ArmVisualizer { public ArmVisualizer(String logKey, double ligamentWidth, Color color) { m_logKey = logKey; m_mechanism = new Mechanism2d(4, 3, new Color8Bit(Color.kGray)); - m_mechanismRoot = m_mechanism.getRoot("Arm", 2 + ArmModel.origin.getX(), 0); + m_mechanismRoot = m_mechanism.getRoot("Arm", 2 + origin.getX(), 0); m_fixedShoulderLigament = m_mechanismRoot.append( new MechanismLigament2d( - "Shoulder", - ArmModel.origin.getY(), - kShoulderAngleDegrees, - 6, - new Color8Bit(Color.kBlack))); + "Shoulder", origin.getY(), kShoulderAngleDegrees, 6, new Color8Bit(Color.kBlack))); m_elbowLigament = m_fixedShoulderLigament.append( new MechanismLigament2d( @@ -50,11 +49,7 @@ public void update(double elbowAngle, double wristAngle) { Logger.recordOutput("Mechanism2d/" + m_logKey, m_mechanism); m_elbowPose = - new Pose3d( - ArmModel.origin.getX(), - 0.0, - ArmModel.origin.getY(), - new Rotation3d(0.0, -elbowAngle, 0.0)); + new Pose3d(origin.getX(), 0.0, origin.getY(), new Rotation3d(0.0, -elbowAngle, 0.0)); m_wristPose = m_elbowPose.transformBy( new Transform3d( diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index 230196a..ebb9dab 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -212,7 +212,13 @@ public Command joystickDrive( } public Command zeroGyro() { - return Commands.runOnce(() -> m_imuIO.setGyroAngle(0.0)).ignoringDisable(true); + return Commands.runOnce( + () -> + resetPose.accept( + PoseEstimation.getInstance() + .getPose() + .rotateBy(m_imuInputs.yawPosition.unaryMinus()))) + .ignoringDisable(true); } public Command sysIdDriveQuasistatic(SysIdRoutine.Direction direction) { diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 2ec5270..2531a72 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -53,9 +53,9 @@ public Shooter(ShooterIO io) { null, null, null, - (state) -> Logger.recordOutput("Shooter/SysIdState", state.toString())), + state -> Logger.recordOutput("Shooter/SysIdState", state.toString())), new SysIdRoutine.Mechanism( - (voltage) -> { + voltage -> { m_io.setTopRollerVoltage(voltage.in(Volts)); m_io.setBottomRollerVoltage(voltage.in(Volts)); },