From 5225e1ff636bdf5b96206512bbcc3d373d9a1e76 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Fri, 8 Dec 2023 15:18:19 -0500 Subject: [PATCH] Address comments --- .../src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/sim/TalonFXSimProfile.java | 24 ++---- .../src/main/java/frc/robot/Robot.java | 1 - .../src/main/java/frc/robot/Mechanisms.java | 22 ++--- .../src/main/java/frc/robot/Mechanisms.java | 25 ------ .../src/main/java/frc/robot/Robot.java | 21 ++++- .../main/java/frc/robot/sim/PhysicsSim.java | 86 +++++++++++++++++++ .../java/frc/robot/sim/TalonFXSimProfile.java | 65 ++++++++++++++ .../src/main/java/frc/robot/Robot.java | 6 +- .../java/frc/robot/sim/TalonFXSimProfile.java | 16 ++-- .../java/frc/robot/sim/TalonFXSimProfile.java | 19 ++-- 11 files changed, 201 insertions(+), 86 deletions(-) create mode 100644 java/Pigeon2/src/main/java/frc/robot/sim/PhysicsSim.java create mode 100644 java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java diff --git a/java/CANcoder/src/main/java/frc/robot/Robot.java b/java/CANcoder/src/main/java/frc/robot/Robot.java index 9c3a399a..cffcf8c5 100644 --- a/java/CANcoder/src/main/java/frc/robot/Robot.java +++ b/java/CANcoder/src/main/java/frc/robot/Robot.java @@ -29,7 +29,7 @@ public class Robot extends TimedRobot { private final DutyCycleOut fwdOut = new DutyCycleOut(0); private final XboxController controller = new XboxController(0); - double currentTime = Timer.getFPGATimestamp(); + private double currentTime = Timer.getFPGATimestamp(); private final Mechanisms mechanism = new Mechanisms(); diff --git a/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java index 0ad1ddb6..8f58c88f 100644 --- a/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.sim.PhysicsSim.SimProfile; @@ -21,11 +20,10 @@ class TalonFXSimProfile extends SimProfile { private final DCMotorSim _motorSim; private final CANcoderSimState _cancoderSim; // We need a sim state in order to change the values of CANcoder - private final XboxController m_controller = new XboxController(0); /** * Creates a new simulation profile for a TalonFX device. - * + * * @param falcon * The TalonFX device * @param accelToFullTime @@ -46,7 +44,7 @@ public TalonFXSimProfile(final TalonFX falcon, final CANcoder canCoder, final do /** * Runs the simulation profile. - * + * * This uses very rudimentary physics simulation and exists to allow users to * test features of our products in simulation using our examples out of the * box. Users may modify this to utilize more accurate physics simulation. @@ -58,24 +56,18 @@ public void run() { _motorSim.update(getPeriod()); // SET SIM PHYSICS INPUTS - double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); + double position = _motorSim.getAngularPositionRotations(); + double velocity = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); - _falcon.getSimState().setRawRotorPosition(_motorSim.getAngularPositionRotations()); - _falcon.getSimState().setRotorVelocity(velocity_rps); + _falcon.getSimState().setRawRotorPosition(position); + _falcon.getSimState().setRotorVelocity(velocity); _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); - _canCoder.getSimState().setRawPosition(_motorSim.getAngularPositionRotations()/_gearRatio); - _canCoder.getSimState().setVelocity(velocity_rps/_gearRatio); + _canCoder.getSimState().setRawPosition(_motorSim.getAngularPositionRotations() / _gearRatio); + _canCoder.getSimState().setVelocity(velocity / _gearRatio); - double Yaxis = m_controller.getLeftY(); - double motorVoltage = Yaxis * 12; // Scales joystick axcis to motor voltage ( +-12v) - _motorSim.setInputVoltage(motorVoltage); - _motorSim.update(.02); - double position = _motorSim.getAngularPositionRotations(); - double velocity = _motorSim.getAngularVelocityRPM()/60; _cancoderSim.setRawPosition(position); _cancoderSim.setVelocity(velocity); - } } \ No newline at end of file diff --git a/java/FusedCANcoder/src/main/java/frc/robot/Robot.java b/java/FusedCANcoder/src/main/java/frc/robot/Robot.java index c4804e47..48abb170 100644 --- a/java/FusedCANcoder/src/main/java/frc/robot/Robot.java +++ b/java/FusedCANcoder/src/main/java/frc/robot/Robot.java @@ -138,7 +138,6 @@ public void testPeriodic() {} @Override public void simulationInit() { PhysicsSim.getInstance().addTalonFX(m_fx, m_cc, 25, 0.001); - cc_pos.setUpdateFrequency(10); } @Override diff --git a/java/MotionMagic/src/main/java/frc/robot/Mechanisms.java b/java/MotionMagic/src/main/java/frc/robot/Mechanisms.java index 0e434cf8..15b441ef 100644 --- a/java/MotionMagic/src/main/java/frc/robot/Mechanisms.java +++ b/java/MotionMagic/src/main/java/frc/robot/Mechanisms.java @@ -14,22 +14,22 @@ public class Mechanisms { double HEIGHT = 1; // Controls the height of the mech2d SmartDashboard double WIDTH = 1; // Controls the height of the mech2d SmartDashboard - + Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); // Velocity MechanismLigament2d VelocityMech = mech. getRoot("velocityLineReferencePosition", 0.75, 0.5). append(new MechanismLigament2d("velocityLine", 1,90, 6, new Color8Bit(Color.kAliceBlue))); - + MechanismLigament2d midline = mech. getRoot("midline", 0.7, 0.5). append(new MechanismLigament2d("midline", 0.1, 0, 3, new Color8Bit(Color.kCyan))); - - //Position + + //Position MechanismLigament2d arm = mech. getRoot("pivotPoint", 0.25, 0.5). append(new MechanismLigament2d("arm", .2, 0, 0, new Color8Bit(Color.kAliceBlue))); - + MechanismLigament2d side1 = arm.append(new MechanismLigament2d("side1", 0.15307, 112.5, 6, new Color8Bit(Color.kAliceBlue))); MechanismLigament2d side2 = side1.append(new MechanismLigament2d("side2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); MechanismLigament2d side3 = side2.append(new MechanismLigament2d("side3", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); @@ -41,14 +41,14 @@ public class Mechanisms { /** * Runs the mech2d widget in GUI. - * - * This utilizes GUI to simulate and display a TalonFX and exists to allow users to test and understand - * features of our products in simulation using our examples out of the box. Users may modify to have a + * + * This utilizes GUI to simulate and display a TalonFX and exists to allow users to test and understand + * features of our products in simulation using our examples out of the box. Users may modify to have a * display interface that they find more intuitive or visually appealing. - */ - public void update(StatusSignal position, StatusSignal velocity) { + */ + public void update(StatusSignal position, StatusSignal velocity) { VelocityMech.setLength(velocity.getValue()/120); // Divide by 120 to scale motion to fit in the window arm.setAngle(position.getValue() * 360); SmartDashboard.putData("mech2d", mech); // Creates mech2d in SmartDashboard - } + } } diff --git a/java/Pigeon2/src/main/java/frc/robot/Mechanisms.java b/java/Pigeon2/src/main/java/frc/robot/Mechanisms.java index 23316674..8eb6100d 100644 --- a/java/Pigeon2/src/main/java/frc/robot/Mechanisms.java +++ b/java/Pigeon2/src/main/java/frc/robot/Mechanisms.java @@ -1,12 +1,7 @@ package frc.robot; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.sim.Pigeon2SimState; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -22,9 +17,6 @@ public class Mechanisms { private final double ROOT_X = WIDTH / 2; private final double ROOT_Y = HEIGHT / 2; - private final Pigeon2SimState pidgeySim; // We need a sim state in order to change the values of pidgey - private final DCMotorSim motorSim = new DCMotorSim(DCMotor.getFalcon500(1), 100, 100); - private final XboxController controller = new XboxController(0); // Uses an Xbox controller for setting the CANcoder simulation private Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); // Main mechanism object private MechanismLigament2d wrist = mech. getRoot("base", ROOT_X, ROOT_Y). @@ -35,25 +27,8 @@ public class Mechanisms { @SuppressWarnings("unused") private MechanismLigament2d rightArrow = wrist.append(new MechanismLigament2d("RightArrow", 0.1, -150, 6, new Color8Bit(Color.kAliceBlue))); - public Mechanisms(Pigeon2 pigeon) { - pidgeySim = pigeon.getSimState(); - } - public void update(StatusSignal position) { wrist.setAngle(position.getValue()); SmartDashboard.putData("mech2d", mech); } - - /** - * Didn't want to move in our full physics class, since this is just for pigeon - * Call this periodically in the simulation periodic method - */ - public void simUpdate(){ - double Yaxis = controller.getLeftY(); - double motorVoltage = Yaxis * 12; // scales joystick axis to motor voltage ( +-12v) - motorSim.setInputVoltage(motorVoltage); - motorSim.update(.02); - double position = motorSim.getAngularPositionRotations()*360; - pidgeySim.setRawYaw(position); - } } diff --git a/java/Pigeon2/src/main/java/frc/robot/Robot.java b/java/Pigeon2/src/main/java/frc/robot/Robot.java index a0057f57..0d1acc0e 100644 --- a/java/Pigeon2/src/main/java/frc/robot/Robot.java +++ b/java/Pigeon2/src/main/java/frc/robot/Robot.java @@ -5,10 +5,14 @@ package frc.robot; import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.XboxController; +import frc.robot.sim.PhysicsSim; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -19,10 +23,15 @@ public class Robot extends TimedRobot { private final double PRINT_PERIOD = 0.5; // Update every 500 ms + /* Keep a reference for a TalonFX around so we can drive the thing the Pigeon is on */ + private final TalonFX talonfx = new TalonFX(0, "rio"); private final Pigeon2 pidgey = new Pigeon2(1, "rio"); private double currentTime = Timer.getFPGATimestamp(); - private final Mechanisms mechanisms = new Mechanisms(pidgey); + private final XboxController joystick = new XboxController(0); + private final DutyCycleOut control = new DutyCycleOut(0); + + private final Mechanisms mechanisms = new Mechanisms(); /** * This function is run when the robot is first started up and should be used for any @@ -101,7 +110,9 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + talonfx.setControl(control.withOutput(joystick.getLeftY())); + } @Override public void disabledInit() {} @@ -116,10 +127,12 @@ public void testInit() {} public void testPeriodic() {} @Override - public void simulationInit() {} + public void simulationInit() { + PhysicsSim.getInstance().addTalonFX(talonfx, pidgey, 0.001); + } @Override public void simulationPeriodic() { - mechanisms.simUpdate(); + PhysicsSim.getInstance().run(); } } diff --git a/java/Pigeon2/src/main/java/frc/robot/sim/PhysicsSim.java b/java/Pigeon2/src/main/java/frc/robot/sim/PhysicsSim.java new file mode 100644 index 00000000..26eb9102 --- /dev/null +++ b/java/Pigeon2/src/main/java/frc/robot/sim/PhysicsSim.java @@ -0,0 +1,86 @@ +package frc.robot.sim; + +import java.util.ArrayList; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.hardware.TalonFX; + +/** + * Manages physics simulation for CTRE products. + */ +public class PhysicsSim { + private static final PhysicsSim sim = new PhysicsSim(); + + /** + * Gets the robot simulator instance. + */ + public static PhysicsSim getInstance() { + return sim; + } + + /** + * Adds a TalonFX controller to the simulator. + * + * @param falcon + * The TalonFX device + * @param can + * The CANcoder device + * @param gearRatio + * The gear reduction of the TalonFX + * @param rotorInertia + * Rotational Inertia of the mechanism at the rotor + */ + public void addTalonFX(TalonFX falcon, Pigeon2 pigeon, final double rotorInertia) { + if (falcon != null) { + TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, pigeon, rotorInertia); + _simProfiles.add(simFalcon); + } + } + + /** + * Runs the simulator: + * - enable the robot + * - simulate sensors + */ + public void run() { + // Simulate devices + for (SimProfile simProfile : _simProfiles) { + simProfile.run(); + } + } + + private final ArrayList _simProfiles = new ArrayList(); + + /** + * Holds information about a simulated device. + */ + static class SimProfile { + private double _lastTime; + private boolean _running = false; + + /** + * Runs the simulation profile. + * Implemented by device-specific profiles. + */ + public void run() { + } + + /** + * Returns the time since last call, in seconds. + */ + protected double getPeriod() { + // set the start time if not yet running + if (!_running) { + _lastTime = Utils.getCurrentTimeSeconds(); + _running = true; + } + + double now = Utils.getCurrentTimeSeconds(); + final double period = now - _lastTime; + _lastTime = now; + + return period; + } + } +} \ No newline at end of file diff --git a/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java new file mode 100644 index 00000000..452e5a83 --- /dev/null +++ b/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -0,0 +1,65 @@ +package frc.robot.sim; + +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.sim.PhysicsSim.SimProfile; + +/** + * Holds information about a simulated TalonFX. + */ +class TalonFXSimProfile extends SimProfile { + private final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation + private final TalonFX _falcon; + private final Pigeon2 _pigeon; + + private final DCMotorSim _motorSim; + + /** + * Creates a new simulation profile for a TalonFX device. + * + * @param falcon + * The TalonFX device + * @param accelToFullTime + * The time the motor takes to accelerate from 0 to full, + * in seconds + * @param fullVel + * The maximum motor velocity, in rotations per second + * @param sensorPhase + * The phase of the TalonFX sensors + */ + public TalonFXSimProfile(final TalonFX falcon, final Pigeon2 pigeon, final double rotorInertia) { + this._falcon = falcon; + this._pigeon = pigeon; + this._motorSim = new DCMotorSim(DCMotor.getFalcon500(1), 1.0, rotorInertia); + } + + /** + * Runs the simulation profile. + * + * This uses very rudimentary physics simulation and exists to allow users to + * test features of our products in simulation using our examples out of the + * box. Users may modify this to utilize more accurate physics simulation. + */ + public void run() { + /// DEVICE SPEED SIMULATION + + _motorSim.setInputVoltage(_falcon.getSimState().getMotorVoltage()); + + _motorSim.update(getPeriod()); + + /// SET SIM PHYSICS INPUTS + double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); + + _falcon.getSimState().setRawRotorPosition(_motorSim.getAngularPositionRotations()); + _falcon.getSimState().setRotorVelocity(velocity_rps); + + _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); + + _pigeon.getSimState().setRawYaw(_motorSim.getAngularPositionRotations()); + + } +} \ No newline at end of file diff --git a/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java b/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java index ea5d3ae9..653f9349 100644 --- a/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java +++ b/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java @@ -43,8 +43,8 @@ public class Robot extends TimedRobot { @Override public void robotInit() { TalonFXConfiguration configs = new TalonFXConfiguration(); - configs.Slot0.kP = 2; // An error of 0.5 rotations results in 12V output - configs.Slot0.kD = .1; // A change of 1 rotation per second results in 0.1 volts output + configs.Slot0.kP = 2.4; // An error of 0.5 rotations results in 1.2 volts output + configs.Slot0.kD = 0.1; // A change of 1 rotation per second results in 0.1 volts output // Peak output of 8 volts configs.Voltage.PeakForwardVoltage = 8; configs.Voltage.PeakReverseVoltage = -8; @@ -86,7 +86,7 @@ public void teleopInit() {} @Override public void teleopPeriodic() { double desiredRotations = m_joystick.getLeftY() * 10; // Go for plus/minus 10 rotations - if (Math.abs(desiredRotations) <= 0.5) { // Joystick deadzone + if (Math.abs(desiredRotations) <= 0.1) { // Joystick deadzone desiredRotations = 0; } if (m_joystick.getLeftBumper()) { diff --git a/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java index f60e64b3..b53a05d9 100644 --- a/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -13,7 +13,6 @@ */ class TalonFXSimProfile extends SimProfile { private final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation - private final TalonFX _falcon; private final DCMotorSim _motorSim; private final TalonFXSimState _falconSim; @@ -32,9 +31,8 @@ class TalonFXSimProfile extends SimProfile { * The phase of the TalonFX sensors */ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { - this._falcon = falcon; this._motorSim = new DCMotorSim(DCMotor.getFalcon500(1), 1.0, rotorInertia); - this._falconSim = _falcon.getSimState(); + this._falconSim = falcon.getSimState(); } /** @@ -47,20 +45,16 @@ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { public void run() { /// DEVICE SPEED SIMULATION - _motorSim.setInputVoltage(_falcon.getSimState().getMotorVoltage()); + _motorSim.setInputVoltage(_falconSim.getMotorVoltage()); _motorSim.update(getPeriod()); /// SET SIM PHYSICS INPUTS double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); - _falcon.getSimState().setRawRotorPosition(_motorSim.getAngularPositionRotations()); - _falcon.getSimState().setRotorVelocity(velocity_rps); + _falconSim.setRawRotorPosition(_motorSim.getAngularPositionRotations()); + _falconSim.setRotorVelocity(velocity_rps); - _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); - - ///GUI SIM - double rotorPosition = _motorSim.getAngularPositionRotations(); - _falconSim.setRawRotorPosition(rotorPosition); + _falconSim.setSupplyVoltage(12 - _falconSim.getSupplyCurrent() * kMotorResistance); } } \ No newline at end of file diff --git a/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java index db252bc7..9c73a39d 100644 --- a/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -13,7 +13,6 @@ */ class TalonFXSimProfile extends SimProfile { private final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation - private final TalonFX _falcon; private final TalonFXSimState _falconSim; private final DCMotorSim _motorSim; @@ -31,9 +30,8 @@ class TalonFXSimProfile extends SimProfile { * The phase of the TalonFX sensors */ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { - this._falcon = new TalonFX(0); this._motorSim = new DCMotorSim(DCMotor.getFalcon500(1), 1.0, rotorInertia); - this._falconSim = _falcon.getSimState(); + this._falconSim = falcon.getSimState(); } /** @@ -46,23 +44,16 @@ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { public void run() { /// DEVICE SPEED SIMULATION - _motorSim.setInputVoltage(_falcon.getSimState().getMotorVoltage()); + _motorSim.setInputVoltage(_falconSim.getMotorVoltage()); _motorSim.update(getPeriod()); /// SET SIM PHYSICS INPUTS double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); - _falcon.getSimState().setRawRotorPosition(_motorSim.getAngularPositionRotations()); - _falcon.getSimState().setRotorVelocity(velocity_rps); + _falconSim.setRawRotorPosition(_motorSim.getAngularPositionRotations()); + _falconSim.setRotorVelocity(velocity_rps); - _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); - - /// GUI SIM - double rotorPosition = _motorSim.getAngularPositionRotations() / 10; //Gear reduction by 10:1 so rotation is visible in sim - _falconSim.setRawRotorPosition(rotorPosition); - - double rotorVelocity = _motorSim.getAngularVelocityRadPerSec(); - _falconSim.setRotorVelocity(rotorVelocity); + _falconSim.setSupplyVoltage(12 - _falconSim.getSupplyCurrent() * kMotorResistance); } } \ No newline at end of file