diff --git a/Docs/2_Architecture/2.10_KitbotSim.md b/Docs/2_Architecture/2.10_KitbotSim.md index 3648892..802cb82 100644 --- a/Docs/2_Architecture/2.10_KitbotSim.md +++ b/Docs/2_Architecture/2.10_KitbotSim.md @@ -12,112 +12,52 @@ In the end your code should be able to do this: ## Code Walkthrough -You'll notice that a lot of this looks similar to the real classes that you added in Lesson [2.8](2.8_KitbotAKitRefactor.md). -This is intentional! +You'll notice that our sim code `extends` our real code, just adding functionality to simulate a mechanism. We want our simulated behavior to be as similar to real-life behavior as possible, otherwise our simulation would not be very valuable. For the most part, writing this will consist of substituting stuff like simulated motors for real motors. -Let's add another IO Implementation. -Like the real IO class, this file will define the `updateInputs()` and `setVolts()` methods, but implement them differently (through sim). -Since we aren't writing this for real hardware, we are going to make `DrivetrainIOSim`. +Let's add the sim IO implementation. +We can use the hardware interfaces from the real implementation and connect them to a simulator (this will make more sense in a minute). -Make a new `DrivetrainIOSim` class that implements `DrivetrainIO` the [same way](2.8_KitbotAKitRefactor.md#drivetrainioreal) you did for `DrivetrainIOReal`. +Make a new `DrivetrainIOSim` class that extends `DrivetrainIO` ```Java -public class DrivetrainIOSim implements DrivetrainIO { - - @Override - public void updateInputs(DrivetrainIOInputs inputs) { - // TODO Auto-generated method stub - - } - - @Override - public void setVolts(double left, double right) { - // TODO Auto-generated method stub - - } -} +public class DrivetrainIOSim extends DrivetrainIO {} ``` Some of these fields we can get directly from a simulated motor. -TalonFX comes with built-in simulation support, so we can treat it like a regular motor for the most part. -Add them the same way you did for `DrivetrainIOReal`. +TalonFX comes with built-in simulation support, so we can use it for our sim. We just need to be able to access it (they're currently private). +Change the "visibility" of the two TalonFX objects in `DrivetrainIO` to `protected`. This allows any subclasses (like `DrivetrainIOSim`) to access them. ```Java -TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); -TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); +protected TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); +protected TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); ``` To get the simulated outputs we can call `talon.getSimState()` and store the results in a variable. -Do this in `updateInputs`. +Do this in `the constructor`. We also need to construct a `super` before we access these (so they actually have values). Store the sim states to instance variables. ```Java -@Override -public void updateInputs(DrivetrainIOInputs inputs) { - var leftSimState = leftTalon.getSimState(); - var rightSimState = rightTalon.getSimState(); - // Snip -} -``` - -Now you can call `simState.getMotorVoltage()` to get the voltage for each motor. -Do this for the `left-` and `rightOutputVolts`. - -```Java -@Override -public void updateInputs(DrivetrainIOInputs inputs) { - // Snip - inputs.leftOutputVolts = leftSimState.getMotorVoltage(); - inputs.rightOutputVolts = rightSimState.getMotorVoltage(); - // Snip -} -``` +TalonFXSimState leftSimState; +TalonFXSimState rightSimState; -We can also get the current from each of the motor's sim states. -There are two types of current we can measure: `TorqueCurrent` and `SupplyCurrent`. -`SupplyCurrent` is like the available current for the motor, and is what the breaker measures. -`TorqueCurrent` is the amount of current the motor is actually using, and is important for heat management. -`TorqueCurrent` is more useful for our purposes since it shows the actual current usage of the motor. +public DrivetrainIOSim(CANbus canbus) { + super(canbus); -```Java -@Override -public void updateInputs(DrivetrainIOInputs inputs) { + leftSimState = leftTalon.getSimState(); + rightSimState = rightTalon.getSimState(); // Snip - inputs.leftCurrentAmps = leftSimState.getTorqueCurrent(); - inputs.leftTempCelsius = 0.0; - inputs.rightCurrentAmps = rightSimState.getTorqueCurrent(); - inputs.rightTempCelsius = 0.0; } ``` -This information is useless without having a way to set the motor voltage. -Copy the `VoltageOut` objects from `DrivetrainIOReal` below the `TalonFX`s. - -```Java -TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); -TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); - -VoltageOut leftVoltage = new VoltageOut(0); -VoltageOut rightVoltage = new VoltageOut(0); -``` - -Then copy the `setVolts` method from `DrivetrainIOReal`. - -```Java -@Override -public void setVolts(double left, double right) { - leftTalon.setControl(leftVoltage.withOutput(left)); - rightTalon.setControl(rightVoltage.withOutput(right)); -} -``` +Note that we don't have to implement `updateInputs()` or `setVolts()` or anything else to make this work. Finally, we need to add a physics sim. This will take these motor voltages and figure out how the mechanisms might actually behave using a mathematical model. WPILib provides the `DifferentialDrivetrainSim` class for this. This class has the `createKitbotSim()` static method to provide a convenient way to set this up with the physical constants for a kitbot. -Start by creating a new `DifferentialDrivetrainSim` called `physicsSim` under the `VoltageOut`s. +Start by creating a new `DifferentialDrivetrainSim` called `physicsSim` as an instance variable. ```Java DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( @@ -128,14 +68,14 @@ DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim ``` The first value in this constructor is the motor for the sim. -Let's assume we're using Falcons and use `KitbotMotor.kDoubleFalcon500PerSide` to generate the constants for this. +Let's assume we're using Falcons and use `KitbotMotor.kSingleFalcon500PerSide` to generate the constants for this. This model assumes we have 2 motors on each side of the drivetrain, although we only programmed one. For a sim this doesn't matter, but for a real robot we would need to add follower motors. We are going to ignore that for this tutorial. ```Java DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( - KitbotMotor.kDoubleFalcon500PerSide, + KitbotMotor.kSingleFalcon500PerSide, null, null, null); @@ -147,7 +87,7 @@ In this case, since the drivetrain is simulated, we can use the standard 8p45 ge ```Java DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( - KitbotMotor.kDoubleFalcon500PerSide, + KitbotMotor.kSingleFalcon500PerSide, KitbotGearing.k8p45, null, null); @@ -158,7 +98,7 @@ By default the kitbot comes with 6 inch wheels. ```Java DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( - KitbotMotor.kDoubleFalcon500PerSide, + KitbotMotor.kSingleFalcon500PerSide, KitbotGearing.k8p45, KitbotWheelSize.kSixInch, null); @@ -172,76 +112,92 @@ Now we have a fully set up physics sim. On real robots we often have to be more careful when finding these physical constants, since they aren't usually so standard, but the kitbot is nice and easy. Make sure to work with mechanical to find these constants in season. -Next we need to actually use the physics sim. -In `updateInputs` add a call to `physicsSim.update()`. +Now, we need our simulation to run periodically. For this, we use a `Notifier`. +A `Notifier` is a thread (another bit of code running in parallel to the main code), which runs the passed-in function on a specified interval. We will use this to simulate the faster loop times on the motor controller. +This doesn't matter as much when using voltage control, but with more advanced controllers, loop times can significantly change the motor output. +First, add a notifier instance variable, and construct it in the constructor. ```Java -@Override - public void updateInputs(DrivetrainIOInputs inputs) { - physicsSim.update(0.020); +private Notifier notifier; - var leftSimState = leftTalon.getSimState(); - var rightSimState = rightTalon.getSimState(); - // Snip +// SNIP... +public DrivetrainIOSim(CANbus canbus) { + // SNIP... + + notifier = new Notifier(() -> { + // Code to run periodically goes here + }); } -``` -This method runs the simulation for the number of seconds passed in. -We use 20 milliseconds, which is the default loop time of the rio. +``` -Next we need up update the inputs to the sim based on the motor voltages. -Call `physicsSim.setInputs()` with the motor sim state voltages. +Next, add an instance variable to store the last time the loop ran +```Java +private double lastLoopTime = 0.0; +private Notifier notifier; +``` +And calculate the amount of time passed since the last loop cycle (this is important for the physics simulation) ```Java -@Override - public void updateInputs(DrivetrainIOInputs inputs) { - physicsSim.update(0.020); +notifier = new Notifier(() -> { + double currentTime = Timer.getTimestamp(); + double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; +}); +``` +Next, tell the TalonFXs (through the sim state) how much voltage they have to work with: +```Java +notifier = new Notifier(() -> { + double currentTime = Timer.getTimestamp(); + double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; + + leftSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + rightSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); +}); +``` +The simulated motor controller will now calculate an output. If this was a real robot, this output would go to a mechanism. Here, we pass it to a simulated mechanism and update the simulation. +```Java +notifier = new Notifier(() -> { + double currentTime = Timer.getTimestamp(); + double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; - var leftSimState = leftTalon.getSimState(); - var rightSimState = rightTalon.getSimState(); + leftSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + rightSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); physicsSim.setInputs(leftSimState.getMotorVoltage(), rightSimState.getMotorVoltage()); - // Snip -} + physicsSim.update(deltaTime); +}); ``` - -We're almost done with `DrivetrainIOSim`! -Now we just need to update the rest of the IOInputs based on the simulator. -Start by making the wheel speeds update with the `physicsSim.getLeftVelocityMetersPerSecond`. -Then update the wheel positions with `physicsSim.getLeftPositionMeters`. - +Next, we pass the new mechanism position and velocity back to the simulated Talon. This allows us to view the mechanism's movement in Advantage Scope, and allows the simulator to use feedback-based control modes (like position control). ```Java -@Override -public void updateInputs(DrivetrainIOInputs inputs) { - // Snip - inputs.leftVelocityMetersPerSecond = physicsSim.getLeftVelocityMetersPerSecond(); - inputs.rightVelocityMetersPerSecond = physicsSim.getRightVelocityMetersPerSecond(); +notifier = new Notifier(() -> { + double currentTime = Timer.getTimestamp(); + double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; - inputs.leftPositionMeters = physicsSim.getLeftPositionMeters(); - inputs.rightPositionMeters = physicsSim.getRightPositionMeters(); - // Snip -} -``` + leftSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + rightSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); -Theres one more thing we should do to help make this sim realistic. -When our motors are running the voltage of the battery will "sag" or drop. -This can reduce our motor's output in the extreme case. -To simulate this use `simState.setSupplyVoltage(RoboRioSim.getVInVoltage())`. -`setSupplyVoltage` will control how much voltage is available to the motor simulation. -`RoboRioSim.getVInVoltage()` will record how much battery our motors should be using, and the effect that will have on the battery. + physicsSim.setInputs(leftSimState.getMotorVoltage(), rightSimState.getMotorVoltage()); + physicsSim.update(deltaTime); -```Java -@Override -public void updateInputs(DrivetrainIOInputs inputs) { - physicsSim.update(0.020); + leftSimState.setRawRotorPosition(physicsSim.getLeftPositionMeters()); + leftSimState.setRotorVelocity(physicsSim.getLeftVelocityMetersPerSecond()); - var leftSimState = leftTalon.getSimState(); - leftSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); + rightSimState.setRawRotorPosition(physicsSim.getRightPositionMeters()); + rightSimState.setRotorVelocity(physicsSim.getRightVelocityMetersPerSecond()); +}); +``` +Note: In this case, we don't care that much about units, but on a real mechanism, you would have to convert the measured mechanism position to angular units, and ensure the positions and velocities are passed in as if they're measured at the rotor (before any gear ratios). - var rightSimState = rightTalon.getSimState(); - rightSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); - // Snip -} +Finally, start the notifier thread going with a period of 0.002s (500 Hz). +```Java +notifier = new Notifier(() -> { + // SNIP.. +}); +notifier.startPeriodic(0.002); ``` Now we're done with the `DrivetrainIOSim` class! ๐ŸŽ‰ @@ -252,7 +208,7 @@ Replace the `io` assignment with the following line: ```Java public class DrivetrainSubsystem extends SubsystemBase { - DrivetrainIO io = Robot.isReal() ? new DrivetrainIOReal() : new DrivetrainIOSim(); + DrivetrainIO io = Robot.isReal() ? new DrivetrainIO() : new DrivetrainIOSim(); } ``` This will now select the correct io object to create, and will now work in sim. diff --git a/Docs/2_Architecture/2.3_CommandBased.md b/Docs/2_Architecture/2.3_CommandBased.md index d964b8d..5fb55c8 100644 --- a/Docs/2_Architecture/2.3_CommandBased.md +++ b/Docs/2_Architecture/2.3_CommandBased.md @@ -12,7 +12,7 @@ They are the "nouns" of our robot, or things that it is. ### Commands Commands are the "verbs" of the robot, or what our robot does. Each Subsystem can be used by one Command at the same time, but Commands may use many Subsystems. -Commands can be composed together, so the `LineUp`, `Extend`, and `Outtake` Commands might be put together to make a `Score` Command. +Commands can be composed together, so the `lineUp`, `extend`, and `outtake` Commands might be put together to make a `score` Command. Because each Subsystem can only be used by one Command at once, we are safe from multiple pieces of code trying to command the same motor to different speeds, for example. Some hardware might not be stored in a Subsystem if multiple things can/should use it at the same time safely. @@ -27,7 +27,7 @@ A Trigger is something which can start a Command. The classic form of this is a button on the driver's controller. Another common type is one which checks if the robot is enabled. One non-obvious Trigger we used in 2024 was one which checked when we had detected a game piece in the robot, which then triggered the Commands to flash our LEDs and vibrate the driver controller. -Triggers can be made of any function that returns a boolean, which makes them very powerful, and can be composed together with boolean operators. +Triggers can be made of any function that returns a boolean (also known as a `BooleanSupplier`), which makes them very powerful, and can be composed together with boolean operators. Triggers can also be bound to a certain activation event. For example, we might want something to happen while a condition is true (`whileTrue()`), when a condition changes from false to true (`onTrue()`), or is true for some amount of time (`debounce()`). @@ -38,7 +38,7 @@ This will be covered in more detail in the [Superstructure article.](2.11_Supers ### Resources -- Read through [this article](https://docs.wpilib.org/en/stable/docs/software/basic-programming/functions-as-data.html) on lambda expressions and functional programming if you haven't already. +- Read through [this article](https://docs.wpilib.org/en/stable/docs/software/basic-programming/functions-as-data.html) on lambda expressions and functional programming if you haven't already. We did also discuss this in lesson 11 of Java training. - Read through [these docs](https://docs.wpilib.org/en/stable/docs/software/commandbased/index.html) until you finish "Organizing Command-Based Robot Projects" OR watch [this video](https://drive.google.com/file/d/1ykFDfXVYk27aHlXYKTAqtj1U2T80Szdj/view?usp=drive_link). Presentation notes for the video are [here](2.4_CommandBasedPresentationNotes.md). diff --git a/Docs/2_Architecture/2.5_KitbotIntro.md b/Docs/2_Architecture/2.5_KitbotIntro.md index 56c01b6..a0ebb89 100644 --- a/Docs/2_Architecture/2.5_KitbotIntro.md +++ b/Docs/2_Architecture/2.5_KitbotIntro.md @@ -26,7 +26,7 @@ Inputs/Outputs: Electronics: -- Each side of the chassis has two Kraken X60 motors, which will work together to power the left and right sides of the chassis. +- Each side of the chassis has two Kraken X60 motors, which will work together to power the left and right sides of the chassis. Each Kraken X60 is controlled by a motor controller called a TalonFX. In code this will look like a total of 4 Talon FX motor controllers, which is the component we can talk to. ## Code Walkthrough @@ -85,14 +85,17 @@ Once you have installed the vendor library, create two TalonFX objects in your s By convention, hardware should have a succinct, descriptive name followed by the type of hardware it is. ```Java -TalonFX leftTalon = new TalonFX(0); -TalonFX rightTalon = new TalonFX(0); +TalonFX leftTalon; +TalonFX rightTalon; /** Creates a new Drivetrain. */ -public DrivetrainSubsystem() {} +public DrivetrainSubsystem(CANbus canbus) { + leftTalon = new TalonFX(0, canbus); + rightTalon = new TalonFX(0, canbus); +} ``` -The number being passed into the constructor for the TalonFXs is the ID number of the motor, which we set using Tuner. +The number being passed into the constructor for the TalonFXs is the ID number of the motor, which we set using Tuner. Each motor must have its own, individual ID. Since we don't have real hardware for this example this number is arbitrary, but it's good practice to have these sorts of constants defined at the top of the file. At the top of the `DrivetrainSubsystem` file, add two `public static final int`s, one for the left motor's ID and one for the right motor, then pass those to the `TalonFX` constructors. @@ -102,8 +105,15 @@ public class DrivetrainSubsystem extends SubsystemBase { public static final int LEFT_TALON_ID = 0; public static final int RIGHT_TALON_ID = 1; - TalonFX leftTalon = new TalonFX(LEFT_TALON_ID); - TalonFX rightTalon = new TalonFX(RIGHT_TALON_ID); + TalonFX leftTalon; + TalonFX rightTalon; + + /** Creates a new Drivetrain. */ + public DrivetrainSubsystem(CANbus canbus) { + leftTalon = new TalonFX(LEFT_TALON_ID, canbus); + rightTalon = new TalonFX(RIGHT_TALON_ID, canbus); + } + //... } ``` @@ -117,14 +127,17 @@ Make sure you import these classes. You can do this by hovering over the red underlined code, hitting "quick fix", and pressing "import . . .". ```Java -TalonFX leftTalon = new TalonFX(LEFT_TALON_ID); -TalonFX rightTalon = new TalonFX(RIGHT_TALON_ID); - VoltageOut leftVoltage = new VoltageOut(0); VoltageOut rightVoltage = new VoltageOut(0); +TalonFX leftTalon; +TalonFX rightTalon; + /** Creates a new Drivetrain. */ -public DrivetrainSubsystem() {} +public DrivetrainSubsystem(CANbus canbus) { + leftTalon = new TalonFX(LEFT_TALON_ID, canbus); + rightTalon = new TalonFX(RIGHT_TALON_ID, canbus); +} ``` We have the control requests now, but we need a way to set them. @@ -137,9 +150,12 @@ private void setVoltages(double left, double right) { } ``` +Here, we use the `setControl()` method to send a control request to the motors. The `.withOutput()` method modifies the request from outputting `0` volts to outputting the passed in voltage. + You might notice that this method is private. This is because whenever we want to interact with the hardware of a subsystem we should go through a Command, which guarantees that each piece of hardware is only requested to do one thing at a time. (If you're feeling unsure about Commands, review the Command Based [article](2.3_CommandBased.md) and/or [presentation](2.4_CommandBasedPresentationNotes.md).) +This prevents issues where one motor has multiple requested behaviors at once, ensuring that each motor can only be accessed once. To do this, we need to make a Command factory method, or a method that returns a `Command`. ```Java @@ -151,7 +167,7 @@ public Command setVoltagesCommand(DoubleSupplier left, DoubleSupplier right) { Notice how instead of passing in `double`s, we pass in `DoubleSupplier`s. A `DoubleSupplier` is just any function that returns a `double`. This function could always return the same value, effectively acting like a double, or it can get its value some other way. -This lets us use this command to drive the robot with joysticks, an autonomous controller, or other input that may change. +This lets us use this command to drive the robot with joysticks, an autonomous controller, or other input that may change after the Command has been created. In the body of the method we return `this.run()`, which implicitly creates a `RunCommand`. `RunCommand` is a subtype of Command, and represents a Command which runs a single function over and over again. @@ -200,12 +216,14 @@ CommandXboxController controller = new CommandXboxController(0); ``` `CommandXboxController` is a convenience wrapper around a `XboxController` that makes it easy to bind commands to buttons on the controller. -We also need to add an instance of the `DrivetrainSubsystem` so that we can control it. +We also need to add an instance of the `DrivetrainSubsystem` so that we can control it, and a `CANbus` instance to give to the TalonFXs. Here, the `*` passed in means "choose whichever CAN bus is attached to a CANivore." This doesn't really matter for sim, but does for real lide. ```Java CommandXboxController controller = new CommandXboxController(0); -DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); +CANbus canbus = new CANbus("*"); + +DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(canbus); ``` Finally, we can bind the arcade drive command to the joysticks. diff --git a/Docs/2_Architecture/2.6_AdvantageKit.md b/Docs/2_Architecture/2.6_AdvantageKit.md index 137482f..88b0db9 100644 --- a/Docs/2_Architecture/2.6_AdvantageKit.md +++ b/Docs/2_Architecture/2.6_AdvantageKit.md @@ -31,7 +31,7 @@ AdvantageKit is closely integrated with AdvantageScope, a log and sim viewer bui ### Drawbacks Running this amount of logging has performance overhead on the rio, using valuable cpu time each loop. -Logging also requires a non-insignificant architecture change to our codebase by using an IO layer under each of our subsystems. +Logging also requires a significant architecture change to our codebase by using an IO layer under each of our subsystems. While this does require some additional effort to write subsystems, it also makes simulating subsystems easier so it is a worthwhile tradeoff. 8033-specific usage of AdvantageKit features will be covered in more detail in the next couple of lessons. @@ -47,7 +47,7 @@ While this does require some additional effort to write subsystems, it also make - [6328 2023 code](https://github.com/Mechanical-Advantage/RobotCode2023) - [3476 2023 code](https://github.com/FRC3476/FRC-2023) -- [8033 2025 code](https://github.com/HighlanderRobotics/Reefscape) +- [8033 2026 code](https://github.com/HighlanderRobotics/Rebuilt) ### Exercises diff --git a/Docs/2_Architecture/2.7_AKitStructureReference.md b/Docs/2_Architecture/2.7_AKitStructureReference.md index 4d40ff1..4962314 100644 --- a/Docs/2_Architecture/2.7_AKitStructureReference.md +++ b/Docs/2_Architecture/2.7_AKitStructureReference.md @@ -13,13 +13,9 @@ flowchart TD Subsystem-->IO subgraph IO direction LR - B[IO]-->E[IOInputs] - end - IO-->IOImpl - subgraph IOImpl - C[IO Implementation Real] - D[IO Implementation Sim] + B[IO-real impl]-->E[IOInputs] end + IO-->C[IOSim] ``` This diagram shows the basic structure of an AKit Subsystem. @@ -34,57 +30,49 @@ It includes 3 layers: - It might contain information about the current target of a mechanism, or whether or not the mechanism has homed its position yet. - *Generally, this information should all be **"processed" information** that we derive from our IOInputs. - The `Subsystem` file will contain one `IO` instance and one `IOInputs` instance, conventionally called `io` and `inputs` respectively. + - Keep in mind: A subsystem can contain multiple IOs that do different things (for example a roller and a pivot might have 2 IOs). You could also inlcude multiple degrees of freedom within one IO, its mostly up to personal preference. ### IO - The "IO" layer defines the interface with our hardware, as well as all the values we will log. -- This includes an `interface` called `SubsystemIO` which defines a set of methods to interact with the hardware, such as `setVoltage` or `getPosition`. -However, it doesn't define *how* to do them, because that's specific to each implementation. -The interface just provides a template. +- This includes an `interface` called `SomethingIO` which defines the real implementation (most of the time) of the hardware interface - This class will also include an `updateInputs` method, which takes in an `IOInputs` object and updates it with the latest sensor data from the mechanism. - The `IOInputs` object is a class that contains various measurements and sensor data about the mechanism, like current draw, encoder position, and voltage output. -- It is marked with `@AutoLog` which means all the inputs and outputs for the subsystem will be automatically recorded in the log, so we can play it back later. +- `IOInputs` is marked with `@AutoLog` which means all the inputs and outputs for the subsystem will be automatically recorded in the log, so we can play it back later. -### IO Implementations +### Sim Implementation -- These are classes which `implement` the aforementioned `IO` class. -- This means they will specify how to do all of the methods defined in the `IO` class. -- Generally we will have 2 types of `IOImplementation`, `IOSim` and `IOReal`. - - `IOReal` defines its methods to command real hardware to have real outputs. - - `IOSim` often looks similar to `IOReal`, but will have some behaviour added to fake the behaviour of real world physics. - This can include a physics sim which approximates physical behaviour, setting outputs which we can't sim to a default value (like temperature), or other ways of "faking" real world behaviour. -- `IOImplementation`s will contain the actual objects such as `TalonFX`s (for `IOReal`) or `DCMotorSim`s (for `IOSim`). +- IOSim extends IO (the real implementation), and adds additional functionality to simulate the movement of the mechanism. ## Intake Example A cad render of the intake from our 2025 offseason robot -This example will cover how the code for an intake such as the one above might be set up. +This example will cover how the code for an intake such as the one above might be set up. We will only walk through the pivot IO because the roller IO is very similar ```mermaid flowchart TD subgraph Subsystem A[IntakeSubsystem] end - Subsystem-->IO - subgraph IO + Subsystem-->PivotIO + subgraph PivotIO direction LR - B[IntakeIO]-->E[IntakeIOInputs] + B[PivotIO]-->D[PivotIOInputs] end - IO-->IOImpl - subgraph IOImpl - C[IntakeIOReal] - D[IntakeIOSim] + PivotIO-->H[PivotIOSim] + Subsystem-->RollerIO + subgraph RollerIO + direction LR + E[RollerIO]-->F[RollerIOInputs] end + RollerIO-->G[RollerIOSim] ``` -Let's start by defining the methods in the `IntakeIO` interface. -There are two motors on this slapdown intake--one that controls the pivot, and one that controls the rollers. +Let's start by defining the methods in the `PivotIO` class. The intake needs to set its position (extended or retracted), so we will need a `setAngle(Rotation2d angle)` method. -We will also need a way to set the rollers' output, so let's add a `setRollerVoltage(double volts)` method. -For convenience, let's add a method `stop()` that calls `setRollerVoltage()` with a voltage of 0. -We also need to add our `IntakeIOInputs` to the `IntakeIO` file. +We also need to add our `PivotIOInputs` to the `PivotIO` file. This should contain all of the sensor information we need to know about our intake so that we can debug its behaviour with logs. Then we can add our logged fields for the motor. @@ -94,6 +82,7 @@ Here is a list of common logged fields for motors: - This is the main field we care about to see if the motor is moving. - Current draw (Amps) - This lets us see if the motor is stalling (trying to move but stuck) as well as how much energy the motor is using. + - We usually log both stator and supply current. Stator current is related to the acceleration and torque of the motor, while supply current is the amount of current drawn from the battery. - Temperature (Celsius) - If a motor gets too hot it will turn itself off to protect its electronics. This lets us see if we are having issues related to that. @@ -111,99 +100,68 @@ Our `IntakeIO` file should look something like this now: ```Java // Imports go here -public interface IntakeIO { +public class PivotIO { @AutoLog - public class IntakeIOInputs { + public class PivotIOInputs { // Pivot motor values - public double pivotVelocityRotationsPerSec; - public double pivotCurrentDrawAmps; - public double pivotTemperatureCelsius; - public double pivotVoltage; - public Rotation2d pivotMotorPosition; - - // Roller motor values - public double rollerVelocityRotationsPerSec; - public double rollerCurrentDrawAmps; - public double rollerTemperatureCelsius; - public double rollerVoltage; + public double velocityRotationsPerSec; + public double statorCurrentAmps; + public double supplyCurrentAmps; + public double temperatureCelsius; + public double voltage; + public Rotation2d motorPosition; } - // Methods that IOImplementations will implement - public void setAngle(Rotation2d angle); + // These still need to be implemented + public void setAngle(Rotation2d angle) {} - public void setRollerVoltage(double volts); - - // Note the use of "default" - // This means that we don't have to re-implement this method in each IOImplementation, because it will default to - // whatever the specific implementation of setVoltage() is - public default void stop() { - setRollerVoltage(0); - } - - public void updateInputs(IntakeIOInputs inputs); + public void updateInputs(PivotIOInputs inputs) {} } ``` -Next, let's write `IntakeIOReal`. -This will contain all of the hardware we want to interact with on the real robot. +Next, let's flesh out this class. +We need to add all of our hardware we want to interact with on the real robot. First, we will need to define the hardware we want to use. -In this case, they will be two `TalonFX`s. +In this case, they will be one `TalonFX` (to interface with a Kraken motor). ```Java -private final TalonFX pivot = new TalonFX(IntakeSubsystem.PIVOT_MOTOR_ID); -private final TalonFX roller = new TalonFX(IntakeSubsystem.ROLLER_MOTOR_ID); +private final TalonFX pivot; ``` Next we will need to implement each of the methods from `IntakeIO`. -Each should `@Override` the template method. For the sake of brevity, I won't cover that in detail here. *MAKE SUBSYSTEM WALKTHROUGH AND LINK HERE* In the end you should have something like: - +TODO: FIX THIS! ```Java public class IntakeIOReal implements IntakeIO { private final TalonFX pivot = new TalonFX(IntakeSubsystem.PIVOT_MOTOR_ID); - private final TalonFX roller = new TalonFX(IntakeSubsystem.ROLLER_MOTOR_ID); - @Override public void updateInputs(IntakeIOInputs inputs) { - // Note that the exact calls here are just examples, and might not work if copy-pasted - - inputs.pivotVelocityRotationsPerSec = pivot.getVelocity(); - inputs.pivotCurrentDrawAmps = pivot.getStatorCurrent(); - inputs.pivotTemperatureCelsius = pivot.getDeviceTemp(); - inputs.pivotVoltage = pivot.getMotorVoltage(); - inputs.pivotMotorPosition = pivot.getMotorPosition(); - - // Roller motor values - inputs.rollerVelocityRotationsPerSec = roller.getVelocity(); - inputs.rollerCurrentDrawAmps = roller.getStatorCurrent(); - inputs.rollerTemperatureCelsius = roller.getDeviceTemp(); - inputs.rollerVoltage = roller.getMotorVoltage(); + // Note that the exact calls here are just examples, and don't work if copy pasted. To see + + inputs.velocityRotationsPerSec = pivot.getVelocity(); + inputs.statorCurrentAmps = pivot.getStatorCurrent(); + inputs.supplyCurrentAmps = pivot.getSupplyCurrent(); + inputs.temperatureCelsius = pivot.getDeviceTemp(); + inputs.voltage = pivot.getMotorVoltage(); + inputs.motorPosition = pivot.getMotorPosition(); } - @Override - public void setRollerVoltage(double volts) { - roller.setVoltage(volts); - } - - // Note how we don't need to define stop() because it has a default implementation that does what we want - - @Override public void setAngle(Rotation2d angle) { - pivot.setAngle(angle); } } ``` We can make a similar class for `IntakeIOSim`, although instead of getting motor outputs directly we would have to use `motor.getSimState()`. -For more information about that, check the [CTRE docs](https://pro.docs.ctr-electronics.com/en/stable/docs/api-reference/simulation/simulation-intro.html). +For more information about that, check the [CTRE docs](https://pro.docs.ctr-electronics.com/en/stable/docs/api-reference/simulation/simulation-intro.html), and see [2.9 Simulation](2.9_Simulation.md). Finally, let's write the `IntakeSubsystem` class. -This class will include an instance of `IntakeIO` and an instance of `IntakeIOInputs`. +This class will include instances of our IOs and instances of their respective inputs. It will also contain Command factories to allow the rest of our code to interface with it. +Lets assume `RollerIO` has been implemented elsewhere. Add the io and io inputs to the class: @@ -211,12 +169,16 @@ Add the io and io inputs to the class: // Snip imports public class IntakeSubsystem extends SubsystemBase { - private final IntakeIO io; - private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + private final PivotIO pivotIo; + private final PivotIOInputsAutoLogged pivotInputs = new PivotIOInputsAutoLogged(); - public IntakeSubsystem(IntakeIO io) { + private final RollerIO rollerIo; + private final RollerIOInputsAutoLogged rollerInputs = new RollerIOInputsAutoLogged(); + + public IntakeSubsystem(PivotIO pivotIo, RollerIO rollerIO) { // Pass in either the sim io or real io - this.io = io; + this.pivotIo = pivotIo; + this.rollerIo = rollerIo; } } ``` @@ -226,15 +188,15 @@ Then we can add a few Command factories to control the subsystem: ```Java public Command intake(double volts) { return Commands.run(() -> { - io.setAngle(INTAKE_ANGLE); - io.setRollerVoltage(volts); + pivotIo.setAngle(INTAKE_ANGLE); + rollerIo.setRollerVoltage(volts); }); } public Command stop() { return Commands.runOnce(() -> { - io.setAngle(RETRACTED_ANGLE); - io.stop(); + pivotIo.setAngle(RETRACTED_ANGLE); + rollerIo.stop(); }); } ``` @@ -244,9 +206,12 @@ Finally, let's add our `periodic()` method to update and log our inputs. ```Java @Override public void periodic() { - io.updateInputs(); + pivotIo.updateInputs(pivotInputs); // Make sure to import the "littletonRobotics" Logger, not one of the other ones. - Logger.processInputs("Intake", inputs); + Logger.processInputs("Intake/Pivot", pivotInputs); + + rollerIo.updateInputs(rollerInputs); + Logger.processInputs("Intake/Roller", rollerInputs); } ``` @@ -256,34 +221,40 @@ Overall, `IntakeSubsystem` should roughly look like: // Snip imports public class IntakeSubsystem extends SubsystemBase { - IntakeIO io; - IntakeIOInputsAutoLogged inputs; + private final PivotIO pivotIo; + private final PivotIOInputsAutoLogged pivotInputs = new PivotIOInputsAutoLogged(); - public IntakeSubsystem(IntakeIO io) { + private final RollerIO rollerIo; + private final RollerIOInputsAutoLogged rollerInputs = new RollerIOInputsAutoLogged(); + + public IntakeSubsystem(PivotIO pivotIo, RollerIO rollerIO) { // Pass in either the sim io or real io - this.io = io; - inputs = new IntakeIOInputsAutoLogged(); + this.pivotIo = pivotIo; + this.rollerIo = rollerIo; } public Command intake(double volts) { return Commands.run(() -> { - io.setAngle(INTAKE_ANGLE); - io.setRollerVoltage(volts); + pivotIo.setAngle(INTAKE_ANGLE); + rollerIo.setRollerVoltage(volts); }); } public Command stop() { return Commands.runOnce(() -> { - io.setAngle(RETRACTED_ANGLE); - io.stop(); + pivotIo.setAngle(RETRACTED_ANGLE); + rollerIo.stop(); }); } @Override public void periodic() { - io.updateInputs(); + pivotIo.updateInputs(pivotInputs); // Make sure to import the "littletonRobotics" Logger, not one of the other ones. - Logger.processInputs("Intake", inputs); + Logger.processInputs("Intake/Pivot", pivotInputs); + + rollerIo.updateInputs(rollerInputs); + Logger.processInputs("Intake/Roller", rollerInputs); } } ``` diff --git a/Docs/2_Architecture/2.8_KitbotAKitRefactor.md b/Docs/2_Architecture/2.8_KitbotAKitRefactor.md index 8938c1b..c889b09 100644 --- a/Docs/2_Architecture/2.8_KitbotAKitRefactor.md +++ b/Docs/2_Architecture/2.8_KitbotAKitRefactor.md @@ -46,17 +46,16 @@ Let's put it in a `Drivetrain` folder under `Subsystems` so that everything stay Next, let's make a new file called `DrivetrainIO` in the same folder. Remember that this file will define all the methods we use to interact with the hardware on the drivetrain. You can use the `Create a new class/command` option when you right click on the folder to speed this up. -Make sure to change the type of `DrivetrainIO` to **interface**, not class. -(Feel free to google that or reference the HeadFirst Java book if you don't remember what that is.) +In general, the base "IO" class represents a real implementation, with the simulated implementation extending. ```Java -package frc.robot.Subsystems.Drivetrain; +package frc.robot.subsystems.drivetrain; -public interface DrivetrainIO {} +public class DrivetrainIO {} ``` -Inside of this interface let's define our `IOInputs` class. -Remember this is a container for all of the inputs (sensor readings) and outputs (motor commands) for this mechanism. +Inside of this class let's define our `IOInputs` class. +Remember this is a container for all of the inputs (sensor readings) for this mechanism. ```Java public static class DrivetrainIOInputs {} @@ -109,6 +108,7 @@ For this sim we will use odometry to give us a position that we can use to visua On a real robot, it can be useful to log motor current draw and temperature. Current draw is the amount of power the motor is actually using to try to reach its desired output, and is measured in Amperes (amps). Checking current draw can be useful for things like verifying [current zeroing](2.2_ElectronicsCrashCourse.md#limit-switches) or to debug issues like a mechanism that draws too much current and causes other mechanisms to brown out. +There are two types of current that we typically log: stator and supply current. Stator current is the current flowing through the windings of the motor. This value increases with acceleration. Supply current is the current flowing through the wires to the motor. It is typically much lower than stator current. The amount of power flowing through these motors also causes them to heat up over the course of a match. When the motors get hotter, they get less efficient or may even shut down to prevent damage. @@ -120,27 +120,28 @@ This simulation will not include temperature simulation, but it's good to get in ```Java public static class DrivetrainIOInputs { // Snip - public double leftCurrentAmps = 0.0; + public double leftStatorCurrentAmps = 0.0; + public double leftSupplyCurrentAmps = 0.0; public double leftTempCelsius = 0.0; - public double rightCurrentAmps = 0.0; + public double rightStatorCurrentAmps = 0.0; + public double rightSupplyCurrentAmps = 0.0; public double rightTempCelsius = 0.0; } ``` -Seeing as we only have 1 motor on each side for this example, it makes more sense to leave these as doubles. -However, if you have multiple motors per side, you may want to make these arrays of doubles. - #### `@AutoLog` + `updateInputs` Finally, to finish this IOInputs class we need to add the `@AutoLog` annotation above its definition. This annotation automatically generates the code to log all this data in a new class called `IOInputsAutoLogged` so we don't have to worry about it. However it is limited to only certain types of values, so be careful when making IOInputs classes. If you want to log other types than `@AutoLog` supports, check out the [AdvantageKit docs](https://docs.advantagekit.org/data-flow/supported-types) on logging protobufs/structs. -Next, we need to add a method called `updateInputs` that takes in the `IOInputs` object and updates it based off of the new values from our sensors. +Next, we need to add a method called `updateInputs` inside of our `DrivetrainIO` class that takes in the `IOInputs` object and updates it based off of the new values from our sensors. This needs to be called periodically to get the most up to date data. ```Java -public void updateInputs(DrivetrainIOInputs inputs); +public void updateInputs(DrivetrainIOInputs inputs) { + +} ``` #### Adding methods @@ -154,7 +155,7 @@ package frc.robot.Subsystems.Drivetrain; import org.littletonrobotics.junction.AutoLog; -public interface DrivetrainIO { +public class DrivetrainIO { @AutoLog public static class DrivetrainIOInputs { public double leftOutputVolts = 0.0; @@ -172,61 +173,37 @@ public interface DrivetrainIO { public double rightTempCelsius = 0.0; } - public void updateInputs(DrivetrainIOInputs inputs); - - public void setVolts(double left, double right); -} -``` - -### `DrivetrainIOReal` -Next, let's add the IO **implementation**. -This will be a file that defines *how* to "do the `updateInputs()` (how do we get and assign the data?) and `setVolts()` (how do we send a voltage to the motor?) methods. - -#### Creating the class -Start by making a new class called `DrivetrainIOReal` in the Drivetrain folder. -Then, add `implements DrivetrainIO` to the class declaration. -This is very similar to using `extends`, except for interfaces instead of classes. -This means we could have multiple interfaces it implements, instead of just one class. - -```Java -public class DrivetrainIOReal implements DrivetrainIO {} -``` - -This will make vscode angry since `DrivetrainIOReal` needs to implement all the template methods in `DrivetrainIO`. -Hover over it, hit "quick fix", and click "add unimplemented methods". -Now you should have a template for both of these methods. - -```Java -public class DrivetrainIOReal implements DrivetrainIO { - - @Override public void updateInputs(DrivetrainIOInputs inputs) { - // TODO Auto-generated method stub - + } - @Override public void setVolts(double left, double right) { - // TODO Auto-generated method stub - + } } ``` +### `Real implementation` + #### Adding hardware Before we get into implementing these, we need to add the hardware to this IO Implementation. Add in the motors from `DrivetrainSubsystem`. ```Java -TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); -TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); +TalonFX leftTalon; +TalonFX rightTalon; + +/** Creates a new Drivetrain. */ +public DrivetrainIO(CANbus canbus) { + leftTalon = new TalonFX(LEFT_TALON_ID, canbus); + rightTalon = new TalonFX(RIGHT_TALON_ID, canbus); +} ``` Let's add all of the needed fields to `updateInputs`, even though we won't be able to set any of them yet. One way to get all of the needed fields is to copy and paste the body of `DrivetrainIOInputs` into `updateInputs`. ```Java -@Override public void updateInputs(DrivetrainIOInputs inputs) { public double leftOutputVolts = 0.0; public double rightOutputVolts = 0.0; @@ -247,7 +224,6 @@ public void updateInputs(DrivetrainIOInputs inputs) { Then replace the `public` and types with `inputs.` ```Java -@Override public void updateInputs(DrivetrainIOInputs inputs) { inputs.leftOutputVolts = 0.0; inputs.rightOutputVolts = 0.0; @@ -265,6 +241,8 @@ public void updateInputs(DrivetrainIOInputs inputs) { } ``` +Now we're accessing the values stored within the inputs object. + #### Status Signals Now, how do we get these values? The `StatusSignal` class is how we can get live data reported by a CTRE device, like position, velocity, etc. @@ -276,7 +254,7 @@ Since we want to know the voltage draw of the left and right motors, we will nee We'll define these member variables at the top of `DrivetrainIOReal`. ```Java -public class DrivetrainIOReal implements DrivetrainIO { +public class DrivetrainIO { TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); @@ -289,7 +267,7 @@ Then, we'll instantiate these signals and match them to the correct motors + sig Call `.getMotorVoltage();` on both of these motors, which returns a `StatusSignal,` and assign that to their respective signals. ```Java -public class DrivetrainIOReal implements DrivetrainIO { +public class DrivetrainIO { TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); @@ -301,7 +279,7 @@ public class DrivetrainIOReal implements DrivetrainIO { Go ahead and fill out the rest of the status signals in the same way. ```Java -public class DrivetrainIOReal implements DrivetrainIO { +public class DrivetrainIO { TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); @@ -326,7 +304,9 @@ The values of these signals will need to be updated every loop (every 20 millise This will need to be set in the `DrivetrainIOReal` constructor, so that it will get called when a new drivetrain object is created. ```Java -public DrivetrainIOReal() { +public DrivetrainIO(CANbus canbus) { + // SNIP + // Sets the following status signals to be updated at a frequency of 50hz BaseStatusSignal.setUpdateFrequencyForAll( 50.0, // update every 20ms @@ -340,14 +320,16 @@ public DrivetrainIOReal() { rightSupplyCurrent, leftTempCelsius, rightTempCelsius); + leftTalon.optimizeBusUtilization(); + rightTalon.optimizeBusUtilization(); } ``` +The call to `optimizeBusUtilization()` ensures that the motor doesn't send any data that doesn't need to be sent. This reduces traffic on the CAN bus. It doesn't matter that much for our simulated drivetrain, but is super important on a real robot! -At the top of `updateInputs()` (which gets called periodically), we're going to refresh all these signals with the `BaseStatusSignal.refreshAll()` method. +At the top of `updateInputs()` (which gets called periodically), we're going to refresh all these signals with the `BaseStatusSignal.refreshAll()` method. This ensures that all of these signals have the most up-to-date data from the motor. We'll pass in all our status signals as parameters. ```Java -@Override public void updateInputs(DrivetrainIOInputs inputs) { BaseStatusSignal.refreshAll(leftAppliedVoltage, rightAppliedVoltage, @@ -369,7 +351,6 @@ You'll need to call some sort of `.getValueAsDouble()` on these signals, since w You might also need to then create a new `Rotation2d` or something similar from that depending on what the field is. ```Java -@Override public void updateInputs(DrivetrainIOInputs inputs) { BaseStatusSignal.refreshAll(leftAppliedVoltage, rightAppliedVoltage, @@ -398,7 +379,7 @@ public void updateInputs(DrivetrainIOInputs inputs) { } ``` -Your `DrivetrainIOReal` class is now (mostly) complete! +Your `DrivetrainIO` class is now (mostly) complete! ### Refactoring `DrivetrainSubsystem` Let's refactor our `DrivetrainSubsystem` to finish off this rewrite. @@ -408,7 +389,7 @@ Get rid of the `TalonFX` objects and `VoltageOut` requests, since those have all ```Java public class DrivetrainSubsystem extends SubsystemBase { - DrivetrainIO io = new DrivetrainIOReal(); + DrivetrainIO io = new DrivetrainIO(); // Snip } ``` @@ -418,7 +399,7 @@ Since we used the `@AutoLog` annotation, the class we actually want to use is `D ```Java public class DrivetrainSubsystem extends SubsystemBase { - DrivetrainIO io = new DrivetrainIOReal(); + DrivetrainIO io = new DrivetrainIO(); DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); } ``` @@ -448,7 +429,7 @@ Finally, let's change the `setVoltages` method to use the io layer. Copy over the lines in `DrivetrainSubsystem` about `setControl` over to `setVolts` in `DrivetrainIOReal`, as well as the `VoltageOut` objects. ```Java -public class DrivetrainIOReal implements DrivetrainIO { +public class DrivetrainIO { TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); @@ -458,7 +439,6 @@ public class DrivetrainIOReal implements DrivetrainIO { //snip - @Override public void setVolts(double left, double right) { leftTalon.setControl(leftVoltage.withOutput(left)); rightTalon.setControl(rightVoltage.withOutput(left)); @@ -466,10 +446,10 @@ public class DrivetrainIOReal implements DrivetrainIO { } ``` -Then in `DrivetrainSubsystem`, replace `setVoltages` with the following: +Then in `DrivetrainSubsystem`, replace `runVoltagesCommand` with the following: ```Java -private void setVoltages(double left, double right) { - io.setVolts(left, right); +public Command setVoltagesCommand(DoubleSupplier left, DoubleSupplier right) { + return this.run(() -> io.setVolts(left.getAsDouble(), right.getAsDouble())); } ``` *This still calls the same `setControl` method on the same `TalonFX` objects that it did before*โ€”the only difference is that it now goes through `DrivetrainIOReal` first. diff --git a/Examples/2.10_KitbotSim/.gitignore b/Examples/2.10_KitbotSim/.gitignore index 34cbaac..243c3dc 100644 --- a/Examples/2.10_KitbotSim/.gitignore +++ b/Examples/2.10_KitbotSim/.gitignore @@ -185,3 +185,5 @@ compile_commands.json # Eclipse generated file for annotation processors .factorypath + +simgui-ds.json diff --git a/Examples/2.10_KitbotSim/gradlew b/Examples/2.10_KitbotSim/gradlew old mode 100644 new mode 100755 diff --git a/Examples/2.10_KitbotSim/src/main/java/frc/robot/Robot.java b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Robot.java index cca3a87..b184aff 100644 --- a/Examples/2.10_KitbotSim/src/main/java/frc/robot/Robot.java +++ b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Robot.java @@ -9,17 +9,21 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import com.ctre.phoenix6.CANBus; + import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Subsystems.Drivetrain.DrivetrainSubsystem; +import frc.robot.subsystems.drivetrain.DrivetrainSubsystem; public class Robot extends LoggedRobot { CommandXboxController controller = new CommandXboxController(0); - DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); + CANBus canBus = new CANBus("*"); + + DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(canBus); public Robot() { diff --git a/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java index fdfb2a2..2652ca0 100644 --- a/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java +++ b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java @@ -2,11 +2,24 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Subsystems.Drivetrain; +package frc.robot.subsystems.drivetrain; import org.littletonrobotics.junction.AutoLog; -public interface DrivetrainIO { +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; + +/** Add your docs here. */ +public class DrivetrainIO { @AutoLog public static class DrivetrainIOInputs { public double leftOutputVolts = 0.0; @@ -24,7 +37,94 @@ public static class DrivetrainIOInputs { public double rightTempCelsius = 0.0; } - public void updateInputs(DrivetrainIOInputs inputs); + protected TalonFX leftTalon; + protected TalonFX rightTalon; + + VoltageOut leftVoltage = new VoltageOut(0); + VoltageOut rightVoltage = new VoltageOut(0); + + private final StatusSignal leftAppliedVoltage; + private final StatusSignal rightAppliedVoltage; + private final StatusSignal leftAngularVelocityRPS; + private final StatusSignal rightAngularVelocityRPS; + + // A little hacky - the units don't match, but that would typically be handled in the + // SensorToMechanismRatio config. For the purposes of this lesson, YOU DO NOT NEED TO + // WORRY ABOUT THIS, but ask a lead if you have questions! + private final StatusSignal leftPositionMeters; + private final StatusSignal rightPositionMeters; + + private final StatusSignal leftSupplyCurrent; + private final StatusSignal rightSupplyCurrent; + private final StatusSignal leftTempCelsius; + private final StatusSignal rightTempCelsius; + + public DrivetrainIO(CANBus canbus) { + leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID, canbus); + rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID, canbus); + + leftAppliedVoltage = leftTalon.getMotorVoltage(); + rightAppliedVoltage = rightTalon.getMotorVoltage(); + + leftAngularVelocityRPS = leftTalon.getVelocity(); + rightAngularVelocityRPS = rightTalon.getVelocity(); + + rightPositionMeters = rightTalon.getPosition(); + leftPositionMeters = leftTalon.getPosition(); - public void setVolts(double left, double right); + rightSupplyCurrent = rightTalon.getSupplyCurrent(); + leftSupplyCurrent = leftTalon.getSupplyCurrent(); + leftTempCelsius = leftTalon.getDeviceTemp(); + rightTempCelsius = rightTalon.getDeviceTemp(); + + // Sets the following status signals to be updated at a frequency of 50hz + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, // update every 20ms + leftAppliedVoltage, + rightAppliedVoltage, + leftAngularVelocityRPS, + rightAngularVelocityRPS, + leftPositionMeters, + rightPositionMeters, + leftSupplyCurrent, + rightSupplyCurrent, + leftTempCelsius, + rightTempCelsius); + leftTalon.optimizeBusUtilization(); + rightTalon.optimizeBusUtilization(); + } + + public void updateInputs(DrivetrainIOInputs inputs) { + + BaseStatusSignal.refreshAll(leftAppliedVoltage, + rightAppliedVoltage, + leftAngularVelocityRPS, + rightAngularVelocityRPS, + leftPositionMeters, + rightPositionMeters, + leftSupplyCurrent, + rightSupplyCurrent, + leftTempCelsius, + rightTempCelsius); + + inputs.leftOutputVolts = leftAppliedVoltage.getValueAsDouble(); + inputs.rightOutputVolts = rightAppliedVoltage.getValueAsDouble(); + + inputs.leftVelocityMetersPerSecond = leftAngularVelocityRPS.getValueAsDouble(); + inputs.rightVelocityMetersPerSecond = rightAngularVelocityRPS.getValueAsDouble(); + + inputs.leftPositionMeters = leftPositionMeters.getValueAsDouble(); + inputs.rightPositionMeters = rightPositionMeters.getValueAsDouble(); + + inputs.leftCurrentAmps = leftSupplyCurrent.getValueAsDouble(); + inputs.leftTempCelsius = rightSupplyCurrent.getValueAsDouble(); + inputs.rightCurrentAmps = rightSupplyCurrent.getValueAsDouble(); + inputs.rightTempCelsius = rightTempCelsius.getValueAsDouble(); + } + + public void setVolts(double left, double right) { + leftTalon.setControl(leftVoltage.withOutput(left)); + rightTalon.setControl(rightVoltage.withOutput(left)); + } + } diff --git a/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOReal.java b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOReal.java deleted file mode 100644 index 9f84186..0000000 --- a/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOReal.java +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.Subsystems.Drivetrain; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Temperature; -import edu.wpi.first.units.measure.Voltage; - -/** Add your docs here. */ -public class DrivetrainIOReal implements DrivetrainIO { - - TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); - TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); - - VoltageOut leftVoltage = new VoltageOut(0); - VoltageOut rightVoltage = new VoltageOut(0); - - private final StatusSignal leftAppliedVoltage = leftTalon.getMotorVoltage(); - private final StatusSignal rightAppliedVoltage = rightTalon.getMotorVoltage(); - private final StatusSignal leftAngularVelocityRPS = leftTalon.getVelocity(); - private final StatusSignal rightAngularVelocityRPS = rightTalon.getVelocity(); - - // A little hacky - the units don't match, but that would typically be handled in the - // SensorToMechanismRatio config. For the purposes of this lesson, YOU DO NOT NEED TO - // WORRY ABOUT THIS, but ask a lead if you have questions! - private final StatusSignal leftPositionMeters = leftTalon.getPosition(); - private final StatusSignal rightPositionMeters = rightTalon.getPosition(); - - private final StatusSignal leftSupplyCurrent = leftTalon.getSupplyCurrent(); - private final StatusSignal rightSupplyCurrent = rightTalon.getSupplyCurrent(); - private final StatusSignal leftTempCelsius = leftTalon.getDeviceTemp(); - private final StatusSignal rightTempCelsius = rightTalon.getDeviceTemp(); - - public DrivetrainIOReal() { - // Sets the following status signals to be updated at a frequency of 50hz - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, // update every 20ms - leftAppliedVoltage, - rightAppliedVoltage, - leftAngularVelocityRPS, - rightAngularVelocityRPS, - leftPositionMeters, - rightPositionMeters, - leftSupplyCurrent, - rightSupplyCurrent, - leftTempCelsius, - rightTempCelsius); - } - - @Override - public void updateInputs(DrivetrainIOInputs inputs) { - - BaseStatusSignal.refreshAll(leftAppliedVoltage, - rightAppliedVoltage, - leftAngularVelocityRPS, - rightAngularVelocityRPS, - leftPositionMeters, - rightPositionMeters, - leftSupplyCurrent, - rightSupplyCurrent, - leftTempCelsius, - rightTempCelsius); - - inputs.leftOutputVolts = leftAppliedVoltage.getValueAsDouble(); - inputs.rightOutputVolts = rightAppliedVoltage.getValueAsDouble(); - - inputs.leftVelocityMetersPerSecond = leftAngularVelocityRPS.getValueAsDouble(); - inputs.rightVelocityMetersPerSecond = rightAngularVelocityRPS.getValueAsDouble(); - - inputs.leftPositionMeters = leftPositionMeters.getValueAsDouble(); - inputs.rightPositionMeters = rightPositionMeters.getValueAsDouble(); - - inputs.leftCurrentAmps = leftSupplyCurrent.getValueAsDouble(); - inputs.leftTempCelsius = rightSupplyCurrent.getValueAsDouble(); - inputs.rightCurrentAmps = rightSupplyCurrent.getValueAsDouble(); - inputs.rightTempCelsius = rightTempCelsius.getValueAsDouble(); - } - - @Override - public void setVolts(double left, double right) { - leftTalon.setControl(leftVoltage.withOutput(left)); - rightTalon.setControl(rightVoltage.withOutput(left)); - } - -} diff --git a/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOSim.java b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOSim.java index ee4cb91..83dd30c 100644 --- a/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOSim.java +++ b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOSim.java @@ -2,79 +2,57 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Subsystems.Drivetrain; +package frc.robot.subsystems.drivetrain; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim.KitbotGearing; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim.KitbotMotor; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim.KitbotWheelSize; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -public class DrivetrainIOSim implements DrivetrainIO { - // Here we have 2 motors, one for each side - // Since we have 2 motors per side we would need to add "follower" motors - // Since this is a sim we can pretend those motors don't exist - TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); - TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); - - // ControlRequest objects represent what we want our motor to do - // There might be a cleaner way to have both voltage and velocity control than making two sets of objects - // This is part of the Phoenix v6/pro api, old code uses the v5 api which looks different - VoltageOut leftVoltage = new VoltageOut(0); - VoltageOut rightVoltage = new VoltageOut(0); - - // This is a physics sim object - // This will calculate the movement of the drivetrain based off of a mathmatical model - // To learn more about how these models work, look at the book linked in the state space course - DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( - KitbotMotor.kDoubleFalcon500PerSide, - // This is the default gearing for the kitbot - // If this was a real robot, we would check with mechanical for actual numbers - // Note that the default gearbox is not compatible with falcons so this configuration is not realistic - KitbotGearing.k8p45, - // Default wheels - KitbotWheelSize.kSixInch, - // This is a way for us to model noise from our measurements - // We can leave it null to pretend our simulation is perfect for this exercise - null); - - @Override - public void updateInputs(DrivetrainIOInputs inputs) { - // Start by updating our physics model with the default loop time of 20 ms - physicsSim.update(0.020); - - // Update the voltage available to each motor based off of a simulated robot battery - // This accounts for "voltage sag" when motors are running - var leftSimState = leftTalon.getSimState(); - leftSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); - - var rightSimState = rightTalon.getSimState(); - rightSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); - - // Use the motor output voltage to update the sim - physicsSim.setInputs(leftSimState.getMotorVoltage(), rightSimState.getMotorVoltage()); - - inputs.leftOutputVolts = leftSimState.getMotorVoltage(); - inputs.rightOutputVolts = rightSimState.getMotorVoltage(); - - inputs.leftVelocityMetersPerSecond = physicsSim.getLeftVelocityMetersPerSecond(); - inputs.rightVelocityMetersPerSecond = physicsSim.getRightVelocityMetersPerSecond(); - - inputs.leftPositionMeters = physicsSim.getLeftPositionMeters(); - inputs.rightPositionMeters = physicsSim.getRightPositionMeters(); - - inputs.leftCurrentAmps = leftSimState.getTorqueCurrent(); - inputs.leftTempCelsius = 0.0; - inputs.rightCurrentAmps = rightSimState.getTorqueCurrent(); - inputs.rightTempCelsius = 0.0; - } - - @Override - public void setVolts(double left, double right) { - leftTalon.setControl(leftVoltage.withOutput(left)); - rightTalon.setControl(rightVoltage.withOutput(right)); +public class DrivetrainIOSim extends DrivetrainIO { + + private DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( + KitbotMotor.kSingleFalcon500PerSide, + KitbotGearing.k8p45, + KitbotWheelSize.kSixInch, + null + ); + + private TalonFXSimState leftSimState; + private TalonFXSimState rightSimState; + + private double lastLoopTime = 0.0; + private Notifier notifier; + + public DrivetrainIOSim(CANBus canBus) { + super(canBus); + + leftSimState = leftTalon.getSimState(); + rightSimState = rightTalon.getSimState(); + + notifier = new Notifier(() -> { + double currentTime = Timer.getTimestamp(); + double deltaTime = currentTime - lastLoopTime; + lastLoopTime = currentTime; + + leftSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + rightSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + + physicsSim.setInputs(leftSimState.getMotorVoltage(), rightSimState.getMotorVoltage()); + physicsSim.update(deltaTime); + + leftSimState.setRawRotorPosition(physicsSim.getLeftPositionMeters()); + leftSimState.setRotorVelocity(physicsSim.getLeftVelocityMetersPerSecond()); + + rightSimState.setRawRotorPosition(physicsSim.getRightPositionMeters()); + rightSimState.setRotorVelocity(physicsSim.getRightVelocityMetersPerSecond()); + }); + notifier.startPeriodic(0.002); } } diff --git a/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java index e1214e0..6cda41d 100644 --- a/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java +++ b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java @@ -2,12 +2,14 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Subsystems.Drivetrain; +package frc.robot.subsystems.drivetrain; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.CANBus; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.util.Units; @@ -20,7 +22,7 @@ public class DrivetrainSubsystem extends SubsystemBase { public static final int LEFT_TALON_ID = 0; public static final int RIGHT_TALON_ID = 1; - DrivetrainIO io = Robot.isReal() ? new DrivetrainIOReal() : new DrivetrainIOSim(); + DrivetrainIO io; DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); // Odometry keeps track of our position on the field @@ -29,7 +31,12 @@ public class DrivetrainSubsystem extends SubsystemBase { DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0); /** Creates a new Drivetrain. */ - public DrivetrainSubsystem() { + public DrivetrainSubsystem(CANBus canBus) { + if (Robot.isReal()) { + io = new DrivetrainIO(canBus); + } else { + io = new DrivetrainIOSim(canBus); + } } private void setVoltages(double left, double right) { diff --git a/Examples/2.8_KitbotAKitRefactor/gradlew b/Examples/2.8_KitbotAKitRefactor/gradlew old mode 100644 new mode 100755 diff --git a/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Robot.java b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Robot.java index 8330d07..376a957 100644 --- a/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Robot.java +++ b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Robot.java @@ -9,17 +9,21 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import com.ctre.phoenix6.CANBus; + import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Subsystems.Drivetrain.DrivetrainSubsystem; +import frc.robot.subsystems.drivetrain.DrivetrainSubsystem; public class Robot extends LoggedRobot { CommandXboxController controller = new CommandXboxController(0); - DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); + CANBus canBus = new CANBus("*"); + + DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(canBus); public Robot() { diff --git a/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java index fdfb2a2..b9ad23a 100644 --- a/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java +++ b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java @@ -2,11 +2,24 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Subsystems.Drivetrain; +package frc.robot.subsystems.drivetrain; import org.littletonrobotics.junction.AutoLog; -public interface DrivetrainIO { +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; + +/** Add your docs here. */ +public class DrivetrainIO { @AutoLog public static class DrivetrainIOInputs { public double leftOutputVolts = 0.0; @@ -24,7 +37,94 @@ public static class DrivetrainIOInputs { public double rightTempCelsius = 0.0; } - public void updateInputs(DrivetrainIOInputs inputs); + private TalonFX leftTalon; + private TalonFX rightTalon; + + VoltageOut leftVoltage = new VoltageOut(0); + VoltageOut rightVoltage = new VoltageOut(0); + + private final StatusSignal leftAppliedVoltage; + private final StatusSignal rightAppliedVoltage; + private final StatusSignal leftAngularVelocityRPS; + private final StatusSignal rightAngularVelocityRPS; + + // A little hacky - the units don't match, but that would typically be handled in the + // SensorToMechanismRatio config. For the purposes of this lesson, YOU DO NOT NEED TO + // WORRY ABOUT THIS, but ask a lead if you have questions! + private final StatusSignal leftPositionMeters; + private final StatusSignal rightPositionMeters; + + private final StatusSignal leftSupplyCurrent; + private final StatusSignal rightSupplyCurrent; + private final StatusSignal leftTempCelsius; + private final StatusSignal rightTempCelsius; + + public DrivetrainIO(CANBus canbus) { + leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID, canbus); + rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID, canbus); + + leftAppliedVoltage = leftTalon.getMotorVoltage(); + rightAppliedVoltage = rightTalon.getMotorVoltage(); + + leftAngularVelocityRPS = leftTalon.getVelocity(); + rightAngularVelocityRPS = rightTalon.getVelocity(); + + rightPositionMeters = rightTalon.getPosition(); + leftPositionMeters = leftTalon.getPosition(); - public void setVolts(double left, double right); + rightSupplyCurrent = rightTalon.getSupplyCurrent(); + leftSupplyCurrent = leftTalon.getSupplyCurrent(); + leftTempCelsius = leftTalon.getDeviceTemp(); + rightTempCelsius = rightTalon.getDeviceTemp(); + + // Sets the following status signals to be updated at a frequency of 50hz + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, // update every 20ms + leftAppliedVoltage, + rightAppliedVoltage, + leftAngularVelocityRPS, + rightAngularVelocityRPS, + leftPositionMeters, + rightPositionMeters, + leftSupplyCurrent, + rightSupplyCurrent, + leftTempCelsius, + rightTempCelsius); + leftTalon.optimizeBusUtilization(); + rightTalon.optimizeBusUtilization(); + } + + public void updateInputs(DrivetrainIOInputs inputs) { + + BaseStatusSignal.refreshAll(leftAppliedVoltage, + rightAppliedVoltage, + leftAngularVelocityRPS, + rightAngularVelocityRPS, + leftPositionMeters, + rightPositionMeters, + leftSupplyCurrent, + rightSupplyCurrent, + leftTempCelsius, + rightTempCelsius); + + inputs.leftOutputVolts = leftAppliedVoltage.getValueAsDouble(); + inputs.rightOutputVolts = rightAppliedVoltage.getValueAsDouble(); + + inputs.leftVelocityMetersPerSecond = leftAngularVelocityRPS.getValueAsDouble(); + inputs.rightVelocityMetersPerSecond = rightAngularVelocityRPS.getValueAsDouble(); + + inputs.leftPositionMeters = leftPositionMeters.getValueAsDouble(); + inputs.rightPositionMeters = rightPositionMeters.getValueAsDouble(); + + inputs.leftCurrentAmps = leftSupplyCurrent.getValueAsDouble(); + inputs.leftTempCelsius = rightSupplyCurrent.getValueAsDouble(); + inputs.rightCurrentAmps = rightSupplyCurrent.getValueAsDouble(); + inputs.rightTempCelsius = rightTempCelsius.getValueAsDouble(); + } + + public void setVolts(double left, double right) { + leftTalon.setControl(leftVoltage.withOutput(left)); + rightTalon.setControl(rightVoltage.withOutput(left)); + } + } diff --git a/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOReal.java b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOReal.java deleted file mode 100644 index 9f84186..0000000 --- a/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOReal.java +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.Subsystems.Drivetrain; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Temperature; -import edu.wpi.first.units.measure.Voltage; - -/** Add your docs here. */ -public class DrivetrainIOReal implements DrivetrainIO { - - TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); - TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); - - VoltageOut leftVoltage = new VoltageOut(0); - VoltageOut rightVoltage = new VoltageOut(0); - - private final StatusSignal leftAppliedVoltage = leftTalon.getMotorVoltage(); - private final StatusSignal rightAppliedVoltage = rightTalon.getMotorVoltage(); - private final StatusSignal leftAngularVelocityRPS = leftTalon.getVelocity(); - private final StatusSignal rightAngularVelocityRPS = rightTalon.getVelocity(); - - // A little hacky - the units don't match, but that would typically be handled in the - // SensorToMechanismRatio config. For the purposes of this lesson, YOU DO NOT NEED TO - // WORRY ABOUT THIS, but ask a lead if you have questions! - private final StatusSignal leftPositionMeters = leftTalon.getPosition(); - private final StatusSignal rightPositionMeters = rightTalon.getPosition(); - - private final StatusSignal leftSupplyCurrent = leftTalon.getSupplyCurrent(); - private final StatusSignal rightSupplyCurrent = rightTalon.getSupplyCurrent(); - private final StatusSignal leftTempCelsius = leftTalon.getDeviceTemp(); - private final StatusSignal rightTempCelsius = rightTalon.getDeviceTemp(); - - public DrivetrainIOReal() { - // Sets the following status signals to be updated at a frequency of 50hz - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, // update every 20ms - leftAppliedVoltage, - rightAppliedVoltage, - leftAngularVelocityRPS, - rightAngularVelocityRPS, - leftPositionMeters, - rightPositionMeters, - leftSupplyCurrent, - rightSupplyCurrent, - leftTempCelsius, - rightTempCelsius); - } - - @Override - public void updateInputs(DrivetrainIOInputs inputs) { - - BaseStatusSignal.refreshAll(leftAppliedVoltage, - rightAppliedVoltage, - leftAngularVelocityRPS, - rightAngularVelocityRPS, - leftPositionMeters, - rightPositionMeters, - leftSupplyCurrent, - rightSupplyCurrent, - leftTempCelsius, - rightTempCelsius); - - inputs.leftOutputVolts = leftAppliedVoltage.getValueAsDouble(); - inputs.rightOutputVolts = rightAppliedVoltage.getValueAsDouble(); - - inputs.leftVelocityMetersPerSecond = leftAngularVelocityRPS.getValueAsDouble(); - inputs.rightVelocityMetersPerSecond = rightAngularVelocityRPS.getValueAsDouble(); - - inputs.leftPositionMeters = leftPositionMeters.getValueAsDouble(); - inputs.rightPositionMeters = rightPositionMeters.getValueAsDouble(); - - inputs.leftCurrentAmps = leftSupplyCurrent.getValueAsDouble(); - inputs.leftTempCelsius = rightSupplyCurrent.getValueAsDouble(); - inputs.rightCurrentAmps = rightSupplyCurrent.getValueAsDouble(); - inputs.rightTempCelsius = rightTempCelsius.getValueAsDouble(); - } - - @Override - public void setVolts(double left, double right) { - leftTalon.setControl(leftVoltage.withOutput(left)); - rightTalon.setControl(rightVoltage.withOutput(left)); - } - -} diff --git a/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java index 269ebd9..1061a7d 100644 --- a/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java +++ b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java @@ -2,12 +2,14 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Subsystems.Drivetrain; +package frc.robot.subsystems.drivetrain; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.CANBus; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.util.Units; @@ -19,13 +21,14 @@ public class DrivetrainSubsystem extends SubsystemBase { public static final int LEFT_TALON_ID = 0; public static final int RIGHT_TALON_ID = 1; - DrivetrainIO io = new DrivetrainIOReal(); + DrivetrainIO io; DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0); /** Creates a new Drivetrain. */ - public DrivetrainSubsystem() { + public DrivetrainSubsystem(CANBus canBus) { + io = new DrivetrainIO(canBus); } private void setVoltages(double left, double right) {