-
Notifications
You must be signed in to change notification settings - Fork 16
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
back port from 2024-frc-software (#97)
* update to WPILib 2024.3.1 and latests vendor deps * changed default to field relative * fix bug where driveFacingAngle translation was reversed on red alliance * add ArmSystemSim class and VelocitySystemSim class to make it easier to incorporate system models with Phoenix 6 devices * incorporate pose ambiguity into standard deviation calcualtion for estimated pose from vision * ignore vision pose estimate if too far from current estimated position (this requires use of a command to reset the pose to the estiamted pose from vision) * update FieldConstants and Field2d classes for Crescendo * add lock to speaker button which keeps the robot pointed at its alliance speaker while driving
- Loading branch information
Showing
23 changed files
with
499 additions
and
91 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,125 @@ | ||
| package frc.lib.team3061.sim; | ||
|
|
||
| import com.ctre.phoenix6.hardware.CANcoder; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import com.ctre.phoenix6.sim.CANcoderSimState; | ||
| import com.ctre.phoenix6.sim.ChassisReference; | ||
| import com.ctre.phoenix6.sim.TalonFXSimState; | ||
| import edu.wpi.first.math.system.plant.DCMotor; | ||
| import edu.wpi.first.math.util.Units; | ||
| import edu.wpi.first.wpilibj.RobotController; | ||
| import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; | ||
| import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; | ||
| import edu.wpi.first.wpilibj.util.Color; | ||
| import edu.wpi.first.wpilibj.util.Color8Bit; | ||
| import frc.robot.Constants; | ||
| import org.littletonrobotics.junction.Logger; | ||
|
|
||
| public class ArmSystemSim { | ||
|
|
||
| private TalonFX motor; | ||
| private CANcoder encoder; | ||
| private TalonFXSimState motorSimState; | ||
| private CANcoderSimState encoderSimState; | ||
| private SingleJointedArmSim systemSim; | ||
| private double sensorToMechanismRatio; | ||
| private double rotorToSensorRatio; | ||
| private String subsystemName; | ||
|
|
||
| // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm. | ||
| private Mechanism2d mech2d; | ||
| private MechanismRoot2d armPivot; | ||
| private MechanismLigament2d armTower; | ||
| private MechanismLigament2d arm; | ||
|
|
||
| public ArmSystemSim( | ||
| TalonFX motor, | ||
| CANcoder encoder, | ||
| boolean motorInverted, | ||
| double sensorToMechanismRatio, | ||
| double rotorToSensorRatio, | ||
| double length, | ||
| double mass, | ||
| double minAngle, | ||
| double maxAngle, | ||
| double startingAngle, | ||
| String subsystemName) { | ||
| if (Constants.getMode() != Constants.Mode.SIM) { | ||
| return; | ||
| } | ||
|
|
||
| this.motor = motor; | ||
| this.encoder = encoder; | ||
| this.sensorToMechanismRatio = sensorToMechanismRatio; | ||
| this.rotorToSensorRatio = rotorToSensorRatio; | ||
|
|
||
| this.motorSimState = this.motor.getSimState(); | ||
| this.motorSimState.Orientation = | ||
| motorInverted | ||
| ? ChassisReference.Clockwise_Positive | ||
| : ChassisReference.CounterClockwise_Positive; | ||
| this.encoderSimState = this.encoder.getSimState(); | ||
| this.encoderSimState.Orientation = | ||
| motorInverted | ||
| ? ChassisReference.Clockwise_Positive | ||
| : ChassisReference.CounterClockwise_Positive; | ||
|
|
||
| this.systemSim = | ||
| new SingleJointedArmSim( | ||
| DCMotor.getFalcon500Foc(1), | ||
| sensorToMechanismRatio * rotorToSensorRatio, | ||
| SingleJointedArmSim.estimateMOI(length, mass), | ||
| length, | ||
| minAngle, | ||
| maxAngle, | ||
| true, | ||
| startingAngle); | ||
|
|
||
| this.mech2d = new Mechanism2d(1, 1); | ||
| this.armPivot = mech2d.getRoot("ArmPivot", 0.5, 0.5); | ||
| this.armTower = armPivot.append(new MechanismLigament2d("ArmTower", .3, -90)); | ||
| this.arm = | ||
| armPivot.append( | ||
| new MechanismLigament2d("Arm", length, startingAngle, 6, new Color8Bit(Color.kYellow))); | ||
| this.armTower.setColor(new Color8Bit(Color.kBlue)); | ||
|
|
||
| this.subsystemName = subsystemName; | ||
| } | ||
|
|
||
| public void updateSim() { | ||
| if (Constants.getMode() != Constants.Mode.SIM) { | ||
| return; | ||
| } | ||
|
|
||
| // update the sim states supply voltage based on the simulated battery | ||
| this.motorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); | ||
| this.encoderSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); | ||
|
|
||
| // update the input voltages of the models based on the outputs of the simulated TalonFXs | ||
| double motorVoltage = this.motorSimState.getMotorVoltage(); | ||
| this.systemSim.setInput(motorVoltage); | ||
|
|
||
| // update the models | ||
| this.systemSim.update(Constants.LOOP_PERIOD_SECS); | ||
|
|
||
| // update the simulated TalonFX based on the model outputs | ||
| double mechanismRadians = this.systemSim.getAngleRads(); | ||
| double mechanismRotations = mechanismRadians / (2 * Math.PI); | ||
| double sensorRotations = mechanismRotations * this.sensorToMechanismRatio; | ||
| double motorRotations = sensorRotations * this.rotorToSensorRatio; | ||
| double mechanismRadiansPerSec = this.systemSim.getVelocityRadPerSec(); | ||
| double mechanismRPS = mechanismRadiansPerSec / (2 * Math.PI); | ||
| double encoderRPS = mechanismRPS * this.sensorToMechanismRatio; | ||
| double motorRPS = encoderRPS * this.rotorToSensorRatio; | ||
| this.motorSimState.setRawRotorPosition(motorRotations); | ||
| this.motorSimState.setRotorVelocity(motorRPS); | ||
| this.encoderSimState.setRawPosition(sensorRotations); | ||
| this.encoderSimState.setVelocity(encoderRPS); | ||
|
|
||
| // Update the Mechanism Arm angle based on the simulated arm angle | ||
| arm.setAngle(Units.radiansToDegrees(mechanismRadians)); | ||
| Logger.recordOutput(subsystemName + "/ArmSim", mech2d); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,58 @@ | ||
| package frc.lib.team3061.sim; | ||
|
|
||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import com.ctre.phoenix6.sim.ChassisReference; | ||
| import com.ctre.phoenix6.sim.TalonFXSimState; | ||
| import edu.wpi.first.math.numbers.N1; | ||
| import edu.wpi.first.math.system.plant.LinearSystemId; | ||
| import edu.wpi.first.wpilibj.RobotController; | ||
| import edu.wpi.first.wpilibj.simulation.LinearSystemSim; | ||
| import frc.robot.Constants; | ||
|
|
||
| public class VelocitySystemSim { | ||
|
|
||
| private TalonFX motor; | ||
| private TalonFXSimState motorSimState; | ||
| private LinearSystemSim<N1, N1, N1> systemSim; | ||
| private double gearRatio; | ||
|
|
||
| public VelocitySystemSim( | ||
| TalonFX motor, boolean motorInverted, double kV, double kA, double gearRatio) { | ||
| this.motor = motor; | ||
| this.gearRatio = gearRatio; | ||
|
|
||
| if (Constants.getMode() != Constants.Mode.SIM) { | ||
| return; | ||
| } | ||
|
|
||
| this.motorSimState = this.motor.getSimState(); | ||
| this.motorSimState.Orientation = | ||
| motorInverted | ||
| ? ChassisReference.Clockwise_Positive | ||
| : ChassisReference.CounterClockwise_Positive; | ||
|
|
||
| this.systemSim = new LinearSystemSim<>(LinearSystemId.identifyVelocitySystem(kV, kA)); | ||
| } | ||
|
|
||
| public void updateSim() { | ||
| if (Constants.getMode() != Constants.Mode.SIM) { | ||
| return; | ||
| } | ||
|
|
||
| // update the sim states supply voltage based on the simulated battery | ||
| this.motorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); | ||
|
|
||
| // update the input voltages of the models based on the outputs of the simulated TalonFXs | ||
| this.systemSim.setInput(this.motorSimState.getMotorVoltage()); | ||
|
|
||
| // update the models | ||
| this.systemSim.update(Constants.LOOP_PERIOD_SECS); | ||
|
|
||
| // update the simulated TalonFX based on the model outputs | ||
| double mechanismRadiansPerSec = this.systemSim.getOutput(0); | ||
| double motorRPS = mechanismRadiansPerSec * this.gearRatio / (2 * Math.PI); | ||
| double motorRotations = motorRPS * Constants.LOOP_PERIOD_SECS; | ||
| this.motorSimState.addRotorPosition(motorRotations); | ||
| this.motorSimState.setRotorVelocity(motorRPS); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.