diff --git a/cpp/CurrentLimits/src/main/cpp/Robot.cpp b/cpp/CurrentLimits/src/main/cpp/Robot.cpp index 171bf5ff..fdb97306 100644 --- a/cpp/CurrentLimits/src/main/cpp/Robot.cpp +++ b/cpp/CurrentLimits/src/main/cpp/Robot.cpp @@ -8,14 +8,15 @@ using namespace ctre::phoenix6; void Robot::RobotInit() { - /* Configure the Talon to use a supply limit of 60 amps IF we exceed 80 amps for over 0.1 seconds */ + /* Configure the Talon to use a supply limit of 70 A, and + * lower to 40 A if we're at 70 A for over 1 second */ configs::TalonFXConfiguration toConfigure{}; m_currentLimits.SupplyCurrentLimit = 70_A; // Default limit of 70 A m_currentLimits.SupplyCurrentLowerLimit = 40_A; // Reduce the limit to 40 A if we've limited to 70 A... m_currentLimits.SupplyCurrentLowerTime = 1_s; // ...for at least 1 second m_currentLimits.SupplyCurrentLimitEnable = true; // And enable it - m_currentLimits.StatorCurrentLimit = 120_A; // Limit stator to 120 amps + m_currentLimits.StatorCurrentLimit = 120_A; // Limit stator to 120 A m_currentLimits.StatorCurrentLimitEnable = true; // And enable it toConfigure.CurrentLimits = m_currentLimits; diff --git a/cpp/MotionMagic/src/main/cpp/sim/PhysicsSim.cpp b/cpp/MotionMagic/src/main/cpp/sim/PhysicsSim.cpp index 4b95f327..9dbfded4 100644 --- a/cpp/MotionMagic/src/main/cpp/sim/PhysicsSim.cpp +++ b/cpp/MotionMagic/src/main/cpp/sim/PhysicsSim.cpp @@ -3,9 +3,9 @@ using namespace ctre::phoenix6; -void PhysicsSim::AddTalonFX(hardware::TalonFX& falcon, units::kilogram_square_meter_t rotorInertia) { - TalonFXSimProfile *simFalcon = new TalonFXSimProfile(falcon, rotorInertia); - _simProfiles.push_back(simFalcon); +void PhysicsSim::AddTalonFX(hardware::TalonFX& talonFX, units::kilogram_square_meter_t rotorInertia) { + TalonFXSimProfile *simTalonFX = new TalonFXSimProfile(talonFX, rotorInertia); + _simProfiles.push_back(simTalonFX); } void PhysicsSim::Run() { diff --git a/cpp/MotionMagic/src/main/cpp/sim/TalonFXSimProfile.cpp b/cpp/MotionMagic/src/main/cpp/sim/TalonFXSimProfile.cpp index 09626611..39e3f480 100644 --- a/cpp/MotionMagic/src/main/cpp/sim/TalonFXSimProfile.cpp +++ b/cpp/MotionMagic/src/main/cpp/sim/TalonFXSimProfile.cpp @@ -3,15 +3,15 @@ using namespace ctre::phoenix6; -TalonFXSimProfile::TalonFXSimProfile(hardware::TalonFX& falcon, units::kilogram_square_meter_t rotorInertia) : +TalonFXSimProfile::TalonFXSimProfile(hardware::TalonFX& talonFX, units::kilogram_square_meter_t rotorInertia) : _motorSim{frc::LinearSystemId::DCMotorSystem(frc::DCMotor::KrakenX60FOC(1), rotorInertia, 1), frc::DCMotor::KrakenX60FOC(1)}, - _falcon{falcon} { -} + _talonFXSim{talonFX.GetSimState()} +{} void TalonFXSimProfile::Run() { /// DEVICE SPEED SIMULATION - _motorSim.SetInputVoltage(_falcon.GetSimState().GetMotorVoltage()); + _motorSim.SetInputVoltage(_talonFXSim.GetMotorVoltage()); _motorSim.Update(GetPeriod()); @@ -19,8 +19,8 @@ void TalonFXSimProfile::Run() { auto const position = _motorSim.GetAngularPosition(); auto const velocity = _motorSim.GetAngularVelocity(); - _falcon.GetSimState().SetRawRotorPosition(position); - _falcon.GetSimState().SetRotorVelocity(velocity); + _talonFXSim.SetRawRotorPosition(position); + _talonFXSim.SetRotorVelocity(velocity); - _falcon.GetSimState().SetSupplyVoltage(12_V - _falcon.GetSimState().GetSupplyCurrent() * kMotorResistance); + _talonFXSim.SetSupplyVoltage(12_V - _talonFXSim.GetSupplyCurrent() * kMotorResistance); } \ No newline at end of file diff --git a/cpp/MotionMagic/src/main/include/sim/PhysicsSim.h b/cpp/MotionMagic/src/main/include/sim/PhysicsSim.h index 33e25a3d..ea16e760 100644 --- a/cpp/MotionMagic/src/main/include/sim/PhysicsSim.h +++ b/cpp/MotionMagic/src/main/include/sim/PhysicsSim.h @@ -25,12 +25,12 @@ class PhysicsSim { /** * Adds a TalonFX controller to the simulator. * - * @param falcon + * @param talonFX * The TalonFX device * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - void AddTalonFX(ctre::phoenix6::hardware::TalonFX& falcon, units::kilogram_square_meter_t rotorInertia); + void AddTalonFX(ctre::phoenix6::hardware::TalonFX& talonFX, units::kilogram_square_meter_t rotorInertia); /** * Runs the simulator: diff --git a/cpp/MotionMagic/src/main/include/sim/TalonFXSimProfile.h b/cpp/MotionMagic/src/main/include/sim/TalonFXSimProfile.h index 6019724f..28b19305 100644 --- a/cpp/MotionMagic/src/main/include/sim/TalonFXSimProfile.h +++ b/cpp/MotionMagic/src/main/include/sim/TalonFXSimProfile.h @@ -11,19 +11,19 @@ class TalonFXSimProfile : public SimProfile { static constexpr units::ohm_t kMotorResistance = 2_mOhm; // Assume 2mOhm resistance for voltage drop calculation frc::sim::DCMotorSim _motorSim; - ctre::phoenix6::hardware::TalonFX& _falcon; + ctre::phoenix6::sim::TalonFXSimState& _talonFXSim; public: /** * Creates a new simulation profile for a TalonFX device. * - * @param falcon + * @param talonFX * The TalonFX device * @param rotorInertia * Rotational inertia at the Rotor */ - TalonFXSimProfile(ctre::phoenix6::hardware::TalonFX& falcon, units::kilogram_square_meter_t rotorInertia); + TalonFXSimProfile(ctre::phoenix6::hardware::TalonFX& talonFX, units::kilogram_square_meter_t rotorInertia); /** * Runs the simulation profile. diff --git a/cpp/Simulation/src/main/include/Robot.h b/cpp/Simulation/src/main/include/Robot.h index 7fc0cc03..ff6c5cdf 100644 --- a/cpp/Simulation/src/main/include/Robot.h +++ b/cpp/Simulation/src/main/include/Robot.h @@ -44,7 +44,7 @@ class Robot : public frc::TimedRobot { static constexpr units::inch_t kWheelRadiusInches = 3_in; frc::sim::DifferentialDrivetrainSim m_driveSim{ - frc::DCMotor::Falcon500FOC(2), + frc::DCMotor::KrakenX60FOC(2), kGearRatio, 2.1_kg_sq_m, // MOI of 2.1 kg m^2 (from CAD model) 26.5_kg, // Mass of robot is 26.5 kg diff --git a/cpp/WaitForAll/src/main/cpp/Robot.cpp b/cpp/WaitForAll/src/main/cpp/Robot.cpp index 70611aa1..09c0bf24 100644 --- a/cpp/WaitForAll/src/main/cpp/Robot.cpp +++ b/cpp/WaitForAll/src/main/cpp/Robot.cpp @@ -20,7 +20,7 @@ void Robot::RobotPeriodic() { if (m_joystick.GetAButtonPressed()) { ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, m_lotsOfSignals); std::cout << "Status of waiting on signals (normal use case): " << status.GetName() << std::endl; - for(auto const &sig : m_lotsOfSignals) { + for (auto const &sig : m_lotsOfSignals) { std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; } } @@ -29,7 +29,7 @@ void Robot::RobotPeriodic() { if (m_joystick.GetBButtonPressed()) { ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, m_signalsAcrossCANbuses); std::cout << "Status of waiting on signals across different CAN busses: " << status.GetName() << std::endl; - for(auto const& sig : m_signalsAcrossCANbuses) { + for (auto const& sig : m_signalsAcrossCANbuses) { std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; } } @@ -38,7 +38,7 @@ void Robot::RobotPeriodic() { if (m_joystick.GetYButtonPressed()) { ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, m_noSignals); std::cout << "Status of waiting on no signals: " << status.GetName() << std::endl; - for(auto const& sig : m_noSignals) { + for (auto const& sig : m_noSignals) { std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; } } diff --git a/java/ArcadeDrive/src/main/java/frc/robot/Robot.java b/java/ArcadeDrive/src/main/java/frc/robot/Robot.java index a6f49395..53def4ba 100644 --- a/java/ArcadeDrive/src/main/java/frc/robot/Robot.java +++ b/java/ArcadeDrive/src/main/java/frc/robot/Robot.java @@ -4,7 +4,7 @@ package frc.robot; -import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; @@ -21,11 +21,12 @@ * project. */ public class Robot extends TimedRobot { - private static final String CANBUS_NAME = "canivore"; - private final TalonFX leftLeader = new TalonFX(1, CANBUS_NAME); - private final TalonFX leftFollower = new TalonFX(2, CANBUS_NAME); - private final TalonFX rightLeader = new TalonFX(3, CANBUS_NAME); - private final TalonFX rightFollower = new TalonFX(4, CANBUS_NAME); + private final CANBus kCANBus = new CANBus("canivore"); + + private final TalonFX leftLeader = new TalonFX(1, kCANBus); + private final TalonFX leftFollower = new TalonFX(2, kCANBus); + private final TalonFX rightLeader = new TalonFX(3, kCANBus); + private final TalonFX rightFollower = new TalonFX(4, kCANBus); private final DutyCycleOut leftOut = new DutyCycleOut(0); private final DutyCycleOut rightOut = new DutyCycleOut(0); diff --git a/java/BasicLatencyCompensation/src/main/java/frc/robot/Robot.java b/java/BasicLatencyCompensation/src/main/java/frc/robot/Robot.java index e3721f07..111be140 100644 --- a/java/BasicLatencyCompensation/src/main/java/frc/robot/Robot.java +++ b/java/BasicLatencyCompensation/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.CANcoder; @@ -25,22 +26,24 @@ * project. */ public class Robot extends TimedRobot { - private static final String CANBUS_NAME = "canivore"; - private final CANcoder m_cc = new CANcoder(0, CANBUS_NAME); - private final TalonFX m_fx = new TalonFX(0, CANBUS_NAME); - private final Pigeon2 m_p2 = new Pigeon2(0, CANBUS_NAME); + private final CANBus kCANBus = new CANBus("canivore"); + + private final CANcoder m_cc = new CANcoder(0, kCANBus); + private final TalonFX m_fx = new TalonFX(0, kCANBus); + private final Pigeon2 m_p2 = new Pigeon2(0, kCANBus); + private int m_printCount = 0; private final DutyCycleOut m_dutycycle = new DutyCycleOut(0); private final XboxController m_joystick = new XboxController(0); - private final StatusSignal m_ccpos = m_cc.getPosition(); - private final StatusSignal m_fxpos = m_fx.getPosition(); - private final StatusSignal m_p2yaw = m_p2.getYaw(); - private final StatusSignal m_ccvel = m_cc.getVelocity(); - private final StatusSignal m_fxvel = m_fx.getVelocity(); - private final StatusSignal m_p2yawRate = m_p2.getAngularVelocityZWorld(); + private final StatusSignal m_ccpos = m_cc.getPosition(false); + private final StatusSignal m_fxpos = m_fx.getPosition(false); + private final StatusSignal m_p2yaw = m_p2.getYaw(false); + private final StatusSignal m_ccvel = m_cc.getVelocity(false); + private final StatusSignal m_fxvel = m_fx.getVelocity(false); + private final StatusSignal m_p2yawRate = m_p2.getAngularVelocityZWorld(false); /** * This function is run when the robot is first started up and should be used for any @@ -64,23 +67,23 @@ public void robotPeriodic() { var p2CompensatedYaw = BaseStatusSignal.getLatencyCompensatedValue(m_p2yaw, m_p2yawRate); /* Print out both values so it shows how they perform */ - if (m_printCount++ > 10 && m_joystick.getAButton()) { + if (++m_printCount >= 10 && m_joystick.getAButton()) { m_printCount = 0; System.out.printf( "CANcoder: Pos: %10.3f - Latency-Compensated: %10.3f - Difference: %6.5f%n", - m_ccpos.getValue().in(Rotations), - ccCompensatedPos.in(Rotations), + m_ccpos.getValue().in(Rotations), + ccCompensatedPos.in(Rotations), ccCompensatedPos.minus(m_ccpos.getValue()).in(Rotations) ); System.out.printf( "Talon FX: Pos: %10.3f - Latency-Compensated: %10.3f - Difference: %6.5f%n", - m_fxpos.getValue().in(Rotations), fxCompensatedPos.in(Rotations), + m_fxpos.getValue().in(Rotations), fxCompensatedPos.in(Rotations), fxCompensatedPos.minus(m_fxpos.getValue()).in(Rotations) ); System.out.printf( "Pigeon2 : Yaw: %10.3f - Latency-Compensated: %10.3f - Difference: %6.5f%n", - m_p2yaw.getValue().in(Degrees), - p2CompensatedYaw.in(Degrees), + m_p2yaw.getValue().in(Degrees), + p2CompensatedYaw.in(Degrees), p2CompensatedYaw.minus(m_p2yaw.getValue()).in(Degrees) ); System.out.println(); diff --git a/java/CANcoder/src/main/java/frc/robot/Robot.java b/java/CANcoder/src/main/java/frc/robot/Robot.java index e7fdb966..1124b65d 100644 --- a/java/CANcoder/src/main/java/frc/robot/Robot.java +++ b/java/CANcoder/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.CANcoder; @@ -26,9 +27,11 @@ public class Robot extends TimedRobot { private static final double PRINT_PERIOD = 0.5; // Update every 500 ms - private static final String canBusName = "rio"; - private final TalonFX talonFX = new TalonFX(2, canBusName); - private final CANcoder cancoder = new CANcoder(1, canBusName); + private final CANBus kCANBus = new CANBus("rio"); + + private final TalonFX talonFX = new TalonFX(2, kCANBus); + private final CANcoder cancoder = new CANcoder(1, kCANBus); + private final DutyCycleOut fwdOut = new DutyCycleOut(0); private final XboxController controller = new XboxController(0); @@ -67,19 +70,21 @@ public void robotPeriodic() { System.out.println("Position is " + pos.toString() + " with " + pos.getTimestamp().getLatency() + " seconds of latency"); /** - * Get the velocity StatusSignalValue + * Get the velocity StatusSignalValue without refreshing */ - var vel = cancoder.getVelocity(); + var vel = cancoder.getVelocity(false); /* This time wait for the signal to reduce latency */ vel.waitForUpdate(PRINT_PERIOD); // Wait up to our period /** * This uses the explicit getValue and getUnits functions to print, even though it's not * necessary for the ostream print */ - System.out.println("Velocity is " + - vel.getValue() + " " + - vel.getUnits() + " with " + - vel.getTimestamp().getLatency() + " seconds of latency"); + System.out.println( + "Velocity is " + + vel.getValue() + " " + + vel.getUnits() + " with " + + vel.getTimestamp().getLatency() + " seconds of latency" + ); /** * Notice when running this example that the second print's latency is always shorter than the first print's latency. * This is because we explicitly wait for the signal using the waitForUpdate() method instead of using the refresh() diff --git a/java/CANcoder/src/main/java/frc/robot/sim/PhysicsSim.java b/java/CANcoder/src/main/java/frc/robot/sim/PhysicsSim.java index 09c642cf..4d981cd7 100644 --- a/java/CANcoder/src/main/java/frc/robot/sim/PhysicsSim.java +++ b/java/CANcoder/src/main/java/frc/robot/sim/PhysicsSim.java @@ -22,7 +22,7 @@ public static PhysicsSim getInstance() { /** * Adds a TalonFX controller to the simulator. * - * @param falcon + * @param talonFX * The TalonFX device * @param can * The CANcoder device @@ -31,10 +31,10 @@ public static PhysicsSim getInstance() { * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public void addTalonFX(TalonFX falcon, CANcoder can, double gearRatio, final double rotorInertia) { - if (falcon != null) { - TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, can, gearRatio, rotorInertia); - _simProfiles.add(simFalcon); + public void addTalonFX(TalonFX talonFX, CANcoder can, double gearRatio, final double rotorInertia) { + if (talonFX != null) { + TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, can, gearRatio, rotorInertia); + _simProfiles.add(simTalonFX); } } 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 f85331b9..5b18c3ed 100644 --- a/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -14,7 +15,7 @@ */ class TalonFXSimProfile extends SimProfile { private static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation - private final TalonFX _falcon; + private final TalonFXSimState _talonFXSim; private final CANcoder _canCoder; private final double _gearRatio; @@ -23,7 +24,7 @@ class TalonFXSimProfile extends SimProfile { /** * Creates a new simulation profile for a TalonFX device. * - * @param falcon + * @param talonFX * The TalonFX device * @param canCoder * The CANcoder associated with the TalonFX @@ -32,8 +33,8 @@ class TalonFXSimProfile extends SimProfile { * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public TalonFXSimProfile(final TalonFX falcon, final CANcoder canCoder, final double gearRatio, final double rotorInertia) { - this._falcon = falcon; + public TalonFXSimProfile(final TalonFX talonFX, final CANcoder canCoder, final double gearRatio, final double rotorInertia) { + this._talonFXSim = talonFX.getSimState(); this._canCoder = canCoder; this._gearRatio = gearRatio; @@ -50,7 +51,7 @@ public TalonFXSimProfile(final TalonFX falcon, final CANcoder canCoder, final do */ public void run() { // DEVICE SPEED SIMULATION - _motorSim.setInputVoltage(_falcon.getSimState().getMotorVoltage()); + _motorSim.setInputVoltage(_talonFXSim.getMotorVoltage()); _motorSim.update(getPeriod()); @@ -58,10 +59,10 @@ public void run() { final double position_rot = _motorSim.getAngularPositionRotations() * _gearRatio; final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()) * _gearRatio; - _falcon.getSimState().setRawRotorPosition(position_rot); - _falcon.getSimState().setRotorVelocity(velocity_rps); + _talonFXSim.setRawRotorPosition(position_rot); + _talonFXSim.setRotorVelocity(velocity_rps); - _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); + _talonFXSim.setSupplyVoltage(12 - _talonFXSim.getSupplyCurrent() * kMotorResistance); _canCoder.getSimState().setRawPosition(position_rot / _gearRatio); _canCoder.getSimState().setVelocity(velocity_rps / _gearRatio); diff --git a/java/CommandBasedDrive/src/main/java/frc/robot/RobotContainer.java b/java/CommandBasedDrive/src/main/java/frc/robot/RobotContainer.java index 84d7afaa..36a3f98e 100644 --- a/java/CommandBasedDrive/src/main/java/frc/robot/RobotContainer.java +++ b/java/CommandBasedDrive/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,6 @@ import frc.robot.commands.DriveStraightCommand; import frc.robot.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; /** @@ -22,23 +21,20 @@ */ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + public final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); private final CommandXboxController m_joystick = new CommandXboxController(0); - /* invert the joystick Y because forward Y is negative */ - private final Command m_teleopDrive = new RunCommand(() -> { - m_driveSubsystem.arcadeDrive(-m_joystick.getLeftY(), m_joystick.getRightX()); - }, - m_driveSubsystem); - - private final Command m_driveStraight = new DriveStraightCommand(m_driveSubsystem, () -> -m_joystick.getLeftY()); - /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - m_driveSubsystem.setDefaultCommand(m_teleopDrive); + m_driveSubsystem.setDefaultCommand( + m_driveSubsystem.run(() -> + /* invert the joystick Y because forward Y is negative */ + m_driveSubsystem.arcadeDrive(-m_joystick.getLeftY(), m_joystick.getRightX()) + ) + ); // Configure the button bindings configureButtonBindings(); @@ -54,7 +50,9 @@ public RobotContainer() { */ private void configureButtonBindings() { /* If the upper left shoulder button is pressed, drive straight */ - m_joystick.leftBumper().whileTrue(m_driveStraight); + m_joystick.leftBumper().whileTrue( + new DriveStraightCommand(m_driveSubsystem, () -> -m_joystick.getLeftY()) + ); } /** diff --git a/java/CommandBasedDrive/src/main/java/frc/robot/commands/DriveStraightCommand.java b/java/CommandBasedDrive/src/main/java/frc/robot/commands/DriveStraightCommand.java index 55838fbe..378a3e65 100644 --- a/java/CommandBasedDrive/src/main/java/frc/robot/commands/DriveStraightCommand.java +++ b/java/CommandBasedDrive/src/main/java/frc/robot/commands/DriveStraightCommand.java @@ -12,7 +12,7 @@ import frc.robot.subsystems.DriveSubsystem; public class DriveStraightCommand extends Command { - private final double MAX_UPDATE_PERIOD = 0.05; // Wait up to 50ms + private static final double MAX_UPDATE_PERIOD = 0.05; // Wait up to 50ms private final DoubleSupplier m_throttle; private final DriveSubsystem m_drivebase; private final StatusSignal m_yawGetter; diff --git a/java/CommandBasedDrive/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/java/CommandBasedDrive/src/main/java/frc/robot/subsystems/DriveSubsystem.java index e5acba31..885aa6e9 100644 --- a/java/CommandBasedDrive/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/java/CommandBasedDrive/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.Pigeon2Configurator; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -20,8 +19,6 @@ import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.AngleUnit; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -64,7 +61,7 @@ public class DriveSubsystem extends SubsystemBase { /* Simulation model of the drivetrain */ private final DifferentialDrivetrainSim m_driveSim = new DifferentialDrivetrainSim( - DCMotor.getFalcon500Foc(2), // 2 CIMS on each side of the drivetrain. + DCMotor.getKrakenX60Foc(2), // 2 Kraken X60 on each side of the drivetrain. kGearRatio, // Standard AndyMark Gearing reduction. 2.1, // MOI of 2.1 kg m^2 (from CAD model). 26.5, // Mass of the robot is 26.5 kg. @@ -187,8 +184,10 @@ public void simulationPeriodic() { * WPILib expects +V to be forward. We have already configured * our orientations to match this behavior. */ - m_driveSim.setInputs(m_leftSimState.getMotorVoltage(), - m_rightSimState.getMotorVoltage()); + m_driveSim.setInputs( + m_leftSimState.getMotorVoltage(), + m_rightSimState.getMotorVoltage() + ); /* * Advance the model by 20 ms. Note that if you are running this @@ -313,9 +312,9 @@ private Angle metersToRotations(Distance meters) { private LinearVelocity rotationsToMetersVel(AngularVelocity rotations) { /* Apply gear ratio to input rotations */ - var gearedRotations = rotations.divide(this.kGearRatio); + var gearedRotations = rotations.in(RadiansPerSecond) / this.kGearRatio; /* Then multiply the wheel radius by radians of rotation to get distance */ - return this.kWheelRadius.per(Second).times(gearedRotations.in(RadiansPerSecond)); + return this.kWheelRadius.per(Second).times(gearedRotations); } private AngularVelocity metersToRotationsVel(LinearVelocity meters) { diff --git a/java/ControlRequestLimits/src/main/java/frc/robot/Robot.java b/java/ControlRequestLimits/src/main/java/frc/robot/Robot.java index c28508a6..af64c5c6 100644 --- a/java/ControlRequestLimits/src/main/java/frc/robot/Robot.java +++ b/java/ControlRequestLimits/src/main/java/frc/robot/Robot.java @@ -70,9 +70,11 @@ public void teleopInit() {} @Override public void teleopPeriodic() { - m_fx.setControl(m_out.withOutput(m_joystick.getLeftY()) - .withLimitForwardMotion(m_joystick.getLeftBumper()) - .withLimitReverseMotion(m_joystick.getRightBumper())); + m_fx.setControl( + m_out.withOutput(m_joystick.getLeftY()) + .withLimitForwardMotion(m_joystick.getLeftBumperButton()) + .withLimitReverseMotion(m_joystick.getRightBumperButton()) + ); } @Override diff --git a/java/CurrentLimits/src/main/java/frc/robot/Robot.java b/java/CurrentLimits/src/main/java/frc/robot/Robot.java index f201eb80..6914ff99 100644 --- a/java/CurrentLimits/src/main/java/frc/robot/Robot.java +++ b/java/CurrentLimits/src/main/java/frc/robot/Robot.java @@ -27,24 +27,25 @@ public class Robot extends TimedRobot { private final CurrentLimitsConfigs m_currentLimits = new CurrentLimitsConfigs(); private final XboxController m_joystick = new XboxController(0); - int printCount = 0; + /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override public void robotInit() { - /* Configure the Talon FX to use a supply limit of 60 amps IF we exceed 80 amps for over 0.1 seconds */ + /* Configure the Talon to use a supply limit of 70 A, and + * lower to 40 A if we're at 70 A for over 1 second */ TalonFXConfiguration toConfigure = new TalonFXConfiguration(); - m_currentLimits.SupplyCurrentLowerLimit = 60; // Limit to 60 amps - m_currentLimits.SupplyCurrentLimit = 80; // If we exceed 80 amps - m_currentLimits.SupplyCurrentLowerTime = 0.1; // For at least 0.1 seconds - m_currentLimits.SupplyCurrentLimitEnable = true; // And enable it + m_currentLimits.withSupplyCurrentLowerLimit(Amps.of(70)) // Default limit of 70 A + .withSupplyCurrentLimit(Amps.of(40)) // Reduce the limit to 40 A if we've limited to 70 A... + .withSupplyCurrentLowerTime(Seconds.of(1.0)) // ...for at least 1 second + .withSupplyCurrentLimitEnable(true); // And enable it - m_currentLimits.StatorCurrentLimit = 120; // Limit stator current to 120 amps - m_currentLimits.StatorCurrentLimitEnable = true; // And enable it + m_currentLimits.withStatorCurrentLimit(Amps.of(120)) // Limit stator current to 120 A + .withStatorCurrentLimitEnable(true); // And enable it toConfigure.CurrentLimits = m_currentLimits; @@ -69,8 +70,8 @@ public void robotPeriodic() { m_fx.getConfigurator().apply(m_currentLimits); } - if (printCount++ > 20) { - printCount= 0; + if (++printCount >= 20) { + printCount = 0; System.out.println("Supply current: " + m_fx.getSupplyCurrent()); System.out.println("Stator current: " + m_fx.getStatorCurrent()); } diff --git a/java/FusedCANcoder/src/main/java/frc/robot/Robot.java b/java/FusedCANcoder/src/main/java/frc/robot/Robot.java index c966d160..5dfb56cc 100644 --- a/java/FusedCANcoder/src/main/java/frc/robot/Robot.java +++ b/java/FusedCANcoder/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -15,8 +17,7 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; import frc.robot.sim.PhysicsSim; @@ -32,16 +33,16 @@ public class Robot extends TimedRobot { private final TalonFX m_fx = new TalonFX(1, canBusName); private final CANcoder m_cc = new CANcoder(1, canBusName); - private final StatusSignal f_fusedSensorOutOfSync = m_fx.getFault_FusedSensorOutOfSync(); - private final StatusSignal sf_fusedSensorOutOfSync = m_fx.getStickyFault_FusedSensorOutOfSync(); - private final StatusSignal f_remoteSensorInvalid = m_fx.getFault_RemoteSensorDataInvalid(); - private final StatusSignal sf_remoteSensorInvalid = m_fx.getStickyFault_RemoteSensorDataInvalid(); + private final StatusSignal f_fusedSensorOutOfSync = m_fx.getFault_FusedSensorOutOfSync(false); + private final StatusSignal sf_fusedSensorOutOfSync = m_fx.getStickyFault_FusedSensorOutOfSync(false); + private final StatusSignal f_remoteSensorInvalid = m_fx.getFault_RemoteSensorDataInvalid(false); + private final StatusSignal sf_remoteSensorInvalid = m_fx.getStickyFault_RemoteSensorDataInvalid(false); - private final StatusSignal fx_pos = m_fx.getPosition(); - private final StatusSignal fx_vel = m_fx.getVelocity(); - private final StatusSignal cc_pos = m_cc.getPosition(); - private final StatusSignal cc_vel = m_cc.getVelocity(); - private final StatusSignal fx_rotorPos = m_fx.getRotorPosition(); + private final StatusSignal fx_pos = m_fx.getPosition(false); + private final StatusSignal fx_vel = m_fx.getVelocity(false); + private final StatusSignal cc_pos = m_cc.getPosition(false); + private final StatusSignal cc_vel = m_cc.getVelocity(false); + private final StatusSignal fx_rotorPos = m_fx.getRotorPosition(false); private final DutyCycleOut m_dutyCycleControl = new DutyCycleOut(0); @@ -61,7 +62,7 @@ public void robotInit() { CANcoderConfiguration cc_cfg = new CANcoderConfiguration(); cc_cfg.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; cc_cfg.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - cc_cfg.MagnetSensor.MagnetOffset = 0.4; + cc_cfg.MagnetSensor.withMagnetOffset(Rotations.of(0.4)); m_cc.getConfigurator().apply(cc_cfg); TalonFXConfiguration fx_cfg = new TalonFXConfiguration(); @@ -75,7 +76,7 @@ public void robotInit() { @Override public void robotPeriodic() { - if (printCount++ > 10) { + if (++printCount >= 10) { printCount = 0; BaseStatusSignal.refreshAll( @@ -84,7 +85,8 @@ public void robotPeriodic() { f_remoteSensorInvalid, sf_remoteSensorInvalid, fx_pos, fx_vel, - cc_pos, cc_vel); + cc_pos, cc_vel + ); // If any faults happen, print them out. Sticky faults will always be present if live-fault occurs boolean anyFault = sf_fusedSensorOutOfSync.getValue() || sf_remoteSensorInvalid.getValue(); diff --git a/java/FusedCANcoder/src/main/java/frc/robot/sim/PhysicsSim.java b/java/FusedCANcoder/src/main/java/frc/robot/sim/PhysicsSim.java index 09c642cf..4d981cd7 100644 --- a/java/FusedCANcoder/src/main/java/frc/robot/sim/PhysicsSim.java +++ b/java/FusedCANcoder/src/main/java/frc/robot/sim/PhysicsSim.java @@ -22,7 +22,7 @@ public static PhysicsSim getInstance() { /** * Adds a TalonFX controller to the simulator. * - * @param falcon + * @param talonFX * The TalonFX device * @param can * The CANcoder device @@ -31,10 +31,10 @@ public static PhysicsSim getInstance() { * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public void addTalonFX(TalonFX falcon, CANcoder can, double gearRatio, final double rotorInertia) { - if (falcon != null) { - TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, can, gearRatio, rotorInertia); - _simProfiles.add(simFalcon); + public void addTalonFX(TalonFX talonFX, CANcoder can, double gearRatio, final double rotorInertia) { + if (talonFX != null) { + TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, can, gearRatio, rotorInertia); + _simProfiles.add(simTalonFX); } } diff --git a/java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java index 9e985ece..930f1b5a 100644 --- a/java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -14,7 +15,7 @@ */ class TalonFXSimProfile extends SimProfile { private static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation - private final TalonFX _falcon; + private final TalonFXSimState _talonFXSim; private final CANcoder _canCoder; private final double _gearRatio; @@ -23,7 +24,7 @@ class TalonFXSimProfile extends SimProfile { /** * Creates a new simulation profile for a TalonFX device. * - * @param falcon + * @param talonFX * The TalonFX device * @param canCoder * The CANcoder associated with the TalonFX @@ -32,8 +33,8 @@ class TalonFXSimProfile extends SimProfile { * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public TalonFXSimProfile(final TalonFX falcon, final CANcoder canCoder, final double gearRatio, final double rotorInertia) { - this._falcon = falcon; + public TalonFXSimProfile(final TalonFX talonFX, final CANcoder canCoder, final double gearRatio, final double rotorInertia) { + this._talonFXSim = talonFX.getSimState(); this._canCoder = canCoder; this._gearRatio = gearRatio; var gearbox = DCMotor.getKrakenX60Foc(1); @@ -50,7 +51,7 @@ public TalonFXSimProfile(final TalonFX falcon, final CANcoder canCoder, final do public void run() { /// DEVICE SPEED SIMULATION - _motorSim.setInputVoltage(_falcon.getSimState().getMotorVoltage()); + _motorSim.setInputVoltage(_talonFXSim.getMotorVoltage()); _motorSim.update(getPeriod()); @@ -58,10 +59,10 @@ public void run() { final double position_rot = _motorSim.getAngularPositionRotations() * _gearRatio; final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()) * _gearRatio; - _falcon.getSimState().setRawRotorPosition(position_rot); - _falcon.getSimState().setRotorVelocity(velocity_rps); + _talonFXSim.setRawRotorPosition(position_rot); + _talonFXSim.setRotorVelocity(velocity_rps); - _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); + _talonFXSim.setSupplyVoltage(12 - _talonFXSim.getSupplyCurrent() * kMotorResistance); _canCoder.getSimState().setRawPosition(position_rot / _gearRatio); _canCoder.getSimState().setVelocity(velocity_rps / _gearRatio); diff --git a/java/MotionMagic/src/main/java/frc/robot/Robot.java b/java/MotionMagic/src/main/java/frc/robot/Robot.java index d284f444..d34f5f9e 100644 --- a/java/MotionMagic/src/main/java/frc/robot/Robot.java +++ b/java/MotionMagic/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; @@ -55,10 +57,10 @@ public void robotInit() { /* Configure Motion Magic */ MotionMagicConfigs mm = cfg.MotionMagic; - mm.MotionMagicCruiseVelocity = 5; // 5 (mechanism) rotations per second cruise - mm.MotionMagicAcceleration = 10; // Take approximately 0.5 seconds to reach max vel - // Take approximately 0.1 seconds to reach max accel - mm.MotionMagicJerk = 100; + mm.withMotionMagicCruiseVelocity(RotationsPerSecond.of(5)) // 5 (mechanism) rotations per second cruise + .withMotionMagicAcceleration(RotationsPerSecondPerSecond.of(10)) // Take approximately 0.5 seconds to reach max vel + // Take approximately 0.1 seconds to reach max accel + .withMotionMagicJerk(RotationsPerSecondPerSecond.per(Second).of(100)); Slot0Configs slot0 = cfg.Slot0; slot0.kS = 0.25; // Add 0.25 V output to overcome static friction @@ -80,7 +82,7 @@ public void robotInit() { @Override public void robotPeriodic() { - if (m_printCount++ > 10) { + if (++m_printCount >= 10) { m_printCount = 0; System.out.println("Pos: " + m_fx.getPosition()); System.out.println("Vel: " + m_fx.getVelocity()); @@ -106,7 +108,7 @@ public void teleopPeriodic() { m_fx.setControl(m_mmReq.withPosition(leftY * 10).withSlot(0)); if (m_joystick.getBButton()) { - m_fx.setPosition(1); + m_fx.setPosition(Rotations.of(1)); } } diff --git a/java/MotionMagic/src/main/java/frc/robot/sim/PhysicsSim.java b/java/MotionMagic/src/main/java/frc/robot/sim/PhysicsSim.java index e1c2ff2a..c29151e3 100644 --- a/java/MotionMagic/src/main/java/frc/robot/sim/PhysicsSim.java +++ b/java/MotionMagic/src/main/java/frc/robot/sim/PhysicsSim.java @@ -21,15 +21,15 @@ public static PhysicsSim getInstance() { /** * Adds a TalonFX controller to the simulator. * - * @param falcon + * @param talonFX * The TalonFX device * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public void addTalonFX(TalonFX falcon, final double rotorInertia) { - if (falcon != null) { - TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, rotorInertia); - _simProfiles.add(simFalcon); + public void addTalonFX(TalonFX talonFX, final double rotorInertia) { + if (talonFX != null) { + TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, rotorInertia); + _simProfiles.add(simTalonFX); } } diff --git a/java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java index 256a5bbd..12b74790 100644 --- a/java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -1,6 +1,7 @@ package frc.robot.sim; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -13,20 +14,20 @@ */ class TalonFXSimProfile extends SimProfile { private static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation - private final TalonFX _falcon; + private final TalonFXSimState _talonFXSim; private final DCMotorSim _motorSim; /** * Creates a new simulation profile for a TalonFX device. * - * @param falcon + * @param talonFX * The TalonFX device * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { - this._falcon = falcon; + public TalonFXSimProfile(final TalonFX talonFX, final double rotorInertia) { + this._talonFXSim = talonFX.getSimState(); var gearbox = DCMotor.getKrakenX60Foc(1); this._motorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, rotorInertia, 1.0), gearbox); } @@ -41,7 +42,7 @@ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { public void run() { /// DEVICE SPEED SIMULATION - _motorSim.setInputVoltage(_falcon.getSimState().getMotorVoltage()); + _motorSim.setInputVoltage(_talonFXSim.getMotorVoltage()); _motorSim.update(getPeriod()); @@ -49,9 +50,9 @@ public void run() { final double position_rot = _motorSim.getAngularPositionRotations(); final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); - _falcon.getSimState().setRawRotorPosition(position_rot); - _falcon.getSimState().setRotorVelocity(velocity_rps); + _talonFXSim.setRawRotorPosition(position_rot); + _talonFXSim.setRotorVelocity(velocity_rps); - _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); + _talonFXSim.setSupplyVoltage(12 - _talonFXSim.getSupplyCurrent() * kMotorResistance); } } \ No newline at end of file diff --git a/java/PhoenixSysId/src/main/java/frc/robot/subsystems/FlywheelMechanism.java b/java/PhoenixSysId/src/main/java/frc/robot/subsystems/FlywheelMechanism.java index af9d3316..f8d67d46 100644 --- a/java/PhoenixSysId/src/main/java/frc/robot/subsystems/FlywheelMechanism.java +++ b/java/PhoenixSysId/src/main/java/frc/robot/subsystems/FlywheelMechanism.java @@ -11,7 +11,6 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -29,11 +28,14 @@ public class FlywheelMechanism extends SubsystemBase { Volts.of(4), // Reduce dynamic voltage to 4 to prevent brownout null, // Use default timeout (10 s) // Log state with Phoenix SignalLogger class - (state)->SignalLogger.writeString("state", state.toString())), + state -> SignalLogger.writeString("state", state.toString()) + ), new SysIdRoutine.Mechanism( - (Voltage volts)-> m_motorToTest.setControl(m_sysIdControl.withOutput(volts.in(Volts))), + volts -> m_motorToTest.setControl(m_sysIdControl.withOutput(volts)), null, - this)); + this + ) + ); public FlywheelMechanism() { setName("Flywheel"); @@ -56,7 +58,7 @@ public FlywheelMechanism() { } public Command joystickDriveCommand(DoubleSupplier output) { - return run(()->m_motorToTest.setControl(m_joystickControl.withOutput(output.getAsDouble()))); + return run(() -> m_motorToTest.setControl(m_joystickControl.withOutput(output.getAsDouble()))); } public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return m_sysIdRoutine.quasistatic(direction); diff --git a/java/Pigeon2/src/main/java/frc/robot/Robot.java b/java/Pigeon2/src/main/java/frc/robot/Robot.java index 25604f93..882310f0 100644 --- a/java/Pigeon2/src/main/java/frc/robot/Robot.java +++ b/java/Pigeon2/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.controls.DutyCycleOut; @@ -66,19 +68,21 @@ public void robotPeriodic() { System.out.println("Yaw is " + yaw.toString() + " with " + yaw.getTimestamp().getLatency() + " seconds of latency"); /** - * Get the gravity vector Z component StatusSignalValue + * Get the gravity vector Z component StatusSignalValue without refreshing */ - var gravityVectorZ = pidgey.getGravityVectorZ(); + var gravityVectorZ = pidgey.getGravityVectorZ(false); /* This time wait for the signal to reduce latency */ gravityVectorZ.waitForUpdate(PRINT_PERIOD); // Wait up to our period /** * This uses the explicit getValue and getUnits functions to print, even though it's not * necessary for the ostream print */ - System.out.println("Gravity Vector in the Z direction is " + - gravityVectorZ.getValue() + " " + - gravityVectorZ.getUnits() + " with " + - gravityVectorZ.getTimestamp().getLatency() + " seconds of latency"); + System.out.println( + "Gravity Vector in the Z direction is " + + gravityVectorZ.getValue() + " " + + gravityVectorZ.getUnits() + " with " + + gravityVectorZ.getTimestamp().getLatency() + " seconds of latency" + ); /** * Notice when running this example that the second print's latency is always shorter than the first print's latency. * This is because we explicitly wait for the signal using the waitForUpdate() method instead of using the refresh() @@ -104,7 +108,7 @@ public void teleopInit() { /** * When we teleop init, set the position of the Pigeon2 and wait for the setter to take affect. */ - pidgey.setYaw(144, 0.1); // Set our yaw to 144 degrees and wait up to 100 ms for the setter to take affect + pidgey.setYaw(Degrees.of(144), 0.1); // Set our yaw to 144 degrees and wait up to 100 ms for the setter to take affect pidgey.getYaw().waitForUpdate(0.1); // And wait up to 100 ms for the position to take affect System.out.println("Set the position to 144 degrees, we are currently at " + pidgey.getYaw()); // Use java's implicit toString operator } diff --git a/java/Pigeon2/src/main/java/frc/robot/sim/PhysicsSim.java b/java/Pigeon2/src/main/java/frc/robot/sim/PhysicsSim.java index 26eb9102..ea986a03 100644 --- a/java/Pigeon2/src/main/java/frc/robot/sim/PhysicsSim.java +++ b/java/Pigeon2/src/main/java/frc/robot/sim/PhysicsSim.java @@ -22,7 +22,7 @@ public static PhysicsSim getInstance() { /** * Adds a TalonFX controller to the simulator. * - * @param falcon + * @param talonFX * The TalonFX device * @param can * The CANcoder device @@ -31,10 +31,10 @@ public static PhysicsSim getInstance() { * @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); + public void addTalonFX(TalonFX talonFX, Pigeon2 pigeon, final double rotorInertia) { + if (talonFX != null) { + TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, pigeon, rotorInertia); + _simProfiles.add(simTalonFX); } } diff --git a/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java index 0ce940d0..1ec3be10 100644 --- a/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -14,7 +15,7 @@ */ class TalonFXSimProfile extends SimProfile { private static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation - private final TalonFX _falcon; + private final TalonFXSimState _talonFXSim; private final Pigeon2 _pigeon; private final DCMotorSim _motorSim; @@ -22,15 +23,15 @@ class TalonFXSimProfile extends SimProfile { /** * Creates a new simulation profile for a TalonFX device. * - * @param falcon + * @param talonFX * The TalonFX device * @param pigeon * The Pigeon 2 associated with the TalonFX * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public TalonFXSimProfile(final TalonFX falcon, final Pigeon2 pigeon, final double rotorInertia) { - this._falcon = falcon; + public TalonFXSimProfile(final TalonFX talonFX, final Pigeon2 pigeon, final double rotorInertia) { + this._talonFXSim = talonFX.getSimState(); this._pigeon = pigeon; var gearbox = DCMotor.getKrakenX60Foc(1); this._motorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, rotorInertia, 1.0), gearbox); @@ -46,7 +47,7 @@ public TalonFXSimProfile(final TalonFX falcon, final Pigeon2 pigeon, final doubl public void run() { /// DEVICE SPEED SIMULATION - _motorSim.setInputVoltage(_falcon.getSimState().getMotorVoltage()); + _motorSim.setInputVoltage(_talonFXSim.getMotorVoltage()); _motorSim.update(getPeriod()); @@ -54,10 +55,10 @@ public void run() { final double position_rot = _motorSim.getAngularPositionRotations(); final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); - _falcon.getSimState().setRawRotorPosition(position_rot); - _falcon.getSimState().setRotorVelocity(velocity_rps); + _talonFXSim.setRawRotorPosition(position_rot); + _talonFXSim.setRotorVelocity(velocity_rps); - _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); + _talonFXSim.setSupplyVoltage(12 - _talonFXSim.getSupplyCurrent() * kMotorResistance); _pigeon.getSimState().setRawYaw(position_rot); diff --git a/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java b/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java index cf435b0d..ce9b0d3b 100644 --- a/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java +++ b/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.NeutralOut; @@ -47,15 +49,15 @@ public void robotInit() { configs.Slot0.kI = 0; // No output for integrated error configs.Slot0.kD = 0.1; // A velocity of 1 rps results in 0.1 V output // Peak output of 8 V - configs.Voltage.PeakForwardVoltage = 8; - configs.Voltage.PeakReverseVoltage = -8; + configs.Voltage.withPeakForwardVoltage(Volts.of(8)) + .withPeakReverseVoltage(Volts.of(-8)); configs.Slot1.kP = 60; // An error of 1 rotation results in 60 A output configs.Slot1.kI = 0; // No output for integrated error configs.Slot1.kD = 6; // A velocity of 1 rps results in 6 A output // Peak output of 120 A - configs.TorqueCurrent.PeakForwardTorqueCurrent = 120; - configs.TorqueCurrent.PeakReverseTorqueCurrent = -120; + configs.TorqueCurrent.withPeakForwardTorqueCurrent(Amps.of(120)) + .withPeakReverseTorqueCurrent(Amps.of(-120)); /* Retry config apply up to 5 times, report if failure */ StatusCode status = StatusCode.StatusCodeNotInitialized; diff --git a/java/PositionClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java b/java/PositionClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java index e1571a39..6000bca7 100644 --- a/java/PositionClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java +++ b/java/PositionClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java @@ -21,15 +21,15 @@ public static PhysicsSim getInstance() { /** * Adds a TalonFX controller to the simulator. * - * @param falcon + * @param talonFX * The TalonFX device * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public void addTalonFX(TalonFX falcon, final double rotorInertia) { - if (falcon != null) { - TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, rotorInertia); - _simProfiles.add(simFalcon); + public void addTalonFX(TalonFX talonFX, final double rotorInertia) { + if (talonFX != null) { + TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, rotorInertia); + _simProfiles.add(simTalonFX); } } 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 17c40b66..e1f7321a 100644 --- a/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -16,20 +16,20 @@ class TalonFXSimProfile extends SimProfile { private static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation private final DCMotorSim _motorSim; - private final TalonFXSimState _falconSim; + private final TalonFXSimState _talonFXSim; /** * Creates a new simulation profile for a TalonFX device. * - * @param falcon + * @param talonFX * The TalonFX device * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { + public TalonFXSimProfile(final TalonFX talonFX, final double rotorInertia) { var gearbox = DCMotor.getKrakenX60Foc(1); this._motorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, rotorInertia, 1.0), gearbox); - this._falconSim = falcon.getSimState(); + this._talonFXSim = talonFX.getSimState(); } /** @@ -42,7 +42,7 @@ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { public void run() { /// DEVICE SPEED SIMULATION - _motorSim.setInputVoltage(_falconSim.getMotorVoltage()); + _motorSim.setInputVoltage(_talonFXSim.getMotorVoltage()); _motorSim.update(getPeriod()); @@ -50,9 +50,9 @@ public void run() { final double position_rot = _motorSim.getAngularPositionRotations(); final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); - _falconSim.setRawRotorPosition(position_rot); - _falconSim.setRotorVelocity(velocity_rps); + _talonFXSim.setRawRotorPosition(position_rot); + _talonFXSim.setRotorVelocity(velocity_rps); - _falconSim.setSupplyVoltage(12 - _falconSim.getSupplyCurrent() * kMotorResistance); + _talonFXSim.setSupplyVoltage(12 - _talonFXSim.getSupplyCurrent() * kMotorResistance); } } \ No newline at end of file diff --git a/java/Simulation/src/main/java/frc/robot/Robot.java b/java/Simulation/src/main/java/frc/robot/Robot.java index d5ba1e38..c38309cb 100644 --- a/java/Simulation/src/main/java/frc/robot/Robot.java +++ b/java/Simulation/src/main/java/frc/robot/Robot.java @@ -23,7 +23,6 @@ import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -75,11 +74,11 @@ public class Robot extends TimedRobot { /* Simulation model of the drivetrain */ private final DifferentialDrivetrainSim m_driveSim = new DifferentialDrivetrainSim( - DCMotor.getFalcon500Foc(2), // 2 CIMS on each side of the drivetrain. + DCMotor.getKrakenX60Foc(2), // 2 Kraken X60 on each side of the drivetrain. kGearRatio, // Standard AndyMark Gearing reduction. 2.1, // MOI of 2.1 kg m^2 (from CAD model). 26.5, // Mass of the robot is 26.5 kg. - kWheelRadius.in(Inches), // Robot uses 3" radius (6" diameter) wheels. + kWheelRadius.in(Meters), // Robot uses 3" radius (6" diameter) wheels. 0.546, // Distance between wheels is _ meters. /* @@ -156,9 +155,11 @@ public void robotPeriodic() { * in the previous article while in simulation, but will use * real values on the robot itself. */ - m_odometry.update(imu.getRotation2d(), - rotationsToMeters(leftSensor.getPosition().getValue()).in(Meters), - rotationsToMeters(rightSensor.getPosition().getValue()).in(Meters)); + m_odometry.update( + imu.getRotation2d(), + rotationsToMeters(leftSensor.getPosition().getValue()).in(Meters), + rotationsToMeters(rightSensor.getPosition().getValue()).in(Meters) + ); m_field.setRobotPose(m_odometry.getPoseMeters()); if (++printCount >= 50) { @@ -209,8 +210,10 @@ public void simulationPeriodic() { * WPILib expects +V to be forward. We have already configured * our orientations to match this behavior. */ - m_driveSim.setInputs(leftSim.getMotorVoltage(), - rightSim.getMotorVoltage()); + m_driveSim.setInputs( + leftSim.getMotorVoltage(), + rightSim.getMotorVoltage() + ); /* * Advance the model by 20 ms. Note that if you are running this @@ -221,15 +224,19 @@ public void simulationPeriodic() { /* Update all of our sensors. */ final var leftPos = metersToRotations( - Meters.of(m_driveSim.getLeftPositionMeters())); + Meters.of(m_driveSim.getLeftPositionMeters()) + ); // This is OK, since the time base is the same final var leftVel = metersToRotationsVel( - MetersPerSecond.of(m_driveSim.getLeftVelocityMetersPerSecond())); + MetersPerSecond.of(m_driveSim.getLeftVelocityMetersPerSecond())) + ; final var rightPos = metersToRotations( - Meters.of(m_driveSim.getRightPositionMeters())); + Meters.of(m_driveSim.getRightPositionMeters()) + ); // This is OK, since the time base is the same final var rightVel = metersToRotationsVel( - MetersPerSecond.of(m_driveSim.getRightVelocityMetersPerSecond())); + MetersPerSecond.of(m_driveSim.getRightVelocityMetersPerSecond()) + ); leftSensSim.setRawPosition(leftPos); leftSensSim.setVelocity(leftVel); rightSensSim.setRawPosition(rightPos); @@ -300,9 +307,9 @@ private Angle metersToRotations(Distance meters) { private LinearVelocity rotationsToMetersVel(AngularVelocity rotations) { /* Apply gear ratio to input rotations */ - var gearedRotations = rotations.divide(this.kGearRatio); + var gearedRotations = rotations.in(RadiansPerSecond) / this.kGearRatio; /* Then multiply the wheel radius by radians of rotation to get distance */ - return this.kWheelRadius.per(Second).times(gearedRotations.in(RadiansPerSecond)); + return this.kWheelRadius.per(Second).times(gearedRotations); } private AngularVelocity metersToRotationsVel(LinearVelocity meters) { diff --git a/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java b/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java index 2c6b6bc8..bcde2bcf 100644 --- a/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java +++ b/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java @@ -4,6 +4,9 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; @@ -23,9 +26,9 @@ * project. */ public class Robot extends TimedRobot { - private static final String canbusName = "canivore"; - private final TalonFX m_fx = new TalonFX(0, canbusName); - private final TalonFX m_fllr = new TalonFX(1, canbusName); + private final CANBus canbus = new CANBus("canivore"); + private final TalonFX m_fx = new TalonFX(0, canbus); + private final TalonFX m_fllr = new TalonFX(1, canbus); /* Be able to switch which control request to use based on a button press */ /* Start at velocity 0, use slot 0 */ @@ -54,8 +57,8 @@ public void robotInit() { configs.Slot0.kI = 0; // No output for integrated error configs.Slot0.kD = 0; // No output for error derivative // Peak output of 8 volts - configs.Voltage.PeakForwardVoltage = 8; - configs.Voltage.PeakReverseVoltage = -8; + configs.Voltage.withPeakForwardVoltage(Volts.of(8)) + .withPeakReverseVoltage(Volts.of(-8)); /* Torque-based velocity does not require a velocity feed forward, as torque will accelerate the rotor up to the desired velocity by itself */ configs.Slot1.kS = 2.5; // To account for friction, add 2.5 A of static feedforward @@ -63,8 +66,8 @@ public void robotInit() { configs.Slot1.kI = 0; // No output for integrated error configs.Slot1.kD = 0; // No output for error derivative // Peak output of 40 A - configs.TorqueCurrent.PeakForwardTorqueCurrent = 40; - configs.TorqueCurrent.PeakReverseTorqueCurrent = -40; + configs.TorqueCurrent.withPeakForwardTorqueCurrent(Amps.of(40)) + .withPeakReverseTorqueCurrent(Amps.of(-40)); /* Retry config apply up to 5 times, report if failure */ StatusCode status = StatusCode.StatusCodeNotInitialized; diff --git a/java/VelocityClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java index e1571a39..6000bca7 100644 --- a/java/VelocityClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java +++ b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java @@ -21,15 +21,15 @@ public static PhysicsSim getInstance() { /** * Adds a TalonFX controller to the simulator. * - * @param falcon + * @param talonFX * The TalonFX device * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public void addTalonFX(TalonFX falcon, final double rotorInertia) { - if (falcon != null) { - TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, rotorInertia); - _simProfiles.add(simFalcon); + public void addTalonFX(TalonFX talonFX, final double rotorInertia) { + if (talonFX != null) { + TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, rotorInertia); + _simProfiles.add(simTalonFX); } } 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 8f8d42e1..0dc97565 100644 --- a/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -14,21 +14,21 @@ */ class TalonFXSimProfile extends SimProfile { private static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation - private final TalonFXSimState _falconSim; + private final TalonFXSimState _talonFXSim; private final DCMotorSim _motorSim; /** * Creates a new simulation profile for a TalonFX device. * - * @param falcon + * @param talonFX * The TalonFX device * @param rotorInertia * Rotational Inertia of the mechanism at the rotor */ - public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { + public TalonFXSimProfile(final TalonFX talonFX, final double rotorInertia) { var gearbox = DCMotor.getKrakenX60Foc(1); this._motorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, rotorInertia, 1.0), gearbox); - this._falconSim = falcon.getSimState(); + this._talonFXSim = talonFX.getSimState(); } /** @@ -41,7 +41,7 @@ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { public void run() { /// DEVICE SPEED SIMULATION - _motorSim.setInputVoltage(_falconSim.getMotorVoltage()); + _motorSim.setInputVoltage(_talonFXSim.getMotorVoltage()); _motorSim.update(getPeriod()); @@ -49,9 +49,9 @@ public void run() { final double position_rot = _motorSim.getAngularPositionRotations(); final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); - _falconSim.setRawRotorPosition(position_rot); - _falconSim.setRotorVelocity(velocity_rps); + _talonFXSim.setRawRotorPosition(position_rot); + _talonFXSim.setRotorVelocity(velocity_rps); - _falconSim.setSupplyVoltage(12 - _falconSim.getSupplyCurrent() * kMotorResistance); + _talonFXSim.setSupplyVoltage(12 - _talonFXSim.getSupplyCurrent() * kMotorResistance); } } \ No newline at end of file diff --git a/java/WaitForAll/src/main/java/frc/robot/Robot.java b/java/WaitForAll/src/main/java/frc/robot/Robot.java index e0486954..c076d8f6 100644 --- a/java/WaitForAll/src/main/java/frc/robot/Robot.java +++ b/java/WaitForAll/src/main/java/frc/robot/Robot.java @@ -28,16 +28,16 @@ public class Robot extends TimedRobot { // selectively power it to completely test this example private final TalonFX m_motor2 = new TalonFX(0, "rio"); // Pick the RIO bus to force a failure we can detect - private final StatusSignal m_canbus1signal1 = m_motor1.getPosition(); - private final StatusSignal m_canbus1signal2 = m_motor1.getVelocity(); - private final StatusSignal m_canbus1signal3 = m_motor1.getControlMode(); - private final StatusSignal m_canbus1signal4 = m_pigdey.getYaw(); - private final StatusSignal m_canbus1signal5 = m_pigdey.getRoll(); + private final StatusSignal m_canbus1signal1 = m_motor1.getPosition(false); + private final StatusSignal m_canbus1signal2 = m_motor1.getVelocity(false); + private final StatusSignal m_canbus1signal3 = m_motor1.getControlMode(false); + private final StatusSignal m_canbus1signal4 = m_pigdey.getYaw(false); + private final StatusSignal m_canbus1signal5 = m_pigdey.getRoll(false); - private final StatusSignal m_canbus2signal1 = m_motor2.getPosition(); + private final StatusSignal m_canbus2signal1 = m_motor2.getPosition(false); - private final StatusSignal m_canbus1transcient1 = m_transcientMotor.getPosition(); - private final StatusSignal m_canbus1transcient2 = m_transcientMotor.getVelocity(); + private final StatusSignal m_canbus1transcient1 = m_transcientMotor.getPosition(false); + private final StatusSignal m_canbus1transcient2 = m_transcientMotor.getVelocity(false); private final StatusSignal[] m_signalsAcrossCANbuses = new StatusSignal[]{ m_canbus1signal1, @@ -70,44 +70,47 @@ public void robotInit() {} @Override public void robotPeriodic() { - if(m_joystick.getLeftBumperButton()){ + if (m_joystick.getLeftBumperButton()){ m_waitForAllTimeout = 0.1; System.out.println("Timeout is now at " + m_waitForAllTimeout); } - if(m_joystick.getRightBumperButton()){ + if (m_joystick.getRightBumperButton()){ m_waitForAllTimeout = 0; System.out.println("Timeout is now at " + m_waitForAllTimeout); } /* If we press the A button, test what happens when we wait on lots of signals (normal use case) */ - if(m_joystick.getAButtonPressed()) { + if (m_joystick.getAButtonPressed()) { var status = BaseStatusSignal.waitForAll(m_waitForAllTimeout, m_lotsOfSignals); System.out.println("Status of waiting on signals (normal use case): " + status); - for(var sig : m_lotsOfSignals) { + for (var sig : m_lotsOfSignals) { System.out.println("Signal status: " + sig.getStatus()); } } + /* If we press the B button, test what happens when we wait on signals from different busses */ - if(m_joystick.getBButtonPressed()) { + if (m_joystick.getBButtonPressed()) { var status = BaseStatusSignal.waitForAll(m_waitForAllTimeout, m_signalsAcrossCANbuses); System.out.println("Status of waiting on signals across different CAN busses: " + status); - for(var sig : m_signalsAcrossCANbuses) { + for (var sig : m_signalsAcrossCANbuses) { System.out.println("Signal status: " + sig.getStatus()); } } + /* If we press the Y button, test what happens when we wait on no signals */ - if(m_joystick.getYButtonPressed()) { + if (m_joystick.getYButtonPressed()) { var status = BaseStatusSignal.waitForAll(m_waitForAllTimeout, m_noSignals); System.out.println("Status of waiting on no signals: " + status); - for(var sig : m_noSignals) { + for (var sig : m_noSignals) { System.out.println("Signal status: " + sig.getStatus()); } } + /* If we press the X button, test what happens when we wait on signals with the transcient motor controller */ - if(m_joystick.getXButtonPressed()) { + if (m_joystick.getXButtonPressed()) { var status = BaseStatusSignal.waitForAll(m_waitForAllTimeout, m_tanscientSignals); System.out.println("Status of waiting on transcient signals: " + status); - for(var sig : m_tanscientSignals) { + for (var sig : m_tanscientSignals) { System.out.println("Signal status: " + sig.getStatus()); } } diff --git a/python/ArcadeDrive/physics.py b/python/ArcadeDrive/physics.py index b7fc4e91..f40d4a27 100644 --- a/python/ArcadeDrive/physics.py +++ b/python/ArcadeDrive/physics.py @@ -1,14 +1,14 @@ import wpilib.simulation as sim from wpilib import RobotController, DriverStation +from wpilib.simulation import DifferentialDrivetrainSim +from wpimath.kinematics import DifferentialDriveKinematics, DifferentialDriveWheelSpeeds from wpimath.system.plant import DCMotor -from wpimath.units import radiansToRotations +from wpimath.units import inchesToMeters from pyfrc.physics.core import PhysicsInterface -from pyfrc.physics import motor_cfgs, tankmodel -from pyfrc.physics.units import units -from phoenix6 import unmanaged +from phoenix6 import unmanaged, sim import math @@ -27,21 +27,29 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): self.left_talon_sim = robot.front_left_motor.sim_state self.right_talon_sim = robot.front_right_motor.sim_state - # Change these parameters to fit your robot! - bumper_width = 3.25 * units.inch - - self.wheel_radius = 3 - self.drivetrain = tankmodel.TankModel.theory( - motor_cfgs.MOTOR_CFG_FALCON_500, # motor configuration - 110 * units.lbs, # robot mass - 10.71, # drivetrain gear ratio - 2, # motors per side - 22 * units.inch, # robot wheelbase - 23 * units.inch + bumper_width * 2, # robot width - 32 * units.inch + bumper_width * 2, # robot length - self.wheel_radius * 2 * units.inch, # wheel diameter + self.wheel_radius = inchesToMeters(3) + self.gear_ratio = 10.71 + track_width = 0.546 + + self.drivetrain = DifferentialDrivetrainSim( + DCMotor.krakenX60FOC(2), # 2 Kraken X60 on each side of the drivetrain + self.gear_ratio, # drivetrain gear ratio + 2.1, # MOI of 2.1 kg m^2 (from CAD model) + 26.5, # Mass of the robot is 26.5 kg + self.wheel_radius, # Robot uses 3" radius (6" diameter) wheels + track_width, # Distance between wheels is _ meters. ) + self.kinematics = DifferentialDriveKinematics(track_width) + + # Set the orientation of the simulated devices relative to the robot chassis. + # WPILib expects +V to be forward. Specify orientations to match that behavior. + + # left devices are CCW+ + self.left_talon_sim.orientation = sim.ChassisReference.CounterClockwise_Positive + # right devices are CW+ + self.right_talon_sim.orientation = sim.ChassisReference.Clockwise_Positive + def update_sim(self, now: float, tm_diff: float) -> None: """ Called when the simulation parameters for the program need to be @@ -51,24 +59,40 @@ def update_sim(self, now: float, tm_diff: float) -> None: :param tm_diff: The amount of time that has passed since the last time that this function was called """ - # If the driver station is enabled, then feed enable for phoenix devices - if DriverStation.isEnabled(): - unmanaged.feed_enable(100) - battery_v = RobotController.getBatteryVoltage() self.left_talon_sim.set_supply_voltage(battery_v) self.right_talon_sim.set_supply_voltage(battery_v) - transform = self.drivetrain.calculate(self.left_talon_sim.motor_voltage / battery_v, self.right_talon_sim.motor_voltage / battery_v, tm_diff) - self.physics_controller.move_robot(transform) - - self.left_talon_sim.set_raw_rotor_position(self.feet_to_rotations(self.drivetrain.l_position)) - self.left_talon_sim.set_rotor_velocity(self.feet_to_rotations(self.drivetrain.l_velocity)) - self.right_talon_sim.set_raw_rotor_position(self.feet_to_rotations(self.drivetrain.r_position)) - self.right_talon_sim.set_rotor_velocity(self.feet_to_rotations(self.drivetrain.r_velocity)) + # CTRE simulation is low-level, so SimState inputs + # and outputs are not affected by user-level inversion. + # However, inputs and outputs *are* affected by the mechanical + # orientation of the device relative to the robot chassis, + # as specified by the `orientation` field. + # + # WPILib expects +V to be forward. We have already configured + # our orientations to match this behavior. + self.drivetrain.setInputs(self.left_talon_sim.motor_voltage, self.right_talon_sim.motor_voltage) + + # Advance the model by tm_diff + self.drivetrain.update(tm_diff) + + self.left_talon_sim.set_raw_rotor_position(self.meters_to_rotations(self.drivetrain.getLeftPosition())) + self.left_talon_sim.set_rotor_velocity(self.meters_to_rotations(self.drivetrain.getLeftVelocity())) + self.right_talon_sim.set_raw_rotor_position(self.meters_to_rotations(self.drivetrain.getRightPosition())) + self.right_talon_sim.set_rotor_velocity(self.meters_to_rotations(self.drivetrain.getRightVelocity())) + + self.physics_controller.drive( + self.kinematics.toChassisSpeeds( + DifferentialDriveWheelSpeeds( + self.drivetrain.getLeftVelocity(), + self.drivetrain.getRightVelocity() + ) + ), + tm_diff, + ) - def feet_to_rotations(self, dist: float) -> float: + def meters_to_rotations(self, dist: float) -> float: circumference = self.wheel_radius * 2.0 * math.pi - rotations_per_foot = 1.0 / (circumference / 12.0) - return rotations_per_foot * dist + rotations_per_meter = self.gear_ratio / circumference + return dist * rotations_per_meter