Mech code training

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.

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).