Skip to content

Commit 4829ca5

Browse files
spellingcatspellingcat
authored
2025 akit kitbot walkthru (#91)
2 parents 9d7f82a + 5127ef9 commit 4829ca5

78 files changed

Lines changed: 1246 additions & 1061 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎Docs/2_Architecture/2.10_KitbotSim.md‎

Lines changed: 340 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,340 @@
1+
# Kitbot Sim Tutorial
2+
3+
## Intro
4+
5+
Writing code is plenty of fun on its own, but its rough having to wait for mechanical to be done to test it.
6+
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.
7+
Luckily, we have a handy piece of code that to refactor to be simulateable!
8+
In this tutorial we are going to rework your kitbot code to be easy to simulate and expand on.
9+
In the end your code should be able to do this:
10+
11+
[Video of robot in sim](https://drive.google.com/file/d/1AD0OLK8Lt1dHbn5YYZ8rYc_6_0wbLPJf/view?usp=sharing)
12+
13+
## Code Walkthrough
14+
15+
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).
16+
This is intentional!
17+
We want our simulated behavior to be as similar to real-life behavior as possible, otherwise our simulation would not be very valuable.
18+
For the most part, writing this will consist of substituting stuff like simulated motors for real motors.
19+
20+
Let's add another IO Implementation.
21+
Like the real IO class, this file will define the `updateInputs()` and `setVolts()` methods, but implement them differently (through sim).
22+
Since we aren't writing this for real hardware, we are going to make `DrivetrainIOSim`.
23+
24+
Make a new `DrivetrainIOSim` class that implements `DrivetrainIO` the [same way](2.8_KitbotAKitRefactor.md#drivetrainioreal) you did for `DrivetrainIOReal`.
25+
26+
```Java
27+
public class DrivetrainIOSim implements DrivetrainIO {
28+
29+
@Override
30+
public void updateInputs(DrivetrainIOInputs inputs) {
31+
// TODO Auto-generated method stub
32+
33+
}
34+
35+
@Override
36+
public void setVolts(double left, double right) {
37+
// TODO Auto-generated method stub
38+
39+
}
40+
}
41+
```
42+
43+
Some of these fields we can get directly from a simulated motor.
44+
TalonFX comes with built-in simulation support, so we can treat it like a regular motor for the most part.
45+
Add them the same way you did for `DrivetrainIOReal`.
46+
47+
```Java
48+
TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID);
49+
TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID);
50+
```
51+
52+
To get the simulated outputs we can call `talon.getSimState()` and store the results in a variable.
53+
Do this in `updateInputs`.
54+
55+
```Java
56+
@Override
57+
public void updateInputs(DrivetrainIOInputs inputs) {
58+
var leftSimState = leftTalon.getSimState();
59+
var rightSimState = rightTalon.getSimState();
60+
// Snip
61+
}
62+
```
63+
64+
Now you can call `simState.getMotorVoltage()` to get the voltage for each motor.
65+
Do this for the `left-` and `rightOutputVolts`.
66+
67+
```Java
68+
@Override
69+
public void updateInputs(DrivetrainIOInputs inputs) {
70+
// Snip
71+
inputs.leftOutputVolts = leftSimState.getMotorVoltage();
72+
inputs.rightOutputVolts = rightSimState.getMotorVoltage();
73+
// Snip
74+
}
75+
```
76+
77+
We can also get the current from each of the motor's sim states.
78+
There are two types of current we can measure: `TorqueCurrent` and `SupplyCurrent`.
79+
`SupplyCurrent` is like the available current for the motor, and is what the breaker measures.
80+
`TorqueCurrent` is the amount of current the motor is actually using, and is important for heat management.
81+
`TorqueCurrent` is more useful for our purposes since it shows the actual current usage of the motor.
82+
83+
```Java
84+
@Override
85+
public void updateInputs(DrivetrainIOInputs inputs) {
86+
// Snip
87+
inputs.leftCurrentAmps = leftSimState.getTorqueCurrent();
88+
inputs.leftTempCelsius = 0.0;
89+
inputs.rightCurrentAmps = rightSimState.getTorqueCurrent();
90+
inputs.rightTempCelsius = 0.0;
91+
}
92+
```
93+
94+
This information is useless without having a way to set the motor voltage.
95+
Copy the `VoltageOut` objects from `DrivetrainIOReal` below the `TalonFX`s.
96+
97+
```Java
98+
TalonFX leftTalon = new TalonFX(DrivetrainSubsystem.LEFT_TALON_ID);
99+
TalonFX rightTalon = new TalonFX(DrivetrainSubsystem.RIGHT_TALON_ID);
100+
101+
VoltageOut leftVoltage = new VoltageOut(0);
102+
VoltageOut rightVoltage = new VoltageOut(0);
103+
```
104+
105+
Then copy the `setVolts` method from `DrivetrainIOReal`.
106+
107+
```Java
108+
@Override
109+
public void setVolts(double left, double right) {
110+
leftTalon.setControl(leftVoltage.withOutput(left));
111+
rightTalon.setControl(rightVoltage.withOutput(right));
112+
}
113+
```
114+
115+
Finally, we need to add a physics sim.
116+
This will take these motor voltages and figure out how the mechanisms might actually behave using a mathematical model.
117+
WPILib provides the `DifferentialDrivetrainSim` class for this.
118+
This class has the `createKitbotSim()` static method to provide a convenient way to set this up with the physical constants for a kitbot.
119+
120+
Start by creating a new `DifferentialDrivetrainSim` called `physicsSim` under the `VoltageOut`s.
121+
122+
```Java
123+
DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim(
124+
null,
125+
null,
126+
null,
127+
null);
128+
```
129+
130+
The first value in this constructor is the motor for the sim.
131+
Let's assume we're using Falcons and use `KitbotMotor.kDoubleFalcon500PerSide` to generate the constants for this.
132+
This model assumes we have 2 motors on each side of the drivetrain, although we only programmed one.
133+
For a sim this doesn't matter, but for a real robot we would need to add follower motors.
134+
We are going to ignore that for this tutorial.
135+
136+
```Java
137+
DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim(
138+
KitbotMotor.kDoubleFalcon500PerSide,
139+
null,
140+
null,
141+
null);
142+
```
143+
144+
The second parameter is the gearing of the drivetrain.
145+
This is something we normally would ask mechanical for.
146+
In this case, since the drivetrain is simulated, we can use the standard 8p45 gearbox.
147+
148+
```Java
149+
DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim(
150+
KitbotMotor.kDoubleFalcon500PerSide,
151+
KitbotGearing.k8p45,
152+
null,
153+
null);
154+
```
155+
156+
The third parameter is the size of the wheels.
157+
By default the kitbot comes with 6 inch wheels.
158+
159+
```Java
160+
DifferentialDrivetrainSim physicsSim = DifferentialDrivetrainSim.createKitbotSim(
161+
KitbotMotor.kDoubleFalcon500PerSide,
162+
KitbotGearing.k8p45,
163+
KitbotWheelSize.kSixInch,
164+
null);
165+
```
166+
167+
The final parameter is the "measurement standard deviations".
168+
This is a way for us to add uncertainty to our simulated model in a way that matches real sensor noise.
169+
For this exercise we can ignore it and leave it as null.
170+
171+
Now we have a fully set up physics sim.
172+
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.
173+
Make sure to work with mechanical to find these constants in season.
174+
175+
Next we need to actually use the physics sim.
176+
In `updateInputs` add a call to `physicsSim.update()`.
177+
178+
```Java
179+
@Override
180+
public void updateInputs(DrivetrainIOInputs inputs) {
181+
physicsSim.update(0.020);
182+
183+
var leftSimState = leftTalon.getSimState();
184+
var rightSimState = rightTalon.getSimState();
185+
// Snip
186+
}
187+
```
188+
189+
This method runs the simulation for the number of seconds passed in.
190+
We use 20 milliseconds, which is the default loop time of the rio.
191+
192+
Next we need up update the inputs to the sim based on the motor voltages.
193+
Call `physicsSim.setInputs()` with the motor sim state voltages.
194+
195+
```Java
196+
@Override
197+
public void updateInputs(DrivetrainIOInputs inputs) {
198+
physicsSim.update(0.020);
199+
200+
var leftSimState = leftTalon.getSimState();
201+
var rightSimState = rightTalon.getSimState();
202+
203+
physicsSim.setInputs(leftSimState.getMotorVoltage(), rightSimState.getMotorVoltage());
204+
// Snip
205+
}
206+
```
207+
208+
We're almost done with `DrivetrainIOSim`!
209+
Now we just need to update the rest of the IOInputs based on the simulator.
210+
Start by making the wheel speeds update with the `physicsSim.getLeftVelocityMetersPerSecond`.
211+
Then update the wheel positions with `physicsSim.getLeftPositionMeters`.
212+
213+
```Java
214+
@Override
215+
public void updateInputs(DrivetrainIOInputs inputs) {
216+
// Snip
217+
inputs.leftVelocityMetersPerSecond = physicsSim.getLeftVelocityMetersPerSecond();
218+
inputs.rightVelocityMetersPerSecond = physicsSim.getRightVelocityMetersPerSecond();
219+
220+
inputs.leftPositionMeters = physicsSim.getLeftPositionMeters();
221+
inputs.rightPositionMeters = physicsSim.getRightPositionMeters();
222+
// Snip
223+
}
224+
```
225+
226+
Theres one more thing we should do to help make this sim realistic.
227+
When our motors are running the voltage of the battery will "sag" or drop.
228+
This can reduce our motor's output in the extreme case.
229+
To simulate this use `simState.setSupplyVoltage(RoboRioSim.getVInVoltage())`.
230+
`setSupplyVoltage` will control how much voltage is available to the motor simulation.
231+
`RoboRioSim.getVInVoltage()` will record how much battery our motors should be using, and the effect that will have on the battery.
232+
233+
```Java
234+
@Override
235+
public void updateInputs(DrivetrainIOInputs inputs) {
236+
physicsSim.update(0.020);
237+
238+
var leftSimState = leftTalon.getSimState();
239+
leftSimState.setSupplyVoltage(RoboRioSim.getVInVoltage());
240+
241+
var rightSimState = rightTalon.getSimState();
242+
rightSimState.setSupplyVoltage(RoboRioSim.getVInVoltage());
243+
// Snip
244+
}
245+
```
246+
247+
Now we're done with the `DrivetrainIOSim` class! 🎉
248+
249+
Let's add a way to switch between using a `DrivetrainIOReal` or a `DrivetrainIOSim` depending on if the robot is real or simulated.
250+
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.
251+
Replace the `io` assignment with the following line:
252+
253+
```Java
254+
public class DrivetrainSubsystem extends SubsystemBase {
255+
DrivetrainIO io = Robot.isReal() ? new DrivetrainIOReal() : new DrivetrainIOSim();
256+
}
257+
```
258+
This will now select the correct io object to create, and will now work in sim.
259+
260+
But it won't have any way to track the position of the drivebase.
261+
Remember that we can use odometry to track the position of a drivebase.
262+
We can create a new `DifferentialDriveOdometry` object in `DrivetrainSubsystem` to handle this.
263+
264+
```Java
265+
public class DrivetrainSubsystem extends SubsystemBase {
266+
DrivetrainIO io = Robot.isReal() ? new DrivetrainIOReal() : new DrivetrainIOSim();
267+
DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged();
268+
269+
DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0);
270+
// Snip
271+
}
272+
```
273+
274+
We will need to update this every processor loop.
275+
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.
276+
However, in simulation we don't have an easy way to make a gyro that behaves how we would expect.
277+
Instead, we can use the following line to fudge it:
278+
279+
```Java
280+
odometry.getPoseMeters().getRotation()
281+
.plus(
282+
Rotation2d.fromRadians(
283+
(inputs.leftSpeedMetersPerSecond - inputs.rightSpeedMetersPerSecond)
284+
* 0.020 / Units.inchesToMeters(26)))
285+
```
286+
287+
This line is using a small piece of math to calculate what the new rotation should be based off of our wheel speeds.
288+
You don't need to know how this works in particular, just know it's a bit of a hack.
289+
290+
We are going to wrap this in an `odometry.update()` call.
291+
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.
292+
293+
```Java
294+
@Override
295+
public void periodic() {
296+
io.updateInputs(inputs);
297+
Logger.processInputs("Drivetrain", inputs);
298+
299+
300+
if (Robot.isSimulation) {
301+
odometry.update(
302+
odometry.getPoseMeters().getRotation()
303+
// Use differential drive kinematics to find the rotation rate based on the wheel speeds and distance between wheels
304+
.plus(Rotation2d.fromRadians((inputs.leftVelocityMetersPerSecond - inputs.rightVelocityMetersPerSecond)
305+
* 0.020 / Units.inchesToMeters(26))),
306+
inputs.leftPositionMeters, inputs.rightPositionMeters);
307+
}
308+
}
309+
```
310+
311+
Finally, we need to send the odometry pose to the logger so we can visualize it.
312+
We do this using `Logger.recordOutput()` and `odometry.getPoseMeters()`.
313+
314+
```Java
315+
@Override
316+
public void periodic() {
317+
// Snip
318+
Logger.recordOutput("Drivetrain Pose", odometry.getPoseMeters());
319+
}
320+
```
321+
322+
This should be everything you need to have a full drivebase simulation!
323+
324+
### Running the Simulation
325+
326+
To run a robot code simulation, click on the WPILib icon in the top right corner.
327+
Then find or search for "Simulate Robot Code" and press it.
328+
Once the code builds, it will prompt you to choose between using the driver station and sim gui.
329+
Choose the sim gui.
330+
331+
The sim gui is perfectly serviceable for testing, but we can do better.
332+
AdvantageScope supports connecting to a simulation to visualize IO.
333+
When you open AdvantageScope, hit "file", then "Connect to Simulator".
334+
This lets you visualize data from the simulator in the same way as you would with a real robot.
335+
You can use the 3d field visualization to have a fun way to show the robot on the field.
336+
337+
To drive the robot, add "keyboard 0" to "joystick 0" in the sim gui.
338+
You might have to change your keyboard settings in the sim gui to make it properly emulate a joystick.
339+
Then make sure to set the mode to teleop.
340+
You can also check your setup against a teammate or lead's.

Docs/2_Architecture/2.10_Superstructure.md renamed to Docs/2_Architecture/2.11_Superstructure.md

File renamed without changes.

‎Docs/2_Architecture/2.1_WPILib.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ Many of these libraries will have their own articles here as well as their own d
2121

2222
### Must read/watch
2323

24-
- Read through our [electrical reference sheet](../General/ElectronicsCrashCourse.md)
24+
- Read through our [electrical reference sheet](2.2_ElectronicsCrashCourse.md)
2525
- In an ideal world, we never have to touch the electrical system.
2626
In reality, often we end up finding problems with electrical early in the season.
2727
Therefore it is important to know what we are working with to debug and fix the robot.

‎Docs/2_Architecture/2.2_ElectronicsCrashCourse.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -280,6 +280,7 @@ Instead, we use "current zeroing".
280280
This involves slowly powering a mechanism into a hardstop with a known position (like an elevator being all the way down).
281281
When the current draw of the motor spikes, we know that the motor is "stalling", or using power but not moving.
282282
This tells us that we are at the hardstop in the same way that a limit switch would.
283+
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.
283284

284285
### IMU
285286

‎Docs/2_Architecture/2.3_CommandBased.md‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ For example, we might want something to happen while a condition is true (`while
3434
Some large Commands are better represented by several Commands and some Triggers!
3535

3636
Subsystems, commands, and triggers are the building blocks of the robot's overall "superstructure".
37-
This will be covered in more detail in the [Superstructure article.](2.10_Superstructure.md)
37+
This will be covered in more detail in the [Superstructure article.](2.11_Superstructure.md)
3838

3939
### Resources
4040

@@ -52,7 +52,7 @@ This will be covered in more detail in the [Superstructure article.](2.10_Supers
5252

5353
### Exercises
5454

55-
- Make basic KitBot code using the Command-Based skeleton. You can follow [this](2.5_KitbotExampleWalkthrough.md) tutorial.
55+
- Make basic KitBot code using the Command-Based skeleton. You can follow [this](2.5_KitbotIntro.md) tutorial.
5656

5757
### Notes
5858

0 commit comments

Comments
 (0)