diff --git a/Docs/2_Architecture/2.10_KitbotSim.md b/Docs/2_Architecture/2.10_KitbotSim.md new file mode 100644 index 0000000..3648892 --- /dev/null +++ b/Docs/2_Architecture/2.10_KitbotSim.md @@ -0,0 +1,340 @@ +# Kitbot Sim Tutorial + +## Intro + +Writing code is plenty of fun on its own, but its rough having to wait for mechanical to be done to test it. +When we only have 2-3 weeks of time (which we have to share with drive practice!) after the robot is built before our first competition, we need to be able to test before the robot is ready. +Luckily, we have a handy piece of code that to refactor to be simulateable! +In this tutorial we are going to rework your kitbot code to be easy to simulate and expand on. +In the end your code should be able to do this: + +[Video of robot in sim](https://drive.google.com/file/d/1AD0OLK8Lt1dHbn5YYZ8rYc_6_0wbLPJf/view?usp=sharing) + +## 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! +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`. + +Make a new `DrivetrainIOSim` class that implements `DrivetrainIO` the [same way](2.8_KitbotAKitRefactor.md#drivetrainioreal) you did for `DrivetrainIOReal`. + +```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 + + } +} +``` + +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`. + +```Java +TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); +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`. + +```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 +} +``` + +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. + +```Java +@Override +public void updateInputs(DrivetrainIOInputs inputs) { + // 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)); +} +``` + +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. + +```Java +DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( + null, + null, + null, + null); +``` + +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. +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, + null, + null, + null); +``` + +The second parameter is the gearing of the drivetrain. +This is something we normally would ask mechanical for. +In this case, since the drivetrain is simulated, we can use the standard 8p45 gearbox. + +```Java +DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( + KitbotMotor.kDoubleFalcon500PerSide, + KitbotGearing.k8p45, + null, + null); +``` + +The third parameter is the size of the wheels. +By default the kitbot comes with 6 inch wheels. + +```Java +DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( + KitbotMotor.kDoubleFalcon500PerSide, + KitbotGearing.k8p45, + KitbotWheelSize.kSixInch, + null); +``` + +The final parameter is the "measurement standard deviations". +This is a way for us to add uncertainty to our simulated model in a way that matches real sensor noise. +For this exercise we can ignore it and leave it as null. + +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()`. + +```Java +@Override + public void updateInputs(DrivetrainIOInputs inputs) { + physicsSim.update(0.020); + + var leftSimState = leftTalon.getSimState(); + var rightSimState = rightTalon.getSimState(); + // Snip +} +``` + +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. + +```Java +@Override + public void updateInputs(DrivetrainIOInputs inputs) { + physicsSim.update(0.020); + + var leftSimState = leftTalon.getSimState(); + var rightSimState = rightTalon.getSimState(); + + physicsSim.setInputs(leftSimState.getMotorVoltage(), rightSimState.getMotorVoltage()); + // Snip +} +``` + +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`. + +```Java +@Override +public void updateInputs(DrivetrainIOInputs inputs) { + // Snip + inputs.leftVelocityMetersPerSecond = physicsSim.getLeftVelocityMetersPerSecond(); + inputs.rightVelocityMetersPerSecond = physicsSim.getRightVelocityMetersPerSecond(); + + inputs.leftPositionMeters = physicsSim.getLeftPositionMeters(); + inputs.rightPositionMeters = physicsSim.getRightPositionMeters(); + // Snip +} +``` + +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. + +```Java +@Override +public void updateInputs(DrivetrainIOInputs inputs) { + physicsSim.update(0.020); + + var leftSimState = leftTalon.getSimState(); + leftSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); + + var rightSimState = rightTalon.getSimState(); + rightSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); + // Snip +} +``` + +Now we're done with the `DrivetrainIOSim` class! πŸŽ‰ + +Let's add a way to switch between using a `DrivetrainIOReal` or a `DrivetrainIOSim` depending on if the robot is real or simulated. +The `Robot.isReal()` method will return whether or not the robot is currently running in real life or not so we can use that to select which object to use. +Replace the `io` assignment with the following line: + +```Java +public class DrivetrainSubsystem extends SubsystemBase { + DrivetrainIO io = Robot.isReal() ? new DrivetrainIOReal() : new DrivetrainIOSim(); +} +``` +This will now select the correct io object to create, and will now work in sim. + +But it won't have any way to track the position of the drivebase. +Remember that we can use odometry to track the position of a drivebase. +We can create a new `DifferentialDriveOdometry` object in `DrivetrainSubsystem` to handle this. + +```Java +public class DrivetrainSubsystem extends SubsystemBase { + DrivetrainIO io = Robot.isReal() ? new DrivetrainIOReal() : new DrivetrainIOSim(); + DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); + + DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0); + // Snip +} +``` + +We will need to update this every processor loop. +Normally we would have a gyro to track the heading (rotation) of the drive base, and we'd update the odometry object with its values instead. +However, in simulation we don't have an easy way to make a gyro that behaves how we would expect. +Instead, we can use the following line to fudge it: + +```Java +odometry.getPoseMeters().getRotation() + .plus( + Rotation2d.fromRadians( + (inputs.leftSpeedMetersPerSecond - inputs.rightSpeedMetersPerSecond) + * 0.020 / Units.inchesToMeters(26))) +``` + +This line is using a small piece of math to calculate what the new rotation should be based off of our wheel speeds. +You don't need to know how this works in particular, just know it's a bit of a hack. + +We are going to wrap this in an `odometry.update()` call. +We only want this to run in simulation, since if it's in real life we'll just use the gyro data, so we'll also wrap this in a check for if the robot is in simulation. + +```Java +@Override +public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drivetrain", inputs); + + + if (Robot.isSimulation) { + odometry.update( + odometry.getPoseMeters().getRotation() + // Use differential drive kinematics to find the rotation rate based on the wheel speeds and distance between wheels + .plus(Rotation2d.fromRadians((inputs.leftVelocityMetersPerSecond - inputs.rightVelocityMetersPerSecond) + * 0.020 / Units.inchesToMeters(26))), + inputs.leftPositionMeters, inputs.rightPositionMeters); + } +} +``` + +Finally, we need to send the odometry pose to the logger so we can visualize it. +We do this using `Logger.recordOutput()` and `odometry.getPoseMeters()`. + +```Java +@Override +public void periodic() { + // Snip + Logger.recordOutput("Drivetrain Pose", odometry.getPoseMeters()); +} +``` + +This should be everything you need to have a full drivebase simulation! + +### Running the Simulation + +To run a robot code simulation, click on the WPILib icon in the top right corner. +Then find or search for "Simulate Robot Code" and press it. +Once the code builds, it will prompt you to choose between using the driver station and sim gui. +Choose the sim gui. + +The sim gui is perfectly serviceable for testing, but we can do better. +AdvantageScope supports connecting to a simulation to visualize IO. +When you open AdvantageScope, hit "file", then "Connect to Simulator". +This lets you visualize data from the simulator in the same way as you would with a real robot. +You can use the 3d field visualization to have a fun way to show the robot on the field. + +To drive the robot, add "keyboard 0" to "joystick 0" in the sim gui. +You might have to change your keyboard settings in the sim gui to make it properly emulate a joystick. +Then make sure to set the mode to teleop. +You can also check your setup against a teammate or lead's. \ No newline at end of file diff --git a/Docs/2_Architecture/2.10_Superstructure.md b/Docs/2_Architecture/2.11_Superstructure.md similarity index 100% rename from Docs/2_Architecture/2.10_Superstructure.md rename to Docs/2_Architecture/2.11_Superstructure.md diff --git a/Docs/2_Architecture/2.1_WPILib.md b/Docs/2_Architecture/2.1_WPILib.md index 22def58..8f9f22a 100644 --- a/Docs/2_Architecture/2.1_WPILib.md +++ b/Docs/2_Architecture/2.1_WPILib.md @@ -21,7 +21,7 @@ Many of these libraries will have their own articles here as well as their own d ### Must read/watch -- Read through our [electrical reference sheet](../General/ElectronicsCrashCourse.md) +- Read through our [electrical reference sheet](2.2_ElectronicsCrashCourse.md) - In an ideal world, we never have to touch the electrical system. In reality, often we end up finding problems with electrical early in the season. Therefore it is important to know what we are working with to debug and fix the robot. diff --git a/Docs/2_Architecture/2.2_ElectronicsCrashCourse.md b/Docs/2_Architecture/2.2_ElectronicsCrashCourse.md index f8bd8af..d35739d 100644 --- a/Docs/2_Architecture/2.2_ElectronicsCrashCourse.md +++ b/Docs/2_Architecture/2.2_ElectronicsCrashCourse.md @@ -280,6 +280,7 @@ Instead, we use "current zeroing". This involves slowly powering a mechanism into a hardstop with a known position (like an elevator being all the way down). When the current draw of the motor spikes, we know that the motor is "stalling", or using power but not moving. This tells us that we are at the hardstop in the same way that a limit switch would. +Think of it as walking towards a wall blindfolded with your hands outstretched- when you get to a point where you're not getting anywhere and are exerting a lot of force in front of you , you'll know you're at the wall. ### IMU diff --git a/Docs/2_Architecture/2.3_CommandBased.md b/Docs/2_Architecture/2.3_CommandBased.md index 93779f7..d964b8d 100644 --- a/Docs/2_Architecture/2.3_CommandBased.md +++ b/Docs/2_Architecture/2.3_CommandBased.md @@ -34,7 +34,7 @@ For example, we might want something to happen while a condition is true (`while Some large Commands are better represented by several Commands and some Triggers! Subsystems, commands, and triggers are the building blocks of the robot's overall "superstructure". -This will be covered in more detail in the [Superstructure article.](2.10_Superstructure.md) +This will be covered in more detail in the [Superstructure article.](2.11_Superstructure.md) ### Resources @@ -52,7 +52,7 @@ This will be covered in more detail in the [Superstructure article.](2.10_Supers ### Exercises -- Make basic KitBot code using the Command-Based skeleton. You can follow [this](2.5_KitbotExampleWalkthrough.md) tutorial. +- Make basic KitBot code using the Command-Based skeleton. You can follow [this](2.5_KitbotIntro.md) tutorial. ### Notes diff --git a/Docs/2_Architecture/2.5_KitbotExampleWalkthrough.md b/Docs/2_Architecture/2.5_KitbotIntro.md similarity index 89% rename from Docs/2_Architecture/2.5_KitbotExampleWalkthrough.md rename to Docs/2_Architecture/2.5_KitbotIntro.md index 1cc87e7..56c01b6 100644 --- a/Docs/2_Architecture/2.5_KitbotExampleWalkthrough.md +++ b/Docs/2_Architecture/2.5_KitbotIntro.md @@ -81,12 +81,12 @@ Make sure to get the V6 API. For this project you may use the online installer, but if you want to use your computer to run and setup the robot it is useful to have Tuner X installed. The [api docs](https://pro.docs.ctr-electronics.com/en/stable/docs/api-reference/index.html) for CTRE are also on the same website. -Once you have installed the vendor library, create two TalonFX objects in your subsystem called `leftFalcon` and `rightFalcon`. +Once you have installed the vendor library, create two TalonFX objects in your subsystem called `leftTalon` and `rightTalon`. By convention, hardware should have a succinct, descriptive name followed by the type of hardware it is. ```Java -TalonFX leftFalcon = new TalonFX(0); -TalonFX rightFalcon = new TalonFX(0); +TalonFX leftTalon = new TalonFX(0); +TalonFX rightTalon = new TalonFX(0); /** Creates a new Drivetrain. */ public DrivetrainSubsystem() {} @@ -95,20 +95,18 @@ public DrivetrainSubsystem() {} The number being passed into the constructor for the TalonFXs is the ID number of the motor, which we set using Tuner. 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. -In Constants.java add two `public static final int`s, one for the left motor's ID and one for the right motor. -By putting these configuration values all in one place we can easily change them if hardware changes. +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. ```Java public class DrivetrainSubsystem extends SubsystemBase { - public static final int drivetrainLeftFalconID = 0; - public static final int drivetrainRightFalconID = 1; + public static final int LEFT_TALON_ID = 0; + public static final int RIGHT_TALON_ID = 1; - TalonFX leftFalcon = new TalonFX(drivetrainLeftFalconID); - TalonFX rightFalcon = new TalonFX(drivetrainRightFalconID); + TalonFX leftTalon = new TalonFX(LEFT_TALON_ID); + TalonFX rightTalon = new TalonFX(RIGHT_TALON_ID); //... } ``` -Then change DrivetrainSubsystem to use the new constants. Next, let's add the `ControlRequest` objects. These represent the output of a device, such as a desired voltage, velocity, position, etc. @@ -119,8 +117,8 @@ 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 leftFalcon = new TalonFX(Constants.drivetrainLeftFalconID); -TalonFX rightFalcon = new TalonFX(Constants.drivetrainRightFalconID); +TalonFX leftTalon = new TalonFX(LEFT_TALON_ID); +TalonFX rightTalon = new TalonFX(RIGHT_TALON_ID); VoltageOut leftVoltage = new VoltageOut(0); VoltageOut rightVoltage = new VoltageOut(0); @@ -130,18 +128,19 @@ public DrivetrainSubsystem() {} ``` We have the control requests now, but we need a way to set them. -Lets add a method for this. +Let's add a method for this. ```Java private void setVoltages(double left, double right) { - leftFalcon.setControl(leftVoltage.withOutput(left)); - rightFalcon.setControl(rightVoltage.withOutput(left)); + leftTalon.setControl(leftVoltage.withOutput(left)); + rightTalon.setControl(rightVoltage.withOutput(left)); } ``` 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. -To do this we need to make a Command factory method, or a method that returns a `Command`. +(If you're feeling unsure about Commands, review the Command Based [article](2.3_CommandBased.md) and/or [presentation](2.4_CommandBasedPresentationNotes.md).) +To do this, we need to make a Command factory method, or a method that returns a `Command`. ```Java public Command setVoltagesCommand(DoubleSupplier left, DoubleSupplier right) { @@ -191,7 +190,9 @@ We can convert to voltage by multiplying by 12, which our robot should nominally These methods are all well and good on their own, but it would be nice if we had a way to actually use them. Let's bind them to a joystick. -Go to `RobotContainer.java` and add a `CommandXboxController` object. +Go to `Robot.java`. +Firstly, you can delete `RobotContainer.java` - we'll be handling initialization stuff in `Robot.java` instead. +Add a `CommandXboxController` object. ```Java // Create a new Xbox controller on port 0 @@ -199,7 +200,7 @@ 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` to RobotContainer so that we can control it. +We also need to add an instance of the `DrivetrainSubsystem` so that we can control it. ```Java CommandXboxController controller = new CommandXboxController(0); @@ -208,7 +209,8 @@ DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); ``` Finally, we can bind the arcade drive command to the joysticks. -To do this, call `setDefaultCommand()` on `drivetrainSubsystem` in RobotContainer's `configureBindings()` method. +To do this, first define the `Robot()` constructor in `Robot.java`. +Then, call `setDefaultCommand()` on `drivetrainSubsystem` in the constructor. This sets a Command that will be run when no other command is using the drivetrain. Pass in `drivetrainSubsystem.setVoltagesArcadeCommand()` as the default Command. @@ -241,7 +243,7 @@ Notice how the graphs overlap outside of the deadband. Also notice the jump in inputs when the deadband stops applying. For an exaggerated one like this it would be an issue, but for a real input the deadband is usually small enough that this jump is not significant. -Let's add a function in RobotContainer to modify the joystick inputs. +Let's add a function to modify the joystick inputs. Create a function that takes in a double and returns a double called `modifyJoystick`. Let's start by adding the squaring of the input. @@ -254,7 +256,7 @@ private double modifyJoystick(double in) { } ``` -Then lets add the deadband. +Then let's add the deadband. If the absolute value of `in` is less than 0.05 (as an example) then return 0. ```Java diff --git a/Docs/2_Architecture/2.6_AdvantageKit.md b/Docs/2_Architecture/2.6_AdvantageKit.md index 34e8803..137482f 100644 --- a/Docs/2_Architecture/2.6_AdvantageKit.md +++ b/Docs/2_Architecture/2.6_AdvantageKit.md @@ -51,9 +51,7 @@ While this does require some additional effort to write subsystems, it also make ### Exercises -- Install AdvantageKit into your kitbot project following [this tutorial](https://docs.advantagekit.org/getting-started/installation/existing-projects). - You do not need to add the suggested block in the `Robot()` constructor. - We will do that as part of the simulation tutorial. +- Follow this [tutorial](2.8_KitbotAKitRefactor.md) to add AdvantageKit to your kitbot code. ### Notes diff --git a/Docs/2_Architecture/2.8_KitbotAKitRefactor.md b/Docs/2_Architecture/2.8_KitbotAKitRefactor.md new file mode 100644 index 0000000..8938c1b --- /dev/null +++ b/Docs/2_Architecture/2.8_KitbotAKitRefactor.md @@ -0,0 +1,480 @@ +# Kitbot, now with AdvantageKit + +## Intro +We'll be refactoring our kitbot code to apply what we just learned about AdvantageKit and how we structure our code with it. +"Refactoring" means rewriting code to improve its structure without changing its functionality. +We'll also learn how to use `StatusSignal`s to get loggable data from CTRE devices. + + +## Code Walkthrough + +### Moving to AdvantageKit + +The first step of moving our standard Command-based code to a loggable, simulateable structure is to install AdvantageKit. +Luckily, AdvantageKit has a handy guide on how to install it on an existing code base. +Follow [this walkthrough](https://docs.advantagekit.org/getting-started/installation/existing-projects). +Follow all the steps in the doc through changing it to `LoggedRobot`. + +You do not need to add the suggested block in `Robot()`. +Instead, use the one below. +The suggested logging configuration is built around replaying logs, while we just want to simulate code. + +```Java +Logger.recordMetadata("ProjectName", "KitbotExample"); // Set a metadata value + + if (isReal()) { + Logger.addDataReceiver(new WPILOGWriter("/media/sda1/")); // Log to a USB stick + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging + } else { + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + } + + Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added. +``` + +Once you've completed this, it's time to break `DrivetrainSubsystem` apart using the IO layer structure. + +### Creating our IOInputs class + +Recall the [IO layer structure](https://docs.advantagekit.org/getting-started/what-is-advantagekit/) for robot code. +Right now, we have one file for `DrivetrainSubsystem`. +Let's put it in a `Drivetrain` folder under `Subsystems` so that everything stays organized as we add more files. + +![A screenshot showing the new folder](../../Assets/KitbotExampleSimScreenshot0.png) + +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.) + +```Java +package frc.robot.Subsystems.Drivetrain; + +public interface 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. + +```Java +public static class DrivetrainIOInputs {} +``` + +#### Voltage + velocity +Let's start by logging the output voltage of each side of the drivetrain. + +```Java +public static class DrivetrainIOInputs { + public double leftOutputVolts = 0.0; + public double rightOutputVolts = 0.0; +} +``` + +Notice how we name our fields with units. +**It is very important to include units in all IOInputs variable names**. +It is very easy to assume different units in different places, which can cause frustrating and hard to find bugs. + +Next, let's log the current velocity of each side of the drivetrain. + +```Java +public static class DrivetrainIOInputs { + public double leftOutputVolts = 0.0; + public double rightOutputVolts = 0.0; + + public double leftVelocityMetersPerSecond = 0.0; + public double rightVelocityMetersPerSecond = 0.0; +} +``` + +Notice the units. +This field is useful both for looking at drivetrain performance and also for tracking our position over time. +In fact, let's add a field for the current position of each side. + +```Java +public static class DrivetrainIOInputs { + // Snip + public double leftPositionMeters = 0.0; + public double rightPositionMeters = 0.0; +} +``` + +These values are useful for odometry. +Odometry is a way to track the robots position on the field by measuring how much each wheel is turning. +On a real robot we use it for auto so that we can draw paths on a map of the field instead of guessing where the robot should go. +For this sim we will use odometry to give us a position that we can use to visualize the robot on a fake field. + +#### Current + temperature +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. + +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. +Logging the temperature of our motors can help us debug problems near the end of a match. +Normally, this issue is not too pronounced over the course of a single match. +However, in elims where we have a number of intense matches back to back or during drive practice where we run continuously for hours this can become an issue. +This simulation will not include temperature simulation, but it's good to get into the habit of including it in your logged values. + +```Java +public static class DrivetrainIOInputs { + // Snip + public double leftCurrentAmps = 0.0; + public double leftTempCelsius = 0.0; + public double rightCurrentAmps = 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. +This needs to be called periodically to get the most up to date data. + +```Java +public void updateInputs(DrivetrainIOInputs inputs); +``` + +#### Adding methods +Finally, let's add the methods that we use to interact with the hardware. +For this case it will just be a method `setVolts` to set the left and right output volts, like our `setVoltages` method that we have right now. + +Now your `DrivetrainIO` file should look like this: + +```Java +package frc.robot.Subsystems.Drivetrain; + +import org.littletonrobotics.junction.AutoLog; + +public interface DrivetrainIO { + @AutoLog + public static class DrivetrainIOInputs { + public double leftOutputVolts = 0.0; + public double rightOutputVolts = 0.0; + + public double leftVelocityMetersPerSecond = 0.0; + public double rightVelocityMetersPerSecond = 0.0; + + public double leftPositionMeters = 0.0; + public double rightPositionMeters = 0.0; + + public double leftCurrentAmps = 0.0; + public double leftTempCelsius = 0.0; + public double rightCurrentAmps = 0.0; + 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 + + } +} +``` + +#### 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); +``` + +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; + + public double leftVelocityMetersPerSecond = 0.0; + public double rightVelocityMetersPerSecond = 0.0; + + public double leftPositionMeters = 0.0; + public double rightPositionMeters = 0.0; + + public double leftCurrentAmps = 0.0; + public double leftTempCelsius = 0.0; + public double rightCurrentAmps = 0.0; + public double rightTempCelsius = 0.0; +} +``` + +Then replace the `public` and types with `inputs.` + +```Java +@Override +public void updateInputs(DrivetrainIOInputs inputs) { + inputs.leftOutputVolts = 0.0; + inputs.rightOutputVolts = 0.0; + + inputs.leftVelocityMetersPerSecond = 0.0; + inputs.rightVelocityMetersPerSecond = 0.0; + + inputs.leftPositionMeters = 0.0; + inputs.rightPositionMeters = 0.0; + + inputs.leftCurrentAmps = 0.0; + inputs.leftTempCelsius = 0.0; + inputs.rightCurrentAmps = 0.0; + inputs.rightTempCelsius = 0.0; +} +``` + +#### 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. +Here, we'll be looking at the values we get from a TalonFX, but you can also get useful `StatusSignal`s from Pigeons, CANcoders, etc. +We'll update these every loop to get the latest data from each device. + +Which `StatusSignal` we use depends on what data we want to collect. +Since we want to know the voltage draw of the left and right motors, we will need 2 `StatusSignal`s. +We'll define these member variables at the top of `DrivetrainIOReal`. + +```Java +public class DrivetrainIOReal implements DrivetrainIO { + TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); + TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); + + private final StatusSignal leftAppliedVoltage; + private final StatusSignal rightAppliedVoltage; +} +``` + +Then, we'll instantiate these signals and match them to the correct motors + signal type. +Call `.getMotorVoltage();` on both of these motors, which returns a `StatusSignal,` and assign that to their respective signals. + +```Java +public class DrivetrainIOReal implements DrivetrainIO { + TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); + TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); + + private final StatusSignal leftAppliedVoltage = leftTalon.getMotorVoltage(); + private final StatusSignal rightAppliedVoltage = rightTalon.getMotorVoltage(); +} +``` + +Go ahead and fill out the rest of the status signals in the same way. + +```Java +public class DrivetrainIOReal implements DrivetrainIO { + TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID); + TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID); + + 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(); +} +``` +The values of these signals will need to be updated every loop (every 20 milliseconds, which equals 50 hertz). +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() { + // 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); +} +``` + +At the top of `updateInputs()` (which gets called periodically), we're going to refresh all these signals with the `BaseStatusSignal.refreshAll()` method. +We'll pass in all our status signals as parameters. + +```Java +@Override +public void updateInputs(DrivetrainIOInputs inputs) { + BaseStatusSignal.refreshAll(leftAppliedVoltage, + rightAppliedVoltage, + leftAngularVelocityRPS, + rightAngularVelocityRPS, + leftPositionMeters, + rightPositionMeters, + leftSupplyCurrent, + rightSupplyCurrent, + leftTempCelsius, + rightTempCelsius); +} +``` + +#### Getting the `StatusSignal` values +Now that we have the latest data for all these signals, we can log them. +Let's get those values and set the corresponding fields in `inputs` equal to them. +You'll need to call some sort of `.getValueAsDouble()` on these signals, since we can't use the `StatusSignal` directly. +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, + 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(); + +} +``` +Your `DrivetrainIOReal` class is now (mostly) complete! + +### Refactoring `DrivetrainSubsystem` +Let's refactor our `DrivetrainSubsystem` to finish off this rewrite. + +First, we need to replace all the functionality that moved to `DrivetrainIOReal` with an instance of `DrivetrainIOReal`. +Get rid of the `TalonFX` objects and `VoltageOut` requests, since those have all been moved to `DrivetrainIOReal.` + +```Java +public class DrivetrainSubsystem extends SubsystemBase { + DrivetrainIO io = new DrivetrainIOReal(); + // Snip +} +``` + +Then we should add an instance of the `DrivetrainIOInputs` class we just made to track all the inputs for the drivetrain. +Since we used the `@AutoLog` annotation, the class we actually want to use is `DrivetrainIOInputsAutoLogged`, which was automatically generated for us. + +```Java +public class DrivetrainSubsystem extends SubsystemBase { + DrivetrainIO io = new DrivetrainIOReal(); + DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); +} +``` + +Add a call to `io.updateInputs(inputs)` in the `periodic()` method of `DrivetrainSubsystem`. + +```Java +@Override +public void periodic() { + io.updateInputs(inputs); +} +``` + +Then we need to push those inputs to the log, and also give them a name so we know that these inputs are coming from the drivetrain. +When you look at a log or sim in AdvantageScope, this is what organizes stuff into a folder in the sidebar. +We do this by calling `Logger.processInputs("Drivetrain", inputs)`. + +```Java +@Override +public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drivetrain", inputs); +} +``` + +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 { + + 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); + + //snip + + @Override + public void setVolts(double left, double right) { + leftTalon.setControl(leftVoltage.withOutput(left)); + rightTalon.setControl(rightVoltage.withOutput(left)); + } +} +``` + +Then in `DrivetrainSubsystem`, replace `setVoltages` with the following: +```Java +private void setVoltages(double left, double right) { + io.setVolts(left, right); +} +``` +*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. + +And that's basically it! +In a future lesson, you'll learn how to simulate all of this as well. + +This general structure of motors/other devices -> `ControlRequest`s and `StatusSignal`s -> various methods + `updateInputs()` is important and used across mechanisms, so use this to guide you as you write more mechanisms in the future. \ No newline at end of file diff --git a/Docs/2_Architecture/2.9_KitbotExampleWalkthroughSim.md b/Docs/2_Architecture/2.9_KitbotExampleWalkthroughSim.md deleted file mode 100644 index a6667e4..0000000 --- a/Docs/2_Architecture/2.9_KitbotExampleWalkthroughSim.md +++ /dev/null @@ -1,591 +0,0 @@ -# Kitbot Sim Tutorial - -## Intro - -Writing code is plenty of fun on its own, but its rough having to wait for mechanical to be done to test it. -When we only have 2-3 weeks of time (which we have to share with drive practice!) after the robot is built before our first competition, we need to be able to test before the robot is ready. -Luckily, we have a handy piece of code that to refactor to be simulateable! -In this tutorial we are going to rework your kitbot code to be easy to simulate and expand on. -In the end your code should be able to do this: - -[Video of robot in sim](../../Assets/SimDemo.mp4) - -## Code Walkthrough - -### Moving to AdvantageKit - -The first step of moving our standard Command-based code to a loggable, simulateable structure is to install AdvantageKit. -Luckily AdvantageKit has a handy guide on how to install it on an existing code base. -Follow [this walkthrough](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/INSTALLATION.md). -Follow all the steps in the doc through adding the `@AutoLog` annotation. -You do not need to add the suggested block in `Robot()`, instead use the one below. - -```Java -Logger.getInstance().recordMetadata("ProjectName", "KitbotExample"); // Set a metadata value - - if (isReal()) { - Logger.getInstance().addDataReceiver(new WPILOGWriter("/media/sda1/")); // Log to a USB stick - Logger.getInstance().addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging - } else { - Logger.getInstance().addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - } - - Logger.getInstance().start(); // Start logging! No more data receivers, replay sources, or metadata values may be added. -``` - -The suggested logging configuration is built around replaying logs, while we just want to simulate code. - -Once you've completed this, it's time to break `DrivetrainSubsystem` appart using the IO layer structure. - -### Refactoring `DrivetrainSubsystem` - -Recall the [IO layer structure](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/CODE-STRUCTURE.md) for robot code. -Right now we have one file for `DrivetrainSubsystem`. -Lets put it in a `Drivetrain` folder under `Subsystems` so that everything stays organized as we add more files. - -![A screenshot showing the new folder](../../Assets/KitbotExampleSimScreenshot0.png) - -Next lets 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. - -```Java -package frc.robot.Subsystems.Drivetrain; - -public interface DrivetrainIO {} -``` - -Inside of this interface lets define our `IOInputs` class. -Remember this is a container for all of the inputs (sensor readings) and outputs (motor commands) for this mechanism. - -```Java -public static class DrivetrainIOInputs {} -``` - -Let's start by logging the output voltage of each side of the drivetrain. - -```Java -public static class DrivetrainIOInputs { - public double leftOutputVolts = 0.0; - public double rightOutputVolts = 0.0; -} -``` - -Notice how we name our fields with units. -**It is very important to include units in all IOInputs variable names**. -It is very easy to assume different units in different places, which can cause frustrating and hard to find bugs. - -Next lets log the current velocity of each side of the drivetrain. - -```Java -public static class DrivetrainIOInputs { - public double leftOutputVolts = 0.0; - public double rightOutputVolts = 0.0; - - public double leftVelocityMetersPerSecond = 0.0; - public double rightVelocityMetersPerSecond = 0.0; -} -``` - -Notice the units. -This field is useful both for being able to look at drivetrain performance but also for being able to track our position over time. -In fact, let's add a field for the current position of each side. - -```Java -public static class DrivetrainIOInputs { - // Snip - public double leftPositionMeters = 0.0; - public double rightPositionMeters = 0.0; -} -``` - -These values are useful for odometry. -Odometry is a way to track the robots position on the field by measuring how much each wheel is turning. -On a real robot we use it for auto so that we can draw paths on a map of the field instead of guessing where the robot should go. -For this sim we will use odometry to give us a position that we can use to visualize the robot on a fake field. - -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 it's desired output, and is measured in Amperes or amps. -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. -Logging the temparature of our motors can help us debug mechanisms which have problems near the end of a match. -Normally this issue is not too pronounced over the course of a single match. -However, in elims where we have a number of intense matches back to back or during drive practice where we run continuously for hours this can become an issue. -This simulation will not include temparature simulation, but its good to get into the habit of including it in your logged values. - -```Java -public static class DrivetrainIOInputs { - // Snip - public double[] leftCurrentAmps = new double[0]; - public double[] leftTempCelsius = new double[0]; - public double[] rightCurrentAmps = new double[0]; - public double[] rightTempCelsius = new double[0]; -} -``` - -Notice how these values arent doubles, but instead are arrays of doubles. -This is because we might have more than one motor on each side of the drivetrain, and we want to log the state of each of them. - -Finally to finish this IOInputs class we need to add the `@AutoLog` annotation above it's definition. -This annotation automatically generates code to convert these values to a loggable format and back. -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 you can follow the [AdvantageKit docs example](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/CODE-STRUCTURE.md#autolog-annotation) on it. - -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. -Because we are just defining the interface instead of the actual method, we can call it an `abstract` method. -`abstract` means that it won't have any default behaviour but will require our IO Implementations to implement it. - -```Java -public abstract void updateInputs(DrivetrainIOInputs inputs); -``` - -Finally, lets add the methods that we use to interact with the hardware. -For this case it will just be a method `setVolts` to set the left and right output volts, like our `setVoltages` method that we have right now. - -Now your `DrivetrainIO` file should look like this: - -```Java -package frc.robot.Subsystems.Drivetrain; - -import org.littletonrobotics.junction.AutoLog; - -public interface DrivetrainIO { - @AutoLog - public static class DrivetrainIOInputs { - public double leftOutputVolts = 0.0; - public double rightOutputVolts = 0.0; - - public double leftVelocityMetersPerSecond = 0.0; - public double rightVelocityMetersPerSecond = 0.0; - - public double leftPositionMeters = 0.0; - public double rightPositionMeters = 0.0; - - public double[] leftCurrentAmps = new double[0]; - public double[] leftTempCelsius = new double[0]; - public double[] rightCurrentAmps = new double[0]; - public double[] rightTempCelsius = new double[0]; - } - - public abstract void updateInputs(DrivetrainIOInputs inputs); - - public abstract void setVolts(double left, double right); -} -``` - -Next lets add the IO Implementation. -This will be a file that defines the `updateInputs()` and `setVolts()` method. -Since we aren't writing this for real hardware, we are going to make `DrivetrainIOSim`. -On a real robot we would also have a `DrivetrainIOFalcon` or something similar, named after the type of hardware it is built with. - -Start by making a new class called `DrivetrainIOSim` 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 DrivetrainIOSim implements DrivetrainIO {} -``` - -This will make vscode angry since `DrivetrainIOSim` needs to implement all the abstract 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 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 - - } -} -``` - -Before we get into implementing these, we need to add the hardware to this IO Implementation. -Add in the falcons from `DrivetrainSubsystem`. - -```Java -TalonFX leftFalcon = new TalonFX(Constants.drivetrainLeftFalconID); -TalonFX rightFalcon = new TalonFX(Constants.drivetrainRightFalconID); -``` - -Lets 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; - - public double leftVelocityMetersPerSecond = 0.0; - public double rightVelocityMetersPerSecond = 0.0; - - public double leftPositionMeters = 0.0; - public double rightPositionMeters = 0.0; - - public double[] leftCurrentAmps = new double[0]; - public double[] leftTempCelsius = new double[0]; - public double[] rightCurrentAmps = new double[0]; - public double[] rightTempCelsius = new double[0]; -} -``` - -Then replace the `public` and types with `inputs.` - -```Java -@Override -public void updateInputs(DrivetrainIOInputs inputs) { - inputs.leftOutputVolts = 0.0; - inputs.rightOutputVolts = 0.0; - - inputs.leftVelocityMetersPerSecond = 0.0; - inputs.rightVelocityMetersPerSecond = 0.0; - - inputs.leftPositionMeters = 0.0; - inputs.rightPositionMeters = 0.0; - - inputs.leftCurrentAmps = new double[0]; - inputs.leftTempCelsius = new double[0]; - inputs.rightCurrentAmps = new double[0]; - inputs.rightTempCelsius = new double[0]; -} -``` - -Some of these fields we can get directly from a simulated falcon. -TalonFX comes with built-in simulation support, so we can treat it like a regular motor for the most part. -To get the simulated outputs we can call `talon.getSimState()` and store the results in a variable. -Do this in `updateInputs`. - -```Java -@Override -public void updateInputs(DrivetrainIOInputs inputs) { - var leftSimState = leftFalcon.getSimState(); - var rightSimState = rightFalcon.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 -} -``` - -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. - -```Java -@Override -public void updateInputs(DrivetrainIOInputs inputs) { - // Snip - inputs.leftCurrentAmps = new double[] {leftSimState.getTorqueCurrent()}; - inputs.leftTempCelsius = new double[0]; - inputs.rightCurrentAmps = new double[] {rightSimState.getTorqueCurrent()}; - inputs.rightTempCelsius = new double[0]; -} -``` - -This information is useless without having a way to set the motor voltage. -Copy the `VoltageOut` objects from `DrivetrainSubsystem` below the `TalonFX`s. - -```Java -TalonFX leftFalcon = new TalonFX(Constants.drivetrainLeftFalconID); -TalonFX rightFalcon = new TalonFX(Constants.drivetrainRightFalconID); - -VoltageOut leftVoltage = new VoltageOut(0); -VoltageOut rightVoltage = new VoltageOut(0); -``` - -Then copy the `setVoltages` method from `DrivetrainSubsystem` into the `setVoltage` method. - -```Java -@Override -public void setVolts(double left, double right) { - leftFalcon.setControl(leftVoltage.withOutput(left)); - rightFalcon.setControl(rightVoltage.withOutput(right)); -} -``` - -Finally we need to add a physics sim. -This will take these motor voltages and figure out how the mechanims 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. - -```Java -DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( - null, - null, - null, - null); -``` - -The first value in this constructor is the motor for the sim. -We can use `KitbotMotor.kDoubleFalcon500PerSide` 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, - null, - null, - null); -``` - -The second parameter is the gearing of the drivetrain. -This is something we normally would ask mechanical for. -In this case, since the drivetrain is simulated, we can use the standard 8p45 gearbox. - -```Java -DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( - KitbotMotor.kDoubleFalcon500PerSide, - KitbotGearing.k8p45, - null, - null); -``` - -The third parameter is the size of the wheels. -By default the kitbot comes with 6 inch wheels. - -```Java -DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( - KitbotMotor.kDoubleFalcon500PerSide, - KitbotGearing.k8p45, - KitbotWheelSize.kSixInch, - null); -``` - -The final parameter is the "measurement standard deviations". -This is a way for us to add uncertainty to our simulated model in a way that matches real sensor noise. -For this exercise we can ignore it and leave it as null. - -Now we have a fully set up physics sim. -On real robots we often have to be more careful when finding these physical constans, 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()`. - -```Java -@Override - public void updateInputs(DrivetrainIOInputs inputs) { - physicsSim.update(0.020); - - var leftSimState = leftFalcon.getSimState(); - var rightSimState = rightFalcon.getSimState(); - // Snip -} -``` - -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. - -```Java -@Override - public void updateInputs(DrivetrainIOInputs inputs) { - physicsSim.update(0.020); - - var leftSimState = leftFalcon.getSimState(); - var rightSimState = rightFalcon.getSimState(); - - physicsSim.setInputs(leftSimState.getMotorVoltage(), rightSimState.getMotorVoltage()); - // Snip -} -``` - -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`. - -```Java -@Override -public void updateInputs(DrivetrainIOInputs inputs) { - // Snip - inputs.leftVelocityMetersPerSecond = physicsSim.getLeftVelocityMetersPerSecond(); - inputs.rightVelocityMetersPerSecond = physicsSim.getRightVelocityMetersPerSecond(); - - inputs.leftPositionMeters = physicsSim.getLeftPositionMeters(); - inputs.rightPositionMeters = physicsSim.getRightPositionMeters(); - // Snip -} -``` - -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. - -```Java -@Override -public void updateInputs(DrivetrainIOInputs inputs) { - physicsSim.update(0.020); - - var leftSimState = leftFalcon.getSimState(); - leftSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); - - var rightSimState = rightFalcon.getSimState(); - rightSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); - // Snip -} -``` - -Now we're done with the `DrivetrainIOSim` class! πŸŽ‰ - -Let's refactor our `DrivetrainSubsystem` to finish off this rewrite. - -First, we need to replace all the functionality that moved to `DrivetrainIOSim` with an instance of `DrivetrainIOSim`. -Get rid of the `TalonFX` objects and `VoltageOut` requests. - -```Java -public class DrivetrainSubsystem extends SubsystemBase { - DrivetrainIO io = new DrivetrainIOSim(); - // Snip -} -``` - -Then we should add an `IOInputs` object to track the inputs to the subsystem. -Theres a catch though: since we used `@AutoLog` we have to use `DrivetrainIOInputsAutoLogged`, otherwise it won't properly log. - -```Java -public class DrivetrainSubsystem extends SubsystemBase { - DrivetrainIO io = new DrivetrainIOSim(); - DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); -} -``` - -Now we need to finish the AdvantageKit io setup. -Add a call to `io.updateInputs(inputs)` in the `periodic()` method of `DrivetrainSubsystem`. - -```Java -@Override -public void periodic() { - io.updateInputs(inputs); -} -``` - -Then we need to push those inputs to the log. -We do this by calling `Logger.getInstance().processInputs("Drivetrain", inputs)`. - -```Java -@Override -public void periodic() { - io.updateInputs(inputs); - Logger.getInstance().processInputs("Drivetrain", inputs); -} -``` - -Finally, lets change the `setVoltages` method to use the io layer. - -```Java -private void setVoltages(double left, double right) { - io.setVolts(left, right); -} -``` - -This will work as a sim! -But it won't have any way to track the position of the drivebase. -Remember that we can use odometry to track the position of a drivebase. -We can create a new `DifferentialDriveOdometry` object in `DrivetrainSubsystem` to handle this. - -```Java -public class DrivetrainSubsystem extends SubsystemBase { - DrivetrainIO io = new DrivetrainIOSim(); - DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); - - DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0); - // Snip -} -``` - -We will need to update this every processor loop. -Normally we would have a gyro to track the heading (rotation) of the drive base. -However, in simulation we don't have an easy way to make a gyro that behaves how we would expect. -Instead, we can use the following line to fudge it: - -```Java -odometry.getPoseMeters().getRotation() - .plus( - Rotation2d.fromRadians( - (inputs.leftSpeedMetersPerSecond - inputs.rightSpeedMetersPerSecond) - * 0.020 / Units.inchesToMeters(26))) -``` - -This line is using a small piece of math to calculate what the new rotation should be based off of our wheel speeds. -You don't need to know how this works in particular, just know it's a bit of a hack. - -We are going to wrap this in an `odometry.update()` call. - -```Java -@Override -public void periodic() { -io.updateInputs(inputs); -Logger.getInstance().processInputs("Drivetrain", inputs); - -odometry.update( - odometry.getPoseMeters().getRotation() - // Use differential drive kinematics to find the rotation rate based on the wheel speeds and distance between wheels - .plus(Rotation2d.fromRadians((inputs.leftVelocityMetersPerSecond - inputs.rightVelocityMetersPerSecond) - * 0.020 / Units.inchesToMeters(26))), - inputs.leftPositionMeters, inputs.rightPositionMeters); -} -``` - -Finally, we need to send the odometry pose to the logger so we can visualize it. -We do this using `Logger.getInstance().recordOutput()` and `odometry.getPoseMeters()`. - -```Java -@Override -public void periodic() { - // Snip - Logger.getInstance().recordOutput("Drivebase Pose", odometry.getPoseMeters()); -} -``` - -This should be everything you need to have a full drivebase simulation! - -### Running the Simulation - -To run a robot code simulation, click on the WPILib icon in the top right corner. -Then find or search for "Simulate Robot Code" and press it. -Once the code builds, it will prompt you to choose between using the driver station and sim gui. -Choose the sim gui. - -The sim gui is perfectly serviceable for testing, but we can do better. -AdvantageScope supports connecting to a simulation to visualize IO. -When you open AdvantageScope, hit "file", then "Connect to Simulator". -This lets you visualize data from the simulator in the same way as you would with a real robot. -You can use the 3d field visualization to have a fun way to show the robot on the field. - -To drive the robot, add "keyboard 0" to "joystick 0" in the sim gui. -You might have to change your keyboard settings in the sim gui to make it properly emulate a joystick. -Then make sure to set the mode to teleop. diff --git a/Docs/2_Architecture/2.8_Simulation.md b/Docs/2_Architecture/2.9_Simulation.md similarity index 100% rename from Docs/2_Architecture/2.8_Simulation.md rename to Docs/2_Architecture/2.9_Simulation.md diff --git a/Examples/KitbotDemoBasic/.gitignore b/Examples/2.10_KitbotSim/.gitignore similarity index 100% rename from Examples/KitbotDemoBasic/.gitignore rename to Examples/2.10_KitbotSim/.gitignore diff --git a/Examples/KitbotDemoBasic/.vscode/launch.json b/Examples/2.10_KitbotSim/.vscode/launch.json similarity index 100% rename from Examples/KitbotDemoBasic/.vscode/launch.json rename to Examples/2.10_KitbotSim/.vscode/launch.json diff --git a/Examples/KitbotDemoBasic/.vscode/settings.json b/Examples/2.10_KitbotSim/.vscode/settings.json similarity index 100% rename from Examples/KitbotDemoBasic/.vscode/settings.json rename to Examples/2.10_KitbotSim/.vscode/settings.json diff --git a/Examples/KitbotDemoBasic/.wpilib/wpilib_preferences.json b/Examples/2.10_KitbotSim/.wpilib/wpilib_preferences.json similarity index 100% rename from Examples/KitbotDemoBasic/.wpilib/wpilib_preferences.json rename to Examples/2.10_KitbotSim/.wpilib/wpilib_preferences.json diff --git a/Examples/KitbotDemoBasic/WPILib-License.md b/Examples/2.10_KitbotSim/WPILib-License.md similarity index 100% rename from Examples/KitbotDemoBasic/WPILib-License.md rename to Examples/2.10_KitbotSim/WPILib-License.md diff --git a/Examples/KitbotDemoFinal/build.gradle b/Examples/2.10_KitbotSim/build.gradle similarity index 100% rename from Examples/KitbotDemoFinal/build.gradle rename to Examples/2.10_KitbotSim/build.gradle diff --git a/Examples/KitbotDemoBasic/gradle/wrapper/gradle-wrapper.jar b/Examples/2.10_KitbotSim/gradle/wrapper/gradle-wrapper.jar similarity index 100% rename from Examples/KitbotDemoBasic/gradle/wrapper/gradle-wrapper.jar rename to Examples/2.10_KitbotSim/gradle/wrapper/gradle-wrapper.jar diff --git a/Examples/KitbotDemoBasic/gradle/wrapper/gradle-wrapper.properties b/Examples/2.10_KitbotSim/gradle/wrapper/gradle-wrapper.properties similarity index 100% rename from Examples/KitbotDemoBasic/gradle/wrapper/gradle-wrapper.properties rename to Examples/2.10_KitbotSim/gradle/wrapper/gradle-wrapper.properties diff --git a/Examples/KitbotDemoBasic/gradlew b/Examples/2.10_KitbotSim/gradlew similarity index 100% rename from Examples/KitbotDemoBasic/gradlew rename to Examples/2.10_KitbotSim/gradlew diff --git a/Examples/KitbotDemoBasic/gradlew.bat b/Examples/2.10_KitbotSim/gradlew.bat similarity index 100% rename from Examples/KitbotDemoBasic/gradlew.bat rename to Examples/2.10_KitbotSim/gradlew.bat diff --git a/Examples/KitbotDemoBasic/settings.gradle b/Examples/2.10_KitbotSim/settings.gradle similarity index 100% rename from Examples/KitbotDemoBasic/settings.gradle rename to Examples/2.10_KitbotSim/settings.gradle diff --git a/Examples/KitbotDemoBasic/src/main/deploy/example.txt b/Examples/2.10_KitbotSim/src/main/deploy/example.txt similarity index 100% rename from Examples/KitbotDemoBasic/src/main/deploy/example.txt rename to Examples/2.10_KitbotSim/src/main/deploy/example.txt diff --git a/Examples/KitbotDemoBasic/src/main/java/frc/robot/Main.java b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Main.java similarity index 100% rename from Examples/KitbotDemoBasic/src/main/java/frc/robot/Main.java rename to Examples/2.10_KitbotSim/src/main/java/frc/robot/Main.java diff --git a/Examples/KitbotDemoSim/src/main/java/frc/robot/Robot.java b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Robot.java similarity index 74% rename from Examples/KitbotDemoSim/src/main/java/frc/robot/Robot.java rename to Examples/2.10_KitbotSim/src/main/java/frc/robot/Robot.java index e3f9502..cca3a87 100644 --- a/Examples/KitbotDemoSim/src/main/java/frc/robot/Robot.java +++ b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Robot.java @@ -11,17 +11,18 @@ import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Subsystems.Drivetrain.DrivetrainSubsystem; public class Robot extends LoggedRobot { - private Command m_autonomousCommand; - private RobotContainer m_robotContainer; + CommandXboxController controller = new CommandXboxController(0); - @Override - @SuppressWarnings("resource") - public void robotInit() { + DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); + + + public Robot() { Logger.recordMetadata("ProjectName", "KitbotExample"); // Set a metadata value if (isReal()) { @@ -32,9 +33,23 @@ public void robotInit() { Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables } - Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may - // be added. - m_robotContainer = new RobotContainer(); + Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added. + + drivetrainSubsystem.setDefaultCommand( + drivetrainSubsystem.setVoltagesArcadeCommand( + () -> modifyJoystick(controller.getLeftY()), + () -> modifyJoystick(controller.getRightX()))); + } + + private double modifyJoystick(double in) { + if (Math.abs(in) < 0.05) { + return 0; + } + return in * in * Math.signum(in); + } + + @Override + public void robotInit() { } @Override @@ -53,11 +68,6 @@ public void disabledExit() {} @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } } @Override @@ -68,9 +78,6 @@ public void autonomousExit() {} @Override public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } } @Override diff --git a/Examples/KitbotDemoSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java similarity index 66% rename from Examples/KitbotDemoSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java rename to Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java index e043a88..fdfb2a2 100644 --- a/Examples/KitbotDemoSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java +++ b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java @@ -18,13 +18,13 @@ public static class DrivetrainIOInputs { public double leftPositionMeters = 0.0; public double rightPositionMeters = 0.0; - public double[] leftCurrentAmps = new double[0]; - public double[] leftTempCelsius = new double[0]; - public double[] rightCurrentAmps = new double[0]; - public double[] rightTempCelsius = new double[0]; + public double leftCurrentAmps = 0.0; + public double leftTempCelsius = 0.0; + public double rightCurrentAmps = 0.0; + public double rightTempCelsius = 0.0; } - public abstract void updateInputs(DrivetrainIOInputs inputs); + public void updateInputs(DrivetrainIOInputs inputs); - public abstract void setVolts(double left, double right); + public void setVolts(double left, double right); } 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 new file mode 100644 index 0000000..9f84186 --- /dev/null +++ b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOReal.java @@ -0,0 +1,94 @@ +// 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/KitbotDemoFinal/src/main/java/frc/robot/subsystems/Drivetrain/DrivetrainIOSim.java b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOSim.java similarity index 55% rename from Examples/KitbotDemoFinal/src/main/java/frc/robot/subsystems/Drivetrain/DrivetrainIOSim.java rename to Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOSim.java index e980479..ee4cb91 100644 --- a/Examples/KitbotDemoFinal/src/main/java/frc/robot/subsystems/Drivetrain/DrivetrainIOSim.java +++ b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOSim.java @@ -2,41 +2,29 @@ // 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.VelocityDutyCycle; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; 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; -/** Sim implementation of Kitbot drivetrain. */ 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 leftFalcon = new TalonFX(0); - TalonFX rightFalcon = new TalonFX(1); + 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 leftVoltageControl = new VoltageOut(0); - VoltageOut rightVoltageControl = new VoltageOut(0); - - VelocityDutyCycle leftVelocityControl = new VelocityDutyCycle(0); - VelocityDutyCycle rightVelocityControl = new VelocityDutyCycle(0); - - // Track if we're using voltage (open loop) or velocity (closed loop) - boolean isClosedLoop = false; - - // Physical constants for converting from motor speed to chassis speed - double gearRatio = DifferentialDrivetrainSim.KitbotGearing.k8p45.value; - double wheelDiameter = DifferentialDrivetrainSim.KitbotWheelSize.kSixInch.value; + 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 @@ -60,46 +48,33 @@ public void updateInputs(DrivetrainIOInputs inputs) { // 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 = leftFalcon.getSimState(); + var leftSimState = leftTalon.getSimState(); leftSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); - var rightSimState = rightFalcon.getSimState(); + var rightSimState = rightTalon.getSimState(); rightSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); // Use the motor output voltage to update the sim physicsSim.setInputs(leftSimState.getMotorVoltage(), rightSimState.getMotorVoltage()); - inputs.leftSpeedMetersPerSecond = physicsSim.getLeftVelocityMetersPerSecond(); - inputs.leftPositionMeters = physicsSim.getLeftPositionMeters(); - inputs.rightSpeedMetersPerSecond = physicsSim.getRightVelocityMetersPerSecond(); - inputs.rightPositionMeters = physicsSim.getRightPositionMeters(); - inputs.leftOutputVolts = leftSimState.getMotorVoltage(); - inputs.leftGoalMetersPerSecond = (leftVelocityControl.Velocity * Math.PI * wheelDiameter) / gearRatio; inputs.rightOutputVolts = rightSimState.getMotorVoltage(); - inputs.rightGoalMetersPerSecond = (rightVelocityControl.Velocity * Math.PI * wheelDiameter) / gearRatio; - inputs.isClosedLoop = isClosedLoop; - // If we had multiple motors per side, we would add all of them to the array - inputs.leftCurrentAmps = new double[] {leftSimState.getTorqueCurrent()}; - inputs.leftTempCelsius = new double[] {0.0}; // For sim, we're going to ignore temparature - // If we had multiple motors per side, we would add all of them to the array - inputs.rightCurrentAmps = new double[] {rightSimState.getTorqueCurrent()}; - inputs.rightTempCelsius = new double[] {0.0}; // For sim, we're going to ignore temparature - } + inputs.leftVelocityMetersPerSecond = physicsSim.getLeftVelocityMetersPerSecond(); + inputs.rightVelocityMetersPerSecond = physicsSim.getRightVelocityMetersPerSecond(); - @Override - public void setVolts(double left, double right) { - isClosedLoop = false; - leftFalcon.setControl(leftVoltageControl.withOutput(left)); - rightFalcon.setControl(rightVoltageControl.withOutput(right)); + 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 setMetersPerSecond(double left, double right) { - isClosedLoop = true; - leftFalcon.setControl(leftVelocityControl.withVelocity((left * gearRatio) / (Math.PI * wheelDiameter))); - rightFalcon.setControl(rightVelocityControl.withVelocity((right * gearRatio) / (Math.PI * wheelDiameter))); + public void setVolts(double left, double right) { + leftTalon.setControl(leftVoltage.withOutput(left)); + rightTalon.setControl(rightVoltage.withOutput(right)); } - } 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 new file mode 100644 index 0000000..e1214e0 --- /dev/null +++ b/Examples/2.10_KitbotSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java @@ -0,0 +1,64 @@ +// 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 java.util.function.DoubleSupplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; + +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(); + DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); + + // Odometry keeps track of our position on the field + // We can upgrade this to a pose estimator, which fuses vision readings to + // correct our estimate + DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0); + + /** Creates a new Drivetrain. */ + public DrivetrainSubsystem() { + } + + private void setVoltages(double left, double right) { + io.setVolts(left, right); + } + + public Command setVoltagesCommand(DoubleSupplier left, DoubleSupplier right) { + return this.run(() -> this.setVoltages(left.getAsDouble(), right.getAsDouble())); + } + + public Command setVoltagesArcadeCommand(DoubleSupplier drive, DoubleSupplier steer) { + return this.run(() -> { + var speeds = DifferentialDrive.arcadeDriveIK(drive.getAsDouble(), steer.getAsDouble(), false); + this.setVoltages(speeds.left * 12, speeds.right * 12); + }); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drivetrain", inputs); + + odometry.update( + odometry.getPoseMeters().getRotation() + // Use differential drive kinematics to find the rotation rate based on the + // wheel speeds and distance between wheels + .plus(Rotation2d.fromRadians((inputs.leftVelocityMetersPerSecond - inputs.rightVelocityMetersPerSecond) + * 0.020 / Units.inchesToMeters(26))), + inputs.leftPositionMeters, inputs.rightPositionMeters); + Logger.recordOutput("Drivetrain Pose", odometry.getPoseMeters()); + } +} diff --git a/Examples/KitbotDemoFinal/vendordeps/AdvantageKit.json b/Examples/2.10_KitbotSim/vendordeps/AdvantageKit.json similarity index 100% rename from Examples/KitbotDemoFinal/vendordeps/AdvantageKit.json rename to Examples/2.10_KitbotSim/vendordeps/AdvantageKit.json diff --git a/Examples/KitbotDemoBasic/vendordeps/Phoenix6-frc2025-latest.json b/Examples/2.10_KitbotSim/vendordeps/Phoenix6-frc2025-latest.json similarity index 100% rename from Examples/KitbotDemoBasic/vendordeps/Phoenix6-frc2025-latest.json rename to Examples/2.10_KitbotSim/vendordeps/Phoenix6-frc2025-latest.json diff --git a/Examples/KitbotDemoBasic/vendordeps/WPILibNewCommands.json b/Examples/2.10_KitbotSim/vendordeps/WPILibNewCommands.json similarity index 100% rename from Examples/KitbotDemoBasic/vendordeps/WPILibNewCommands.json rename to Examples/2.10_KitbotSim/vendordeps/WPILibNewCommands.json diff --git a/Examples/KitbotDemoFinal/.gitignore b/Examples/2.5_KitbotIntro/.gitignore similarity index 100% rename from Examples/KitbotDemoFinal/.gitignore rename to Examples/2.5_KitbotIntro/.gitignore diff --git a/Examples/KitbotDemoFinal/.vscode/launch.json b/Examples/2.5_KitbotIntro/.vscode/launch.json similarity index 100% rename from Examples/KitbotDemoFinal/.vscode/launch.json rename to Examples/2.5_KitbotIntro/.vscode/launch.json diff --git a/Examples/KitbotDemoFinal/.vscode/settings.json b/Examples/2.5_KitbotIntro/.vscode/settings.json similarity index 100% rename from Examples/KitbotDemoFinal/.vscode/settings.json rename to Examples/2.5_KitbotIntro/.vscode/settings.json diff --git a/Examples/KitbotDemoFinal/.wpilib/wpilib_preferences.json b/Examples/2.5_KitbotIntro/.wpilib/wpilib_preferences.json similarity index 100% rename from Examples/KitbotDemoFinal/.wpilib/wpilib_preferences.json rename to Examples/2.5_KitbotIntro/.wpilib/wpilib_preferences.json diff --git a/Examples/KitbotDemoFinal/WPILib-License.md b/Examples/2.5_KitbotIntro/WPILib-License.md similarity index 100% rename from Examples/KitbotDemoFinal/WPILib-License.md rename to Examples/2.5_KitbotIntro/WPILib-License.md diff --git a/Examples/KitbotDemoBasic/build.gradle b/Examples/2.5_KitbotIntro/build.gradle similarity index 100% rename from Examples/KitbotDemoBasic/build.gradle rename to Examples/2.5_KitbotIntro/build.gradle diff --git a/Examples/KitbotDemoFinal/gradle/wrapper/gradle-wrapper.jar b/Examples/2.5_KitbotIntro/gradle/wrapper/gradle-wrapper.jar similarity index 100% rename from Examples/KitbotDemoFinal/gradle/wrapper/gradle-wrapper.jar rename to Examples/2.5_KitbotIntro/gradle/wrapper/gradle-wrapper.jar diff --git a/Examples/KitbotDemoFinal/gradle/wrapper/gradle-wrapper.properties b/Examples/2.5_KitbotIntro/gradle/wrapper/gradle-wrapper.properties similarity index 100% rename from Examples/KitbotDemoFinal/gradle/wrapper/gradle-wrapper.properties rename to Examples/2.5_KitbotIntro/gradle/wrapper/gradle-wrapper.properties diff --git a/Examples/KitbotDemoFinal/gradlew b/Examples/2.5_KitbotIntro/gradlew similarity index 100% rename from Examples/KitbotDemoFinal/gradlew rename to Examples/2.5_KitbotIntro/gradlew diff --git a/Examples/KitbotDemoFinal/gradlew.bat b/Examples/2.5_KitbotIntro/gradlew.bat similarity index 100% rename from Examples/KitbotDemoFinal/gradlew.bat rename to Examples/2.5_KitbotIntro/gradlew.bat diff --git a/Examples/KitbotDemoFinal/settings.gradle b/Examples/2.5_KitbotIntro/settings.gradle similarity index 100% rename from Examples/KitbotDemoFinal/settings.gradle rename to Examples/2.5_KitbotIntro/settings.gradle diff --git a/Examples/KitbotDemoFinal/src/main/deploy/example.txt b/Examples/2.5_KitbotIntro/src/main/deploy/example.txt similarity index 100% rename from Examples/KitbotDemoFinal/src/main/deploy/example.txt rename to Examples/2.5_KitbotIntro/src/main/deploy/example.txt diff --git a/Examples/KitbotDemoFinal/src/main/java/frc/robot/Main.java b/Examples/2.5_KitbotIntro/src/main/java/frc/robot/Main.java similarity index 100% rename from Examples/KitbotDemoFinal/src/main/java/frc/robot/Main.java rename to Examples/2.5_KitbotIntro/src/main/java/frc/robot/Main.java diff --git a/Examples/KitbotDemoBasic/src/main/java/frc/robot/Robot.java b/Examples/2.5_KitbotIntro/src/main/java/frc/robot/Robot.java similarity index 64% rename from Examples/KitbotDemoBasic/src/main/java/frc/robot/Robot.java rename to Examples/2.5_KitbotIntro/src/main/java/frc/robot/Robot.java index b68462c..54d575e 100644 --- a/Examples/KitbotDemoBasic/src/main/java/frc/robot/Robot.java +++ b/Examples/2.5_KitbotIntro/src/main/java/frc/robot/Robot.java @@ -5,17 +5,33 @@ package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Subsystems.DrivetrainSubsystem; public class Robot extends TimedRobot { - private Command m_autonomousCommand; - private RobotContainer m_robotContainer; + CommandXboxController controller = new CommandXboxController(0); + + DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); + + + public Robot() { + drivetrainSubsystem.setDefaultCommand( + drivetrainSubsystem.setVoltagesArcadeCommand( + () -> modifyJoystick(controller.getLeftY()), + () -> modifyJoystick(controller.getRightX()))); + } + + private double modifyJoystick(double in) { + if (Math.abs(in) < 0.05) { + return 0; + } + return in * in * Math.signum(in); + } @Override public void robotInit() { - m_robotContainer = new RobotContainer(); } @Override @@ -34,11 +50,6 @@ public void disabledExit() {} @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } } @Override @@ -49,9 +60,6 @@ public void autonomousExit() {} @Override public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } } @Override diff --git a/Examples/KitbotDemoBasic/src/main/java/frc/robot/Subsystems/DrivetrainSubsystem.java b/Examples/2.5_KitbotIntro/src/main/java/frc/robot/Subsystems/DrivetrainSubsystem.java similarity index 81% rename from Examples/KitbotDemoBasic/src/main/java/frc/robot/Subsystems/DrivetrainSubsystem.java rename to Examples/2.5_KitbotIntro/src/main/java/frc/robot/Subsystems/DrivetrainSubsystem.java index ee79ae2..0378a9f 100644 --- a/Examples/KitbotDemoBasic/src/main/java/frc/robot/Subsystems/DrivetrainSubsystem.java +++ b/Examples/2.5_KitbotIntro/src/main/java/frc/robot/Subsystems/DrivetrainSubsystem.java @@ -12,11 +12,13 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; public class DrivetrainSubsystem extends SubsystemBase { - TalonFX leftFalcon = new TalonFX(Constants.drivetrainLeftFalconID); - TalonFX rightFalcon = new TalonFX(Constants.drivetrainRightFalconID); + 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); VoltageOut leftVoltage = new VoltageOut(0); VoltageOut rightVoltage = new VoltageOut(0); @@ -26,8 +28,8 @@ public DrivetrainSubsystem() { } private void setVoltages(double left, double right) { - leftFalcon.setControl(leftVoltage.withOutput(left)); - rightFalcon.setControl(rightVoltage.withOutput(left)); + leftTalon.setControl(leftVoltage.withOutput(left)); + rightTalon.setControl(rightVoltage.withOutput(left)); } public Command setVoltagesCommand(DoubleSupplier left, DoubleSupplier right) { diff --git a/Examples/KitbotDemoFinal/vendordeps/Phoenix6-frc2025-latest.json b/Examples/2.5_KitbotIntro/vendordeps/Phoenix6-frc2025-latest.json similarity index 100% rename from Examples/KitbotDemoFinal/vendordeps/Phoenix6-frc2025-latest.json rename to Examples/2.5_KitbotIntro/vendordeps/Phoenix6-frc2025-latest.json diff --git a/Examples/KitbotDemoFinal/vendordeps/WPILibNewCommands.json b/Examples/2.5_KitbotIntro/vendordeps/WPILibNewCommands.json similarity index 100% rename from Examples/KitbotDemoFinal/vendordeps/WPILibNewCommands.json rename to Examples/2.5_KitbotIntro/vendordeps/WPILibNewCommands.json diff --git a/Examples/KitbotDemoSim/.gitignore b/Examples/2.8_KitbotAKitRefactor/.gitignore similarity index 100% rename from Examples/KitbotDemoSim/.gitignore rename to Examples/2.8_KitbotAKitRefactor/.gitignore diff --git a/Examples/KitbotDemoSim/.vscode/launch.json b/Examples/2.8_KitbotAKitRefactor/.vscode/launch.json similarity index 100% rename from Examples/KitbotDemoSim/.vscode/launch.json rename to Examples/2.8_KitbotAKitRefactor/.vscode/launch.json diff --git a/Examples/KitbotDemoSim/.vscode/settings.json b/Examples/2.8_KitbotAKitRefactor/.vscode/settings.json similarity index 100% rename from Examples/KitbotDemoSim/.vscode/settings.json rename to Examples/2.8_KitbotAKitRefactor/.vscode/settings.json diff --git a/Examples/KitbotDemoSim/.wpilib/wpilib_preferences.json b/Examples/2.8_KitbotAKitRefactor/.wpilib/wpilib_preferences.json similarity index 100% rename from Examples/KitbotDemoSim/.wpilib/wpilib_preferences.json rename to Examples/2.8_KitbotAKitRefactor/.wpilib/wpilib_preferences.json diff --git a/Examples/KitbotDemoSim/WPILib-License.md b/Examples/2.8_KitbotAKitRefactor/WPILib-License.md similarity index 100% rename from Examples/KitbotDemoSim/WPILib-License.md rename to Examples/2.8_KitbotAKitRefactor/WPILib-License.md diff --git a/Examples/KitbotDemoSim/build.gradle b/Examples/2.8_KitbotAKitRefactor/build.gradle similarity index 100% rename from Examples/KitbotDemoSim/build.gradle rename to Examples/2.8_KitbotAKitRefactor/build.gradle diff --git a/Examples/KitbotDemoSim/gradle/wrapper/gradle-wrapper.jar b/Examples/2.8_KitbotAKitRefactor/gradle/wrapper/gradle-wrapper.jar similarity index 100% rename from Examples/KitbotDemoSim/gradle/wrapper/gradle-wrapper.jar rename to Examples/2.8_KitbotAKitRefactor/gradle/wrapper/gradle-wrapper.jar diff --git a/Examples/KitbotDemoSim/gradle/wrapper/gradle-wrapper.properties b/Examples/2.8_KitbotAKitRefactor/gradle/wrapper/gradle-wrapper.properties similarity index 100% rename from Examples/KitbotDemoSim/gradle/wrapper/gradle-wrapper.properties rename to Examples/2.8_KitbotAKitRefactor/gradle/wrapper/gradle-wrapper.properties diff --git a/Examples/KitbotDemoSim/gradlew b/Examples/2.8_KitbotAKitRefactor/gradlew similarity index 100% rename from Examples/KitbotDemoSim/gradlew rename to Examples/2.8_KitbotAKitRefactor/gradlew diff --git a/Examples/KitbotDemoSim/gradlew.bat b/Examples/2.8_KitbotAKitRefactor/gradlew.bat similarity index 100% rename from Examples/KitbotDemoSim/gradlew.bat rename to Examples/2.8_KitbotAKitRefactor/gradlew.bat diff --git a/Examples/KitbotDemoSim/settings.gradle b/Examples/2.8_KitbotAKitRefactor/settings.gradle similarity index 100% rename from Examples/KitbotDemoSim/settings.gradle rename to Examples/2.8_KitbotAKitRefactor/settings.gradle diff --git a/Examples/KitbotDemoSim/src/main/deploy/example.txt b/Examples/2.8_KitbotAKitRefactor/src/main/deploy/example.txt similarity index 100% rename from Examples/KitbotDemoSim/src/main/deploy/example.txt rename to Examples/2.8_KitbotAKitRefactor/src/main/deploy/example.txt diff --git a/Examples/KitbotDemoSim/src/main/java/frc/robot/Main.java b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Main.java similarity index 100% rename from Examples/KitbotDemoSim/src/main/java/frc/robot/Main.java rename to Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Main.java diff --git a/Examples/KitbotDemoFinal/src/main/java/frc/robot/Robot.java b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Robot.java similarity index 62% rename from Examples/KitbotDemoFinal/src/main/java/frc/robot/Robot.java rename to Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Robot.java index d35c9d1..5e9557b 100644 --- a/Examples/KitbotDemoFinal/src/main/java/frc/robot/Robot.java +++ b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Robot.java @@ -4,24 +4,24 @@ package frc.robot; -import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGWriter; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Subsystems.Drivetrain.DrivetrainSubsystem; public class Robot extends LoggedRobot { - private Command m_autonomousCommand; - private RobotContainer m_robotContainer; + CommandXboxController controller = new CommandXboxController(0); - @Override - @SuppressWarnings("resource") - public void robotInit() { + DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); + + + public Robot() { Logger.recordMetadata("ProjectName", "KitbotExample"); // Set a metadata value if (isReal()) { @@ -31,11 +31,24 @@ public void robotInit() { } else { Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables } + + Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added. - Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may - // be added. + drivetrainSubsystem.setDefaultCommand( + drivetrainSubsystem.setVoltagesArcadeCommand( + () -> modifyJoystick(controller.getLeftY()), + () -> modifyJoystick(controller.getRightX()))); + } - m_robotContainer = new RobotContainer(); + private double modifyJoystick(double in) { + if (Math.abs(in) < 0.05) { + return 0; + } + return in * in * Math.signum(in); + } + + @Override + public void robotInit() { } @Override @@ -44,48 +57,33 @@ public void robotPeriodic() { } @Override - public void disabledInit() { - } + public void disabledInit() {} @Override - public void disabledPeriodic() { - } + public void disabledPeriodic() {} @Override - public void disabledExit() { - } + public void disabledExit() {} @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } } @Override - public void autonomousPeriodic() { - } + public void autonomousPeriodic() {} @Override - public void autonomousExit() { - } + public void autonomousExit() {} @Override public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } } @Override - public void teleopPeriodic() { - } + public void teleopPeriodic() {} @Override - public void teleopExit() { - } + public void teleopExit() {} @Override public void testInit() { @@ -93,10 +91,8 @@ public void testInit() { } @Override - public void testPeriodic() { - } + public void testPeriodic() {} @Override - public void testExit() { - } + public void testExit() {} } 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 new file mode 100644 index 0000000..fdfb2a2 --- /dev/null +++ b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIO.java @@ -0,0 +1,30 @@ +// 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 org.littletonrobotics.junction.AutoLog; + +public interface DrivetrainIO { + @AutoLog + public static class DrivetrainIOInputs { + public double leftOutputVolts = 0.0; + public double rightOutputVolts = 0.0; + + public double leftVelocityMetersPerSecond = 0.0; + public double rightVelocityMetersPerSecond = 0.0; + + public double leftPositionMeters = 0.0; + public double rightPositionMeters = 0.0; + + public double leftCurrentAmps = 0.0; + public double leftTempCelsius = 0.0; + public double rightCurrentAmps = 0.0; + public double rightTempCelsius = 0.0; + } + + public void updateInputs(DrivetrainIOInputs inputs); + + public void setVolts(double left, double right); +} 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 new file mode 100644 index 0000000..9f84186 --- /dev/null +++ b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOReal.java @@ -0,0 +1,94 @@ +// 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/KitbotDemoSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java similarity index 93% rename from Examples/KitbotDemoSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java rename to Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java index 69f548a..269ebd9 100644 --- a/Examples/KitbotDemoSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java +++ b/Examples/2.8_KitbotAKitRefactor/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainSubsystem.java @@ -16,7 +16,10 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class DrivetrainSubsystem extends SubsystemBase { - DrivetrainIO io = new DrivetrainIOSim(); + public static final int LEFT_TALON_ID = 0; + public static final int RIGHT_TALON_ID = 1; + + DrivetrainIO io = new DrivetrainIOReal(); DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0); diff --git a/Examples/KitbotDemoSim/vendordeps/AdvantageKit.json b/Examples/2.8_KitbotAKitRefactor/vendordeps/AdvantageKit.json similarity index 100% rename from Examples/KitbotDemoSim/vendordeps/AdvantageKit.json rename to Examples/2.8_KitbotAKitRefactor/vendordeps/AdvantageKit.json diff --git a/Examples/KitbotDemoSim/vendordeps/Phoenix6-frc2025-latest.json b/Examples/2.8_KitbotAKitRefactor/vendordeps/Phoenix6-frc2025-latest.json similarity index 100% rename from Examples/KitbotDemoSim/vendordeps/Phoenix6-frc2025-latest.json rename to Examples/2.8_KitbotAKitRefactor/vendordeps/Phoenix6-frc2025-latest.json diff --git a/Examples/KitbotDemoSim/vendordeps/WPILibNewCommands.json b/Examples/2.8_KitbotAKitRefactor/vendordeps/WPILibNewCommands.json similarity index 100% rename from Examples/KitbotDemoSim/vendordeps/WPILibNewCommands.json rename to Examples/2.8_KitbotAKitRefactor/vendordeps/WPILibNewCommands.json diff --git a/Examples/KitbotDemoBasic/src/main/java/frc/robot/Constants.java b/Examples/KitbotDemoBasic/src/main/java/frc/robot/Constants.java deleted file mode 100644 index 4fbca5d..0000000 --- a/Examples/KitbotDemoBasic/src/main/java/frc/robot/Constants.java +++ /dev/null @@ -1,11 +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; - -/** Add your docs here. */ -public class Constants { - public static final int drivetrainLeftFalconID = 0; - public static final int drivetrainRightFalconID = 1; -} diff --git a/Examples/KitbotDemoBasic/src/main/java/frc/robot/RobotContainer.java b/Examples/KitbotDemoBasic/src/main/java/frc/robot/RobotContainer.java deleted file mode 100644 index 65311c5..0000000 --- a/Examples/KitbotDemoBasic/src/main/java/frc/robot/RobotContainer.java +++ /dev/null @@ -1,38 +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; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Subsystems.DrivetrainSubsystem; - -public class RobotContainer { - CommandXboxController controller = new CommandXboxController(0); - - DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); - - public RobotContainer() { - configureBindings(); - } - - private void configureBindings() { - drivetrainSubsystem.setDefaultCommand( - drivetrainSubsystem.setVoltagesArcadeCommand( - () -> modifyJoystick(controller.getLeftY()), - () -> modifyJoystick(controller.getRightX()))); - } - - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } - - private double modifyJoystick(double in) { - if (Math.abs(in) < 0.05) { - return 0; - } - return in * in * Math.signum(in); - } -} diff --git a/Examples/KitbotDemoFinal/src/main/java/frc/robot/BuildConstants.java b/Examples/KitbotDemoFinal/src/main/java/frc/robot/BuildConstants.java deleted file mode 100644 index e9cf366..0000000 --- a/Examples/KitbotDemoFinal/src/main/java/frc/robot/BuildConstants.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot; - -/** - * Automatically generated file containing build version information. - */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "KitbotDemoFinal"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 202; - public static final String GIT_SHA = "2cc4b1e3ca5e56dc3af091243ce8814702adb871"; - public static final String GIT_DATE = "2025-07-25 17:20:42 PDT"; - public static final String GIT_BRANCH = "command-based-2024"; - public static final String BUILD_DATE = "2025-07-26 16:23:37 PDT"; - public static final long BUILD_UNIX_TIME = 1753572217766L; - public static final int DIRTY = 1; - - private BuildConstants(){} -} diff --git a/Examples/KitbotDemoFinal/src/main/java/frc/robot/RobotContainer.java b/Examples/KitbotDemoFinal/src/main/java/frc/robot/RobotContainer.java deleted file mode 100644 index b1c4448..0000000 --- a/Examples/KitbotDemoFinal/src/main/java/frc/robot/RobotContainer.java +++ /dev/null @@ -1,38 +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; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.subsystems.Drivetrain.DrivetrainSubsystem; - -public class RobotContainer { - DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); - - CommandXboxController controller = new CommandXboxController(0); - - public RobotContainer() { - configureBindings(); - } - - private void configureBindings() { - // Default Command - drivetrainSubsystem - .setDefaultCommand(drivetrainSubsystem.driveCommand( - () -> modifyJoystickAxis(controller.getLeftY()), - () -> modifyJoystickAxis(-controller.getLeftX()), - () -> false)); - } - - private double modifyJoystickAxis(double joystick) { - return -(Math.abs(Math.pow(joystick, 2)) + 0.05) - * Math.signum(joystick); - } - - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } -} diff --git a/Examples/KitbotDemoFinal/src/main/java/frc/robot/subsystems/Drivetrain/DrivetrainIO.java b/Examples/KitbotDemoFinal/src/main/java/frc/robot/subsystems/Drivetrain/DrivetrainIO.java deleted file mode 100644 index 52c855d..0000000 --- a/Examples/KitbotDemoFinal/src/main/java/frc/robot/subsystems/Drivetrain/DrivetrainIO.java +++ /dev/null @@ -1,36 +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 org.littletonrobotics.junction.AutoLog; - -/** Add your docs here. */ -public interface DrivetrainIO { - @AutoLog - public static class DrivetrainIOInputs { - // Speed and position for odometry - public double leftSpeedMetersPerSecond = 0.0; - public double leftPositionMeters = 0.0; - public double rightSpeedMetersPerSecond = 0.0; - public double rightPositionMeters = 0.0; - // Output logging, volts for open loop, rpm for closed loop - public double leftOutputVolts = 0.0; - public double leftGoalMetersPerSecond = 0.0; - public double rightOutputVolts = 0.0; - public double rightGoalMetersPerSecond = 0.0; - public boolean isClosedLoop = false; - // Current and temparature logging to monitor motor performance - public double[] leftCurrentAmps = new double[0]; - public double[] leftTempCelsius = new double[0]; - public double[] rightCurrentAmps = new double[0]; - public double[] rightTempCelsius = new double[0]; - } - - public abstract void updateInputs(DrivetrainIOInputs inputs); - - public abstract void setVolts(double left, double right); - - public abstract void setMetersPerSecond(double left, double right); -} diff --git a/Examples/KitbotDemoFinal/src/main/java/frc/robot/subsystems/Drivetrain/DrivetrainSubsystem.java b/Examples/KitbotDemoFinal/src/main/java/frc/robot/subsystems/Drivetrain/DrivetrainSubsystem.java deleted file mode 100644 index 141b219..0000000 --- a/Examples/KitbotDemoFinal/src/main/java/frc/robot/subsystems/Drivetrain/DrivetrainSubsystem.java +++ /dev/null @@ -1,65 +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 java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class DrivetrainSubsystem extends SubsystemBase { - // Odometry keeps track of our position on the field - // We can upgrade this to a pose estimator, which fuses vision readings to - // correct our estimate - DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0); - - // We could make this select an io type based off of if the robot was real or - // not - // Robot.isReal() ? new DrivetrainIOReal() : new DrivetrainIOSim() - DrivetrainIO io = new DrivetrainIOSim(); - DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); - - /** Creates a new Drivetrain. */ - public DrivetrainSubsystem() { - } - - // Method to set the driving speed and turning rate of the robot - public void drive(double speed, double angle, boolean isClosedLoop) { - var wheelSpeeds = DifferentialDrive.arcadeDriveIK(speed, angle, false); - if (isClosedLoop) { - io.setMetersPerSecond(wheelSpeeds.left, wheelSpeeds.right); - } else { - io.setVolts(wheelSpeeds.left * 12, wheelSpeeds.right * 12); - } - } - - // Command that wraps drive method - public Command driveCommand(DoubleSupplier speed, DoubleSupplier angle, BooleanSupplier isClosedLoop) { - return this.run(() -> drive(speed.getAsDouble(), angle.getAsDouble(), isClosedLoop.getAsBoolean())); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - io.updateInputs(inputs); - Logger.processInputs("Drivetrain", inputs); - - odometry.update( - // Here we have to do a little hack to get rotation to work in sim - // Normally we use a gyro to find rotation, but we don't have a simulated gyro set up now - odometry.getPoseMeters().getRotation() - // Use differential drive kinematics to find the rotation rate based on the wheel speeds and distance between wheels - .plus(Rotation2d.fromRadians((inputs.leftSpeedMetersPerSecond - inputs.rightSpeedMetersPerSecond) * 0.020 / Units.inchesToMeters(26))), - inputs.leftPositionMeters, inputs.rightPositionMeters); - Logger.recordOutput("Drivebase Pose", odometry.getPoseMeters()); - } -} diff --git a/Examples/KitbotDemoSim/src/main/java/frc/robot/Constants.java b/Examples/KitbotDemoSim/src/main/java/frc/robot/Constants.java deleted file mode 100644 index 4fbca5d..0000000 --- a/Examples/KitbotDemoSim/src/main/java/frc/robot/Constants.java +++ /dev/null @@ -1,11 +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; - -/** Add your docs here. */ -public class Constants { - public static final int drivetrainLeftFalconID = 0; - public static final int drivetrainRightFalconID = 1; -} diff --git a/Examples/KitbotDemoSim/src/main/java/frc/robot/RobotContainer.java b/Examples/KitbotDemoSim/src/main/java/frc/robot/RobotContainer.java deleted file mode 100644 index b8aadee..0000000 --- a/Examples/KitbotDemoSim/src/main/java/frc/robot/RobotContainer.java +++ /dev/null @@ -1,38 +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; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Subsystems.Drivetrain.DrivetrainSubsystem; - -public class RobotContainer { - CommandXboxController controller = new CommandXboxController(0); - - DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem(); - - public RobotContainer() { - configureBindings(); - } - - private void configureBindings() { - drivetrainSubsystem.setDefaultCommand( - drivetrainSubsystem.setVoltagesArcadeCommand( - () -> modifyJoystick(controller.getLeftY()), - () -> modifyJoystick(controller.getRightX()))); - } - - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } - - private double modifyJoystick(double in) { - // if (Math.abs(in) < 0.05) { - // return 0; - // } - return in * in * Math.signum(in); - } -} diff --git a/Examples/KitbotDemoSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOSim.java b/Examples/KitbotDemoSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOSim.java deleted file mode 100644 index 82888c4..0000000 --- a/Examples/KitbotDemoSim/src/main/java/frc/robot/Subsystems/Drivetrain/DrivetrainIOSim.java +++ /dev/null @@ -1,62 +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.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; - -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; -import frc.robot.Constants; - -public class DrivetrainIOSim implements DrivetrainIO { - TalonFX leftFalcon = new TalonFX(Constants.drivetrainLeftFalconID); - TalonFX rightFalcon = new TalonFX(Constants.drivetrainRightFalconID); - - VoltageOut leftVoltage = new VoltageOut(0); - VoltageOut rightVoltage = new VoltageOut(0); - - DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim( - KitbotMotor.kDoubleFalcon500PerSide, - KitbotGearing.k8p45, - KitbotWheelSize.kSixInch, - null); - - @Override - public void updateInputs(DrivetrainIOInputs inputs) { - physicsSim.update(0.020); - - var leftSimState = leftFalcon.getSimState(); - leftSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); - - var rightSimState = rightFalcon.getSimState(); - rightSimState.setSupplyVoltage(RoboRioSim.getVInVoltage()); - - 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 = new double[] {leftSimState.getTorqueCurrent()}; - inputs.leftTempCelsius = new double[0]; - inputs.rightCurrentAmps = new double[] {rightSimState.getTorqueCurrent()}; - inputs.rightTempCelsius = new double[0]; - } - - @Override - public void setVolts(double left, double right) { - leftFalcon.setControl(leftVoltage.withOutput(left)); - rightFalcon.setControl(rightVoltage.withOutput(right)); - } -} diff --git a/README.md b/README.md index 9816827..fb7c2a7 100644 --- a/README.md +++ b/README.md @@ -43,9 +43,9 @@ We encourage you to browse through these, and to ask a lead if you'd like to lea - [2.1 WPILib / Intro to FRC Programming](Docs/2_Architecture/2.1_WPILib.md) - [2.2 Electrical Intro](Docs/2_Architecture/2.2_ElectronicsCrashCourse.md) - [2.3-2.5 Command Based](Docs/2_Architecture/2.3_CommandBased.md) -- [2.6-2.7 AdvantageKit and Logging](Docs/2_Architecture/2.6_AdvantageKit.md) -- [2.8-2.9 Simulation](Docs/2_Architecture/2.8_Simulation.md) -- [2.10 Overall architecture(??)](Docs/2_Architecture/2.10_Superstructure.md) +- [2.6-2.8 AdvantageKit and Logging](Docs/2_Architecture/2.6_AdvantageKit.md) +- [2.9-2.10 Simulation](Docs/2_Architecture/2.9_Simulation.md) +- [2.11 Overall architecture(??)](Docs/2_Architecture/2.11_Superstructure.md) ### Controls