diff --git a/cpp/BasicLatencyCompensation/build.gradle b/cpp/BasicLatencyCompensation/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/BasicLatencyCompensation/build.gradle +++ b/cpp/BasicLatencyCompensation/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/CANcoder/build.gradle b/cpp/CANcoder/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/CANcoder/build.gradle +++ b/cpp/CANcoder/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/CommandBasedDrive/build.gradle b/cpp/CommandBasedDrive/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/CommandBasedDrive/build.gradle +++ b/cpp/CommandBasedDrive/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/CommandBasedDrive/src/main/include/subsystems/DriveSubsystem.h b/cpp/CommandBasedDrive/src/main/include/subsystems/DriveSubsystem.h index 944b3eb3..9ac4f963 100644 --- a/cpp/CommandBasedDrive/src/main/include/subsystems/DriveSubsystem.h +++ b/cpp/CommandBasedDrive/src/main/include/subsystems/DriveSubsystem.h @@ -43,7 +43,7 @@ class DriveSubsystem : public frc2::SubsystemBase static constexpr units::inch_t kWheelRadiusInches = 3_in; frc::sim::DifferentialDrivetrainSim m_driveSim{ - frc::DCMotor::Falcon500(2), + frc::DCMotor::Falcon500FOC(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/CurrentLimits/build.gradle b/cpp/CurrentLimits/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/CurrentLimits/build.gradle +++ b/cpp/CurrentLimits/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/CurrentLimits/src/main/include/Robot.h b/cpp/CurrentLimits/src/main/include/Robot.h index 083fd90f..d43f980d 100644 --- a/cpp/CurrentLimits/src/main/include/Robot.h +++ b/cpp/CurrentLimits/src/main/include/Robot.h @@ -28,7 +28,7 @@ class Robot : public frc::TimedRobot { void SimulationInit() override; void SimulationPeriodic() override; - ctre::phoenix6::hardware::TalonFX m_fx{0, "Fred"}; + ctre::phoenix6::hardware::TalonFX m_fx{0, "canivore"}; ctre::phoenix6::controls::DutyCycleOut m_output{0}; ctre::phoenix6::configs::CurrentLimitsConfigs m_currentLimits{}; diff --git a/cpp/Falcon500ArcadeDrive/build.gradle b/cpp/Falcon500ArcadeDrive/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/Falcon500ArcadeDrive/build.gradle +++ b/cpp/Falcon500ArcadeDrive/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/FusedCANcoder/build.gradle b/cpp/FusedCANcoder/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/FusedCANcoder/build.gradle +++ b/cpp/FusedCANcoder/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/FusedCANcoder/src/main/include/Robot.h b/cpp/FusedCANcoder/src/main/include/Robot.h index aab419c7..70a3f019 100644 --- a/cpp/FusedCANcoder/src/main/include/Robot.h +++ b/cpp/FusedCANcoder/src/main/include/Robot.h @@ -30,8 +30,8 @@ class Robot : public frc::TimedRobot { void SimulationPeriodic() override; - ctre::phoenix6::hardware::TalonFX m_fx{1, "fred"}; - ctre::phoenix6::hardware::CANcoder m_cc{1, "fred"}; + ctre::phoenix6::hardware::TalonFX m_fx{1, "canivore"}; + ctre::phoenix6::hardware::CANcoder m_cc{1, "canivore"}; ctre::phoenix6::StatusSignal f_fusedSensorOutOfSync = m_fx.GetFault_FusedSensorOutOfSync(); ctre::phoenix6::StatusSignal sf_fusedSensorOutOfSync = m_fx.GetStickyFault_FusedSensorOutOfSync(); ctre::phoenix6::StatusSignal f_remoteSensorInvalid = m_fx.GetFault_RemoteSensorDataInvalid(); diff --git a/cpp/MotionMagic/build.gradle b/cpp/MotionMagic/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/MotionMagic/build.gradle +++ b/cpp/MotionMagic/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/MotionMagic/src/main/cpp/Robot.cpp b/cpp/MotionMagic/src/main/cpp/Robot.cpp index cbb7612e..c56b807a 100644 --- a/cpp/MotionMagic/src/main/cpp/Robot.cpp +++ b/cpp/MotionMagic/src/main/cpp/Robot.cpp @@ -20,24 +20,21 @@ void Robot::RobotInit() { configs::TalonFXConfiguration cfg{}; /* Configure current limits */ - configs::MotionMagicConfigs mm{}; + configs::MotionMagicConfigs &mm = cfg.MotionMagic; mm.MotionMagicCruiseVelocity = 5; // 5 rotations per second cruise mm.MotionMagicAcceleration = 10; // Take approximately 0.5 seconds to reach max vel // Take approximately 0.2 seconds to reach max accel mm.MotionMagicJerk = 50; - cfg.MotionMagic = mm; - configs::Slot0Configs slot0{}; + configs::Slot0Configs &slot0 = cfg.Slot0; slot0.kP = 60; slot0.kI = 0; slot0.kD = 0.1; slot0.kV = 0.12; slot0.kS = 0.25; // Approximately 0.25V to get the mechanism moving - cfg.Slot0 = slot0; - configs::FeedbackConfigs fdb{}; + configs::FeedbackConfigs &fdb = cfg.Feedback; fdb.SensorToMechanismRatio = 12.8; - cfg.Feedback = fdb; ctre::phoenix::StatusCode status = ctre::phoenix::StatusCode::StatusCodeNotInitialized; for(int i = 0; i < 5; ++i) { diff --git a/cpp/MotionMagic/src/main/cpp/sim/TalonFXSimProfile.cpp b/cpp/MotionMagic/src/main/cpp/sim/TalonFXSimProfile.cpp index c2e80b4c..c172843e 100644 --- a/cpp/MotionMagic/src/main/cpp/sim/TalonFXSimProfile.cpp +++ b/cpp/MotionMagic/src/main/cpp/sim/TalonFXSimProfile.cpp @@ -3,7 +3,7 @@ using namespace ctre::phoenix6; TalonFXSimProfile::TalonFXSimProfile(hardware::TalonFX& falcon, units::kilogram_square_meter_t rotorInertia) : - _motorSim{frc::DCMotor::Falcon500(1), 1, rotorInertia}, + _motorSim{frc::DCMotor::Falcon500FOC(1), 1, rotorInertia}, _falcon{falcon} { } @@ -15,9 +15,10 @@ void TalonFXSimProfile::Run() { _motorSim.Update(GetPeriod()); /// SET SIM PHYSICS INPUTS - auto velocity = _motorSim.GetAngularVelocity(); + auto const position = _motorSim.GetAngularPosition(); + auto const velocity = _motorSim.GetAngularVelocity(); - _falcon.GetSimState().SetRawRotorPosition(_motorSim.GetAngularPosition()); + _falcon.GetSimState().SetRawRotorPosition(position); _falcon.GetSimState().SetRotorVelocity(velocity); _falcon.GetSimState().SetSupplyVoltage(12_V - _falcon.GetSimState().GetSupplyCurrent() * kMotorResistance); diff --git a/cpp/MotionMagic/src/main/include/sim/TalonFXSimProfile.h b/cpp/MotionMagic/src/main/include/sim/TalonFXSimProfile.h index 37df50f6..6019724f 100644 --- a/cpp/MotionMagic/src/main/include/sim/TalonFXSimProfile.h +++ b/cpp/MotionMagic/src/main/include/sim/TalonFXSimProfile.h @@ -9,7 +9,7 @@ * Holds information about a simulated TalonFX. */ class TalonFXSimProfile : public SimProfile { - units::ohm_t kMotorResistance = 2_mOhm; // Assume 2mOhm resistance for voltage drop calculation + static constexpr units::ohm_t kMotorResistance = 2_mOhm; // Assume 2mOhm resistance for voltage drop calculation frc::sim::DCMotorSim _motorSim; ctre::phoenix6::hardware::TalonFX& _falcon; diff --git a/cpp/Pigeon2/build.gradle b/cpp/Pigeon2/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/Pigeon2/build.gradle +++ b/cpp/Pigeon2/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/PositionClosedLoop/build.gradle b/cpp/PositionClosedLoop/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/PositionClosedLoop/build.gradle +++ b/cpp/PositionClosedLoop/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/Simulation/build.gradle b/cpp/Simulation/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/Simulation/build.gradle +++ b/cpp/Simulation/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/Simulation/src/main/include/Robot.h b/cpp/Simulation/src/main/include/Robot.h index fa8fd899..7fc0cc03 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::Falcon500(2), + frc::DCMotor::Falcon500FOC(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/VelocityClosedLoop/build.gradle b/cpp/VelocityClosedLoop/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/VelocityClosedLoop/build.gradle +++ b/cpp/VelocityClosedLoop/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/cpp/WaitForAll/build.gradle b/cpp/WaitForAll/build.gradle index 07f7fc67..b4a566f2 100644 --- a/cpp/WaitForAll/build.gradle +++ b/cpp/WaitForAll/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/java/BasicLatencyCompensation/build.gradle b/java/BasicLatencyCompensation/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/BasicLatencyCompensation/build.gradle +++ b/java/BasicLatencyCompensation/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/BasicLatencyCompensation/src/main/java/frc/robot/Robot.java b/java/BasicLatencyCompensation/src/main/java/frc/robot/Robot.java index 71b42de7..79532c66 100644 --- a/java/BasicLatencyCompensation/src/main/java/frc/robot/Robot.java +++ b/java/BasicLatencyCompensation/src/main/java/frc/robot/Robot.java @@ -21,21 +21,21 @@ * project. */ public class Robot extends TimedRobot { - private final String CANBUS_NAME = "Fred"; - private CANcoder m_cc = new CANcoder(0, CANBUS_NAME); - private TalonFX m_fx = new TalonFX(0, CANBUS_NAME); - private Pigeon2 m_p2 = new Pigeon2(0, CANBUS_NAME); + 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 int m_printCount = 0; - private DutyCycleOut m_dutycycle = new DutyCycleOut(0); + private final DutyCycleOut m_dutycycle = new DutyCycleOut(0); - XboxController m_joystick = new XboxController(0); + private final XboxController m_joystick = new XboxController(0); - StatusSignal m_ccpos = m_cc.getPosition(); - StatusSignal m_fxpos = m_fx.getPosition(); - StatusSignal m_p2yaw = m_p2.getYaw(); - StatusSignal m_ccvel = m_cc.getVelocity(); - StatusSignal m_fxvel = m_fx.getVelocity(); + 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(); /** * Pigeon2 can only perform this latency compensation if the Z axis is straight up, since the * angular velocity Z value comes from the pre-mount orientation gyroscope. @@ -43,7 +43,7 @@ public class Robot extends TimedRobot { * see section 1.6 of the Pigeon 2's User's Guide * https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf */ - StatusSignal m_p2yawRate = m_p2.getAngularVelocityZ(); + private final StatusSignal m_p2yawRate = m_p2.getAngularVelocityZ(); /** * This function is run when the robot is first started up and should be used for any diff --git a/java/CANcoder/.Glass/glass.json b/java/CANcoder/.Glass/glass.json new file mode 100644 index 00000000..9ec43e9c --- /dev/null +++ b/java/CANcoder/.Glass/glass.json @@ -0,0 +1,24 @@ +{ + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "open": true + } + }, + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/mech2d": "Mechanism2d" + }, + "windows": { + "/SmartDashboard/mech2d": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "7762" + } +} diff --git a/java/CANcoder/.vscode/launch.json b/java/CANcoder/.vscode/launch.json index 5b804e84..769028f0 100644 --- a/java/CANcoder/.vscode/launch.json +++ b/java/CANcoder/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Launch Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "CANcoder" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/java/CANcoder/build.gradle b/java/CANcoder/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/CANcoder/build.gradle +++ b/java/CANcoder/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/CANcoder/src/main/java/frc/robot/Mechanisms.java b/java/CANcoder/src/main/java/frc/robot/Mechanisms.java new file mode 100644 index 00000000..eb4d257f --- /dev/null +++ b/java/CANcoder/src/main/java/frc/robot/Mechanisms.java @@ -0,0 +1,43 @@ +package frc.robot; + +import com.ctre.phoenix6.StatusSignal; + +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +/** + * Class to keep all the mechanism-specific objects together and out of the main example + */ +public class Mechanisms { + private final double HEIGHT = 1; + private final double WIDTH = 1; + private final double ROOT_X = WIDTH / 2; + private final double ROOT_Y = HEIGHT / 2; + + private Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); // Main mechanism object + private MechanismLigament2d wrist = mech.getRoot("base", ROOT_X, ROOT_Y) + .append(new MechanismLigament2d("Wrist", .25, 90, 6, new Color8Bit(Color.kAliceBlue))); + + @SuppressWarnings("unused") + private MechanismLigament2d leftArrow = wrist.append( + new MechanismLigament2d("LeftArrow", + 0.1, + 150, + 6, + new Color8Bit(Color.kAliceBlue))); + @SuppressWarnings("unused") + private MechanismLigament2d rightArrow = wrist.append( + new MechanismLigament2d("RightArrow", + 0.1, + -150, + 6, + new Color8Bit(Color.kAliceBlue))); + + public void update(StatusSignal angle) { + SmartDashboard.putData("mech2d", mech); // Creates a mech2d window in GUI + wrist.setAngle(angle.getValue() * 360); // Converts 1 rotation to 360 degrees + } +} diff --git a/java/CANcoder/src/main/java/frc/robot/Robot.java b/java/CANcoder/src/main/java/frc/robot/Robot.java index 025e9103..aba8d095 100644 --- a/java/CANcoder/src/main/java/frc/robot/Robot.java +++ b/java/CANcoder/src/main/java/frc/robot/Robot.java @@ -5,10 +5,14 @@ package frc.robot; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.CANcoder; +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 @@ -17,11 +21,18 @@ * project. */ public class Robot extends TimedRobot { - private final double PRINT_PERIOD = 0.5; // Update every 500 ms + 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 DutyCycleOut fwdOut = new DutyCycleOut(0); + private final XboxController controller = new XboxController(0); - private final CANcoder cancoder = new CANcoder(1, "rio"); private double currentTime = Timer.getFPGATimestamp(); + private final Mechanisms mechanism = new Mechanisms(); + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -32,7 +43,6 @@ public void robotInit() { var toApply = new CANcoderConfiguration(); /* User can change the configs if they want, or leave it empty for factory-default */ - cancoder.getConfigurator().apply(toApply); /* Speed up signals to an appropriate rate */ @@ -79,6 +89,7 @@ public void robotPeriodic() { */ System.out.println(); } + mechanism.update(cancoder.getPosition()); } @Override @@ -98,7 +109,10 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + /* Send control requests to control Talon that's connected to CANcoder */ + talonFX.setControl(fwdOut.withOutput(controller.getLeftY())); + } @Override public void disabledInit() {} @@ -113,8 +127,12 @@ public void testInit() {} public void testPeriodic() {} @Override - public void simulationInit() {} + public void simulationInit() { + PhysicsSim.getInstance().addTalonFX(talonFX, cancoder, 25, 0.001); + } @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + PhysicsSim.getInstance().run(); + } } diff --git a/java/CANcoder/src/main/java/frc/robot/sim/PhysicsSim.java b/java/CANcoder/src/main/java/frc/robot/sim/PhysicsSim.java new file mode 100644 index 00000000..09c642cf --- /dev/null +++ b/java/CANcoder/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.CANcoder; +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, CANcoder can, double gearRatio, final double rotorInertia) { + if (falcon != null) { + TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, can, gearRatio, 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/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java new file mode 100644 index 00000000..3c581a07 --- /dev/null +++ b/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -0,0 +1,67 @@ +package frc.robot.sim; + +import com.ctre.phoenix6.hardware.CANcoder; +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 static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation + private final TalonFX _falcon; + private final CANcoder _canCoder; + private final double _gearRatio; + + 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 CANcoder canCoder, final double gearRatio, final double rotorInertia) { + this._falcon = falcon; + this._canCoder = canCoder; + this._gearRatio = gearRatio; + this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), gearRatio, .001); + } + + /** + * 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 + 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); + + _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); + + _canCoder.getSimState().setRawPosition(position_rot / _gearRatio); + _canCoder.getSimState().setVelocity(velocity_rps / _gearRatio); + } +} \ No newline at end of file diff --git a/java/CommandBasedDrive/build.gradle b/java/CommandBasedDrive/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/CommandBasedDrive/build.gradle +++ b/java/CommandBasedDrive/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { 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 80019a3d..09fc3d0a 100644 --- a/java/CommandBasedDrive/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/java/CommandBasedDrive/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -56,7 +56,7 @@ public class DriveSubsystem extends SubsystemBase { /* Simulation model of the drivetrain */ private final DifferentialDrivetrainSim m_driveSim = new DifferentialDrivetrainSim( - DCMotor.getFalcon500(2), // 2 CIMS on each side of the drivetrain. + DCMotor.getFalcon500Foc(2), // 2 CIMS 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. diff --git a/java/ControlRequestLimits/build.gradle b/java/ControlRequestLimits/build.gradle index 3b9dc0de..e2b78281 100644 --- a/java/ControlRequestLimits/build.gradle +++ b/java/ControlRequestLimits/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/CurrentLimits/build.gradle b/java/CurrentLimits/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/CurrentLimits/build.gradle +++ b/java/CurrentLimits/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/CurrentLimits/src/main/java/frc/robot/Robot.java b/java/CurrentLimits/src/main/java/frc/robot/Robot.java index 9ad3b868..6fb7190c 100644 --- a/java/CurrentLimits/src/main/java/frc/robot/Robot.java +++ b/java/CurrentLimits/src/main/java/frc/robot/Robot.java @@ -19,12 +19,12 @@ * project. */ public class Robot extends TimedRobot { - TalonFX m_fx = new TalonFX(0, "Fred"); - DutyCycleOut m_output = new DutyCycleOut(0); + private final TalonFX m_fx = new TalonFX(0, "canivore"); + private final DutyCycleOut m_output = new DutyCycleOut(0); - CurrentLimitsConfigs m_currentLimits = new CurrentLimitsConfigs(); + private final CurrentLimitsConfigs m_currentLimits = new CurrentLimitsConfigs(); - XboxController m_joystick = new XboxController(0); + private final XboxController m_joystick = new XboxController(0); int printCount = 0; diff --git a/java/Falcon500ArcadeDrive/build.gradle b/java/Falcon500ArcadeDrive/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/Falcon500ArcadeDrive/build.gradle +++ b/java/Falcon500ArcadeDrive/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/Falcon500ArcadeDrive/src/main/java/frc/robot/Robot.java b/java/Falcon500ArcadeDrive/src/main/java/frc/robot/Robot.java index 672fc836..7f76f2d0 100644 --- a/java/Falcon500ArcadeDrive/src/main/java/frc/robot/Robot.java +++ b/java/Falcon500ArcadeDrive/src/main/java/frc/robot/Robot.java @@ -21,7 +21,7 @@ * project. */ public class Robot extends TimedRobot { - private final String CANBUS_NAME = ""; + 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); diff --git a/java/FusedCANcoder/build.gradle b/java/FusedCANcoder/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/FusedCANcoder/build.gradle +++ b/java/FusedCANcoder/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/FusedCANcoder/src/main/java/frc/robot/Mechanisms.java b/java/FusedCANcoder/src/main/java/frc/robot/Mechanisms.java new file mode 100644 index 00000000..17f6cafc --- /dev/null +++ b/java/FusedCANcoder/src/main/java/frc/robot/Mechanisms.java @@ -0,0 +1,71 @@ +package frc.robot; + +import com.ctre.phoenix6.StatusSignal; + +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +/** + * Class to keep all the mechanism-specific objects together and out of the main example + */ +public class Mechanisms { + double HEIGHT = 1; //Controls the height of the mech2d SmartDashboard + double WIDTH = 1; //Controls the height of the mech2d SmartDashboard + double FX = 0; + double TALON = 0; + double CAN = 0; + + + Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); + /* rotor rotor Ligaments */ + MechanismLigament2d rotorArm = mech. + getRoot("rotorpivotPoint", 0.25, 0.75). + append(new MechanismLigament2d("rotorArm", .1, 0, 0, new Color8Bit(Color.kAliceBlue))); + + MechanismLigament2d rotorSide1 = rotorArm.append(new MechanismLigament2d("rotorSide1", 0.076535, 112.5, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d rotorSide2 = rotorSide1.append(new MechanismLigament2d("rotorSide2", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d rotorSide3 = rotorSide2.append(new MechanismLigament2d("rotorSide3", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d rotorSide4 = rotorSide3.append(new MechanismLigament2d("rotorSide4", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d rotorSide5 = rotorSide4.append(new MechanismLigament2d("rotorSide5", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d rotorSide6 = rotorSide5.append(new MechanismLigament2d("rotorSide6", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d rotorSide7 = rotorSide6.append(new MechanismLigament2d("rotorSide7", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d rotorSide8 = rotorSide7.append(new MechanismLigament2d("rotorSide8", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + /* CANcoder ligaments */ + MechanismLigament2d ccArm = mech. + getRoot("CANpivotPoint", 0.25, 0.25). + append(new MechanismLigament2d("ccArm", .1, 0, 0, new Color8Bit(Color.kAliceBlue))); + + MechanismLigament2d ccSide1 = ccArm.append(new MechanismLigament2d("ccSide1", 0.076535, 112.5, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d ccSide2 = ccSide1.append(new MechanismLigament2d("ccSide2", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d ccSide3 = ccSide2.append(new MechanismLigament2d("ccSide3", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d ccSide4 = ccSide3.append(new MechanismLigament2d("ccSide4", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d ccSide5 = ccSide4.append(new MechanismLigament2d("ccSide5", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d ccSide6 = ccSide5.append(new MechanismLigament2d("ccSide6", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d ccSide7 = ccSide6.append(new MechanismLigament2d("ccSide7", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d ccSide8 = ccSide7.append(new MechanismLigament2d("ccSide8", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); + /* FX Mechanism Ligaments */ + MechanismLigament2d mechanismArm = mech. + getRoot("TALONpivotPoint", 0.75, 0.5). + append(new MechanismLigament2d("mechanismArm", .2, 0, 0, new Color8Bit(Color.kAliceBlue))); + + MechanismLigament2d mechanismSide1 = mechanismArm.append(new MechanismLigament2d("mechanismSide1", 0.15307, 112.5, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d mechanismSide2 = mechanismSide1.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d mechanismSide3 = mechanismSide2.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d mechanismSide4 = mechanismSide3.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d mechanismSide5 = mechanismSide4.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d mechanismSide6 = mechanismSide5.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d mechanismSide7 = mechanismSide6.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d mechanismSide8 = mechanismSide7.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + + + public void update(StatusSignal fxRotorPosition, StatusSignal fxPosition, StatusSignal cancoderPosition) { + + rotorArm.setAngle(fxRotorPosition.refresh().getValue() * 360); + mechanismArm.setAngle(fxPosition.refresh().getValue() * 360); + ccArm.setAngle(cancoderPosition.refresh().getValue() * 360); + SmartDashboard.putData("mech2d", mech); // Creates mech2d in SmartDashboard + } +} diff --git a/java/FusedCANcoder/src/main/java/frc/robot/Robot.java b/java/FusedCANcoder/src/main/java/frc/robot/Robot.java index 5c54bd29..8240f566 100644 --- a/java/FusedCANcoder/src/main/java/frc/robot/Robot.java +++ b/java/FusedCANcoder/src/main/java/frc/robot/Robot.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.TimedRobot; 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 @@ -24,24 +25,29 @@ * project. */ public class Robot extends TimedRobot { - TalonFX m_fx = new TalonFX(1, "fred"); - CANcoder m_cc = new CANcoder(1, "fred"); - StatusSignal f_fusedSensorOutOfSync = m_fx.getFault_FusedSensorOutOfSync(); - StatusSignal sf_fusedSensorOutOfSync = m_fx.getStickyFault_FusedSensorOutOfSync(); - StatusSignal f_remoteSensorInvalid = m_fx.getFault_RemoteSensorDataInvalid(); - StatusSignal sf_remoteSensorInvalid = m_fx.getStickyFault_RemoteSensorDataInvalid(); + private static final String canBusName = "canivore"; + private final TalonFX m_fx = new TalonFX(1, canBusName); + private final CANcoder m_cc = new CANcoder(1, canBusName); - StatusSignal fx_pos = m_fx.getPosition(); - StatusSignal fx_vel = m_fx.getVelocity(); - StatusSignal cc_pos = m_cc.getPosition(); - StatusSignal cc_vel = m_cc.getVelocity(); + 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(); - DutyCycleOut m_dutyCycleControl = new DutyCycleOut(0); + 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(); - XboxController m_joystick = new XboxController(0); + private final DutyCycleOut m_dutyCycleControl = new DutyCycleOut(0); + + private final XboxController m_joystick = new XboxController(0); + + private int printCount = 0; + + private final Mechanisms m_mechanism = new Mechanisms(); - int printCount = 0; - /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -90,7 +96,7 @@ public void robotPeriodic() { } } - if(m_joystick.getAButton()) { + if (m_joystick.getAButton()) { /* Clear sticky faults */ m_fx.clearStickyFaults(); } @@ -102,6 +108,7 @@ public void robotPeriodic() { System.out.println("CC Position: " + cc_pos + " CC Vel: " + cc_vel); System.out.println(""); } + m_mechanism.update(fx_rotorPos, cc_pos, fx_pos); } @Override @@ -131,8 +138,12 @@ public void testInit() {} public void testPeriodic() {} @Override - public void simulationInit() {} + public void simulationInit() { + PhysicsSim.getInstance().addTalonFX(m_fx, m_cc, 12.8, 0.001); + } @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + PhysicsSim.getInstance().run(); + } } diff --git a/java/FusedCANcoder/src/main/java/frc/robot/sim/PhysicsSim.java b/java/FusedCANcoder/src/main/java/frc/robot/sim/PhysicsSim.java new file mode 100644 index 00000000..09c642cf --- /dev/null +++ b/java/FusedCANcoder/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.CANcoder; +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, CANcoder can, double gearRatio, final double rotorInertia) { + if (falcon != null) { + TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, can, gearRatio, 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/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java new file mode 100644 index 00000000..030df799 --- /dev/null +++ b/java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -0,0 +1,69 @@ +package frc.robot.sim; + +import com.ctre.phoenix6.hardware.CANcoder; +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 static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation + private final TalonFX _falcon; + private final CANcoder _canCoder; + private final double _gearRatio; + + 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 CANcoder canCoder, final double gearRatio, final double rotorInertia) { + this._falcon = falcon; + this._canCoder = canCoder; + this._gearRatio = gearRatio; + this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), gearRatio, 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 + 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); + + _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); + + _canCoder.getSimState().setRawPosition(position_rot / _gearRatio); + _canCoder.getSimState().setVelocity(velocity_rps / _gearRatio); + + } +} \ No newline at end of file diff --git a/java/MotionMagic/build.gradle b/java/MotionMagic/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/MotionMagic/build.gradle +++ b/java/MotionMagic/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/MotionMagic/src/main/java/frc/robot/Mechanisms.java b/java/MotionMagic/src/main/java/frc/robot/Mechanisms.java new file mode 100644 index 00000000..15b441ef --- /dev/null +++ b/java/MotionMagic/src/main/java/frc/robot/Mechanisms.java @@ -0,0 +1,54 @@ +package frc.robot; + +import com.ctre.phoenix6.StatusSignal; + +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +/** + * Class to keep all the mechanism-specific objects together and out of the main example + */ +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 + 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))); + MechanismLigament2d side4 = side3.append(new MechanismLigament2d("side4", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d side5 = side4.append(new MechanismLigament2d("side5", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d side6 = side5.append(new MechanismLigament2d("side6", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d side7 = side6.append(new MechanismLigament2d("side7", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d side8 = side7.append(new MechanismLigament2d("side8", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + + /** + * 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 + * display interface that they find more intuitive or visually appealing. + */ + 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/MotionMagic/src/main/java/frc/robot/Robot.java b/java/MotionMagic/src/main/java/frc/robot/Robot.java index b56a67f7..ce79ce70 100644 --- a/java/MotionMagic/src/main/java/frc/robot/Robot.java +++ b/java/MotionMagic/src/main/java/frc/robot/Robot.java @@ -23,14 +23,17 @@ * project. */ public class Robot extends TimedRobot { - TalonFX m_motor = new TalonFX(1, "fred"); - MotionMagicVoltage m_mmReq = new MotionMagicVoltage(0); - XboxController m_joystick = new XboxController(0); - int m_printCount = 0; + private final TalonFX m_fx = new TalonFX(1, "canivore"); + private final MotionMagicVoltage m_mmReq = new MotionMagicVoltage(0); + private final XboxController m_joystick = new XboxController(0); + + private int m_printCount = 0; + + private final Mechanisms m_mechanisms = new Mechanisms(); @Override public void simulationInit() { - PhysicsSim.getInstance().addTalonFX(m_motor, 0.001); + PhysicsSim.getInstance().addTalonFX(m_fx, 0.001); } @Override @@ -47,28 +50,25 @@ public void robotInit() { TalonFXConfiguration cfg = new TalonFXConfiguration(); /* Configure current limits */ - MotionMagicConfigs mm = new MotionMagicConfigs(); + MotionMagicConfigs mm = cfg.MotionMagic; mm.MotionMagicCruiseVelocity = 5; // 5 rotations per second cruise mm.MotionMagicAcceleration = 10; // Take approximately 0.5 seconds to reach max vel // Take approximately 0.2 seconds to reach max accel mm.MotionMagicJerk = 50; - cfg.MotionMagic = mm; - Slot0Configs slot0 = new Slot0Configs(); + Slot0Configs slot0 = cfg.Slot0; slot0.kP = 60; slot0.kI = 0; slot0.kD = 0.1; slot0.kV = 0.12; slot0.kS = 0.25; // Approximately 0.25V to get the mechanism moving - cfg.Slot0 = slot0; - FeedbackConfigs fdb = new FeedbackConfigs(); + FeedbackConfigs fdb = cfg.Feedback; fdb.SensorToMechanismRatio = 12.8; - cfg.Feedback = fdb; StatusCode status = StatusCode.StatusCodeNotInitialized; for(int i = 0; i < 5; ++i) { - status = m_motor.getConfigurator().apply(cfg); + status = m_fx.getConfigurator().apply(cfg); if (status.isOK()) break; } if (!status.isOK()) { @@ -80,10 +80,11 @@ public void robotInit() { public void robotPeriodic() { if (m_printCount++ > 10) { m_printCount = 0; - System.out.println("Pos: " + m_motor.getPosition()); - System.out.println("Vel: " + m_motor.getVelocity()); + System.out.println("Pos: " + m_fx.getPosition()); + System.out.println("Vel: " + m_fx.getVelocity()); System.out.println(); } + m_mechanisms.update(m_fx.getPosition(), m_fx.getVelocity()); } @Override @@ -101,9 +102,9 @@ public void teleopPeriodic() { double leftY = m_joystick.getLeftY(); if(leftY > -0.1 && leftY < 0.1) leftY = 0; - m_motor.setControl(m_mmReq.withPosition(leftY * 10).withSlot(0)); + m_fx.setControl(m_mmReq.withPosition(leftY * 10).withSlot(0)); if(m_joystick.getBButton()) { - m_motor.setPosition(1); + m_fx.setPosition(1); } } 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 9ec300b0..9899bc31 100644 --- a/java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -11,7 +11,7 @@ * Holds information about a simulated TalonFX. */ class TalonFXSimProfile extends SimProfile { - private final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation + private static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation private final TalonFX _falcon; private final DCMotorSim _motorSim; @@ -31,7 +31,7 @@ class TalonFXSimProfile extends SimProfile { */ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { this._falcon = falcon; - this._motorSim = new DCMotorSim(DCMotor.getFalcon500(1), 1.0, rotorInertia); + this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, rotorInertia); } /** @@ -49,9 +49,10 @@ public void run() { _motorSim.update(getPeriod()); /// SET SIM PHYSICS INPUTS - double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); + final double position_rot = _motorSim.getAngularPositionRotations(); + final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); - _falcon.getSimState().setRawRotorPosition(_motorSim.getAngularPositionRotations()); + _falcon.getSimState().setRawRotorPosition(position_rot); _falcon.getSimState().setRotorVelocity(velocity_rps); _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); diff --git a/java/Pigeon2/build.gradle b/java/Pigeon2/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/Pigeon2/build.gradle +++ b/java/Pigeon2/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/Pigeon2/src/main/java/frc/robot/Mechanisms.java b/java/Pigeon2/src/main/java/frc/robot/Mechanisms.java new file mode 100644 index 00000000..8eb6100d --- /dev/null +++ b/java/Pigeon2/src/main/java/frc/robot/Mechanisms.java @@ -0,0 +1,34 @@ +package frc.robot; + +import com.ctre.phoenix6.StatusSignal; + +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +/** + * Class to keep all the mechanism-specific objects together and out of the main example + */ +public class Mechanisms { + private final double HEIGHT = 1; + private final double WIDTH = 1; + private final double ROOT_X = WIDTH / 2; + private final double ROOT_Y = HEIGHT / 2; + + private Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); // Main mechanism object + private MechanismLigament2d wrist = mech. + getRoot("base", ROOT_X, ROOT_Y). + append(new MechanismLigament2d("Wrist", .25, 90, 6, new Color8Bit(Color.kAliceBlue))); + + @SuppressWarnings("unused") + private MechanismLigament2d leftArrow = wrist.append(new MechanismLigament2d("LeftArrow", 0.1, 150, 6, new Color8Bit(Color.kAliceBlue))); + @SuppressWarnings("unused") + private MechanismLigament2d rightArrow = wrist.append(new MechanismLigament2d("RightArrow", 0.1, -150, 6, new Color8Bit(Color.kAliceBlue))); + + public void update(StatusSignal position) { + wrist.setAngle(position.getValue()); + SmartDashboard.putData("mech2d", mech); + } +} diff --git a/java/Pigeon2/src/main/java/frc/robot/Robot.java b/java/Pigeon2/src/main/java/frc/robot/Robot.java index 79953866..047d0efc 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 @@ -17,11 +21,18 @@ * project. */ public class Robot extends TimedRobot { - private final double PRINT_PERIOD = 0.5; // Update every 500 ms + private static 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 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 * initialization code. @@ -79,6 +90,7 @@ public void robotPeriodic() { */ System.out.println(); } + mechanisms.update(pidgey.getYaw()); } @Override @@ -98,7 +110,9 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + talonfx.setControl(control.withOutput(joystick.getLeftY())); + } @Override public void disabledInit() {} @@ -113,8 +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() {} + public void simulationPeriodic() { + 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..36f7c05f --- /dev/null +++ b/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -0,0 +1,66 @@ +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 static 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.getFalcon500Foc(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 + final double position_rot = _motorSim.getAngularPositionRotations(); + final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); + + _falcon.getSimState().setRawRotorPosition(position_rot); + _falcon.getSimState().setRotorVelocity(velocity_rps); + + _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); + + _pigeon.getSimState().setRawYaw(position_rot); + + } +} \ No newline at end of file diff --git a/java/PositionClosedLoop/.Glass/glass.json b/java/PositionClosedLoop/.Glass/glass.json new file mode 100644 index 00000000..d3e9a08a --- /dev/null +++ b/java/PositionClosedLoop/.Glass/glass.json @@ -0,0 +1,19 @@ +{ + "NetworkTables": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/mech2d": "Mechanism2d" + }, + "windows": { + "/SmartDashboard/mech2d": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "7762" + } +} diff --git a/java/PositionClosedLoop/build.gradle b/java/PositionClosedLoop/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/PositionClosedLoop/build.gradle +++ b/java/PositionClosedLoop/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/PositionClosedLoop/src/main/java/frc/robot/Mechanisms.java b/java/PositionClosedLoop/src/main/java/frc/robot/Mechanisms.java new file mode 100644 index 00000000..7a82c7ca --- /dev/null +++ b/java/PositionClosedLoop/src/main/java/frc/robot/Mechanisms.java @@ -0,0 +1,42 @@ +package frc.robot; + +import com.ctre.phoenix6.StatusSignal; + +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +/** + * Class to keep all the mechanism-specific objects together and out of the main example + */ +public class Mechanisms { + private final double HEIGHT = .5; // Controls the height of the mech2d SmartDashboard + private final double WIDTH = 1; // Controls the height of the mech2d SmartDashboard + + Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); + MechanismLigament2d distanceBar = mech. + getRoot("startPoint", 0.5, 0.4). + append(new MechanismLigament2d("distanceBar", 1, 0, 6, new Color8Bit(Color.kAliceBlue))); + + MechanismLigament2d reference = mech. + getRoot("baseLine", 0, .1). + append(new MechanismLigament2d("baseLine", 1, 0, 6, new Color8Bit(Color.kCyan))); + + MechanismLigament2d joint = mech. + getRoot("neckLine", 0.5, .1). + append(new MechanismLigament2d("neckLine", 0.3, 90, 6, new Color8Bit(Color.kCyan))); + + /** + * 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 + * display interface that they find more intuitive or visually appealing. + */ + public void update(StatusSignal position) { + distanceBar.setLength(position.getValue()/30); // Divide by 30 to scale motion to fit in the window + SmartDashboard.putData("mech2d", mech); // Creates mech2d in SmartDashboard + } +} diff --git a/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java b/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java index b56aa19d..f4783807 100644 --- a/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java +++ b/java/PositionClosedLoop/src/main/java/frc/robot/Robot.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.TimedRobot; 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 @@ -21,7 +22,7 @@ * project. */ public class Robot extends TimedRobot { - private final TalonFX m_fx = new TalonFX(0); + private final TalonFX m_fx = new TalonFX(0, "canivore"); /* Be able to switch which control request to use based on a button press */ /* Start at position 0, enable FOC, no feed forward, use slot 0 */ @@ -33,6 +34,8 @@ public class Robot extends TimedRobot { private final XboxController m_joystick = new XboxController(0); + private final Mechanisms m_mechanism = new Mechanisms(); + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -40,7 +43,7 @@ public class Robot extends TimedRobot { @Override public void robotInit() { TalonFXConfiguration configs = new TalonFXConfiguration(); - configs.Slot0.kP = 24; // An error of 0.5 rotations results in 12V 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; @@ -67,7 +70,9 @@ public void robotInit() { } @Override - public void robotPeriodic() {} + public void robotPeriodic() { + m_mechanism.update(m_fx.getPosition()); + } @Override public void autonomousInit() {} @@ -81,6 +86,9 @@ public void teleopInit() {} @Override public void teleopPeriodic() { double desiredRotations = m_joystick.getLeftY() * 10; // Go for plus/minus 10 rotations + if (Math.abs(desiredRotations) <= 0.1) { // Joystick deadzone + desiredRotations = 0; + } if (m_joystick.getLeftBumper()) { /* Use voltage position */ m_fx.setControl(m_voltagePosition.withPosition(desiredRotations)); @@ -108,8 +116,12 @@ public void testInit() {} public void testPeriodic() {} @Override - public void simulationInit() {} + public void simulationInit() { + PhysicsSim.getInstance().addTalonFX(m_fx, 0.001); + } @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + PhysicsSim.getInstance().run(); + } } diff --git a/java/PositionClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java b/java/PositionClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java new file mode 100644 index 00000000..e1571a39 --- /dev/null +++ b/java/PositionClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java @@ -0,0 +1,81 @@ +package frc.robot.sim; + +import java.util.ArrayList; + +import com.ctre.phoenix6.Utils; +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 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); + } + } + + /** + * 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/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java new file mode 100644 index 00000000..87ab6b72 --- /dev/null +++ b/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -0,0 +1,61 @@ +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.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 static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation + + private final DCMotorSim _motorSim; + private final TalonFXSimState _falconSim; + + /** + * 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 double rotorInertia) { + this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, rotorInertia); + this._falconSim = falcon.getSimState(); + } + + /** + * 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(_falconSim.getMotorVoltage()); + + _motorSim.update(getPeriod()); + + /// SET SIM PHYSICS INPUTS + final double position_rot = _motorSim.getAngularPositionRotations(); + final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); + + _falconSim.setRawRotorPosition(position_rot); + _falconSim.setRotorVelocity(velocity_rps); + + _falconSim.setSupplyVoltage(12 - _falconSim.getSupplyCurrent() * kMotorResistance); + } +} \ No newline at end of file diff --git a/java/Simulation/build.gradle b/java/Simulation/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/Simulation/build.gradle +++ b/java/Simulation/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/Simulation/src/main/java/frc/robot/Robot.java b/java/Simulation/src/main/java/frc/robot/Robot.java index 0a2a4c55..ecc62fc8 100644 --- a/java/Simulation/src/main/java/frc/robot/Robot.java +++ b/java/Simulation/src/main/java/frc/robot/Robot.java @@ -68,7 +68,7 @@ public class Robot extends TimedRobot { /* Simulation model of the drivetrain */ private final DifferentialDrivetrainSim m_driveSim = new DifferentialDrivetrainSim( - DCMotor.getFalcon500(2), // 2 CIMS on each side of the drivetrain. + DCMotor.getFalcon500Foc(2), // 2 CIMS 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. diff --git a/java/SwerveWithPathPlanner/build.gradle b/java/SwerveWithPathPlanner/build.gradle index 94b78617..a581ae74 100644 --- a/java/SwerveWithPathPlanner/build.gradle +++ b/java/SwerveWithPathPlanner/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/VelocityClosedLoop/.Glass/glass.json b/java/VelocityClosedLoop/.Glass/glass.json new file mode 100644 index 00000000..d3e9a08a --- /dev/null +++ b/java/VelocityClosedLoop/.Glass/glass.json @@ -0,0 +1,19 @@ +{ + "NetworkTables": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/mech2d": "Mechanism2d" + }, + "windows": { + "/SmartDashboard/mech2d": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "7762" + } +} diff --git a/java/VelocityClosedLoop/build.gradle b/java/VelocityClosedLoop/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/VelocityClosedLoop/build.gradle +++ b/java/VelocityClosedLoop/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/VelocityClosedLoop/src/main/java/frc/robot/Mechanisms.java b/java/VelocityClosedLoop/src/main/java/frc/robot/Mechanisms.java new file mode 100644 index 00000000..0e434cf8 --- /dev/null +++ b/java/VelocityClosedLoop/src/main/java/frc/robot/Mechanisms.java @@ -0,0 +1,54 @@ +package frc.robot; + +import com.ctre.phoenix6.StatusSignal; + +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +/** + * Class to keep all the mechanism-specific objects together and out of the main example + */ +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 + 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))); + MechanismLigament2d side4 = side3.append(new MechanismLigament2d("side4", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d side5 = side4.append(new MechanismLigament2d("side5", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d side6 = side5.append(new MechanismLigament2d("side6", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d side7 = side6.append(new MechanismLigament2d("side7", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + MechanismLigament2d side8 = side7.append(new MechanismLigament2d("side8", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); + + /** + * 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 + * display interface that they find more intuitive or visually appealing. + */ + 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/VelocityClosedLoop/src/main/java/frc/robot/Robot.java b/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java index 450b5375..c6daacf0 100644 --- a/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java +++ b/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.TimedRobot; 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 @@ -22,8 +23,9 @@ * project. */ public class Robot extends TimedRobot { - private final TalonFX m_fx = new TalonFX(0); - private final TalonFX m_fllr = new TalonFX(1); + private static final String canBusName = "canivore"; + private final TalonFX m_fx = new TalonFX(0, canBusName); + private final TalonFX m_fllr = new TalonFX(1, canBusName); /* Be able to switch which control request to use based on a button press */ /* Start at velocity 0, enable FOC, no feed forward, use slot 0 */ @@ -35,6 +37,8 @@ public class Robot extends TimedRobot { private final XboxController m_joystick = new XboxController(0); + private final Mechanisms m_mechanisms = new Mechanisms(); + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -76,6 +80,7 @@ public void robotInit() { @Override public void robotPeriodic() { + m_mechanisms.update(m_fx.getPosition(), m_fx.getVelocity()); } @Override @@ -93,6 +98,9 @@ public void teleopPeriodic() { if (joyValue > -0.1 && joyValue < 0.1) joyValue = 0; double desiredRotationsPerSecond = joyValue * 50; // Go for plus/minus 10 rotations per second + if (Math.abs(desiredRotationsPerSecond) <= 1) { // Joystick deadzone + desiredRotationsPerSecond = 0; + } if (m_joystick.getLeftBumper()) { /* Use voltage velocity */ m_fx.setControl(m_voltageVelocity.withVelocity(desiredRotationsPerSecond)); @@ -121,8 +129,12 @@ public void testInit() {} public void testPeriodic() {} @Override - public void simulationInit() {} + public void simulationInit() { + PhysicsSim.getInstance().addTalonFX(m_fx, 0.001); + } @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + PhysicsSim.getInstance().run(); + } } diff --git a/java/VelocityClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java new file mode 100644 index 00000000..e1571a39 --- /dev/null +++ b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.java @@ -0,0 +1,81 @@ +package frc.robot.sim; + +import java.util.ArrayList; + +import com.ctre.phoenix6.Utils; +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 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); + } + } + + /** + * 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/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java new file mode 100644 index 00000000..ef3ffbef --- /dev/null +++ b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -0,0 +1,60 @@ +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.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 static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation + private final TalonFXSimState _falconSim; + 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 double rotorInertia) { + this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, rotorInertia); + this._falconSim = falcon.getSimState(); + } + + /** + * 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(_falconSim.getMotorVoltage()); + + _motorSim.update(getPeriod()); + + /// SET SIM PHYSICS INPUTS + final double position_rot = _motorSim.getAngularPositionRotations(); + final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); + + _falconSim.setRawRotorPosition(position_rot); + _falconSim.setRotorVelocity(velocity_rps); + + _falconSim.setSupplyVoltage(12 - _falconSim.getSupplyCurrent() * kMotorResistance); + } +} \ No newline at end of file diff --git a/java/WaitForAll/build.gradle b/java/WaitForAll/build.gradle index 359cf8ac..8a03f481 100644 --- a/java/WaitForAll/build.gradle +++ b/java/WaitForAll/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } java { diff --git a/java/WaitForAll/src/main/java/frc/robot/Robot.java b/java/WaitForAll/src/main/java/frc/robot/Robot.java index b5788878..92c590cb 100644 --- a/java/WaitForAll/src/main/java/frc/robot/Robot.java +++ b/java/WaitForAll/src/main/java/frc/robot/Robot.java @@ -20,44 +20,45 @@ * project. */ public class Robot extends TimedRobot { - TalonFX m_motor1 = new TalonFX(0, "*"); // Pick the first CANivore bus - Pigeon2 m_pigdey = new Pigeon2(1, "*"); // Pick the first CANivore bus also - TalonFX m_transcientMotor = new TalonFX(20, "*"); // This motor may or may not be on the bus, - // selectively power it to completely test this example - TalonFX m_motor2 = new TalonFX(0, "rio"); // Pick the RIO bus to force a failure we can detect - - StatusSignal m_canbus1signal1 = m_motor1.getPosition(); - StatusSignal m_canbus1signal2 = m_motor1.getVelocity(); - StatusSignal m_canbus1signal3 = m_motor1.getControlMode(); - StatusSignal m_canbus1signal4 = m_pigdey.getYaw(); - StatusSignal m_canbus1signal5 = m_pigdey.getRoll(); + private final TalonFX m_motor1 = new TalonFX(0, "*"); // Pick the first CANivore bus + private final Pigeon2 m_pigdey = new Pigeon2(1, "*"); // Pick the first CANivore bus also + private final TalonFX m_transcientMotor = new TalonFX(20, "*"); // This motor may or may not be on the bus, + // 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(); - StatusSignal m_canbus2signal1 = m_motor2.getPosition(); + private final StatusSignal m_canbus2signal1 = m_motor2.getPosition(); - StatusSignal m_canbus1transcient1 = m_transcientMotor.getPosition(); - StatusSignal m_canbus1transcient2 = m_transcientMotor.getVelocity(); + private final StatusSignal m_canbus1transcient1 = m_transcientMotor.getPosition(); + private final StatusSignal m_canbus1transcient2 = m_transcientMotor.getVelocity(); - StatusSignal[] m_signalsAcrossCANbuses = new StatusSignal[]{ + private final StatusSignal[] m_signalsAcrossCANbuses = new StatusSignal[]{ m_canbus1signal1, m_canbus2signal1 }; - StatusSignal[] m_lotsOfSignals = new StatusSignal[]{ + private final StatusSignal[] m_lotsOfSignals = new StatusSignal[]{ m_canbus1signal1, m_canbus1signal2, m_canbus1signal3, + m_canbus1signal4, m_canbus1signal5 }; - StatusSignal[] m_noSignals = new StatusSignal[]{}; - StatusSignal[] m_tanscientSignals = new StatusSignal[]{ + private final StatusSignal[] m_noSignals = new StatusSignal[]{}; + private final StatusSignal[] m_tanscientSignals = new StatusSignal[]{ m_canbus1signal1, m_canbus1signal2, m_canbus1transcient1, m_canbus1transcient2 }; - XboxController m_joystick = new XboxController(0); // Allow us to see the different errors + private final XboxController m_joystick = new XboxController(0); // Allow us to see the different errors - double m_waitForAllTimeout = 0.1; + private double m_waitForAllTimeout = 0.1; /** * This function is run when the robot is first started up and should be used for any * initialization code.