Simulations and Subsystem Design
Shaurya Sen and Benjamin Goldfisher
StuyPulse #1
Why Simulate?
StuyPulse #2
Why Simulate?
Simulations allow us to test robot code without having a physical robot.
StuyPulse #3
Research and Development
Over the last year we’ve begun to use simulations as part of our research, especially with swerve.
StuyPulse #4
Teaching
Each year 694’s Software department gets around 50 new members.
Giving each member a physical robot to learn and test with is impossible, so this year we created robot simulations for them to write code for and test with!
StuyPulse #5
Teaching
This year, we gave newbies a Romi simulation in which they could write code to control the drivetrain motors.
StuyPulse #6
Build season
During build season, time can be saved by testing if code will work on a simulated mechanism before the mechanism is even fully ready, speeding up the testing and development process.
StuyPulse #7
Making a Simulated Subsystem
StuyPulse #8
Drivetrain!
Today, we’re going to write a simulated tank drive.
We’ll start with some barebones code for a physical robot and see how it can be simulated!
StuyPulse #9
Our Physical Code
Here we have a basic drivetrain class that we can control with tankDrive()!
public class Drivetrain extends SubsystemBase {
private CANSparkMax[] left, right;
private DifferentialDrive drivetrain;
public Drivetrain() {
...
drivetrain = new DifferentialDrive(
new MotorControllerGroup(left),
new MotorControllerGroup(right)
);
}
public void tankDrive(double left, double right) {
drivetrain.tankDrive(left, right);
}
}
StuyPulse #10
Physical Hardware Control
Subsystem logic
Ex. PID
Encoders
Reads velocity from motors
Send velocity to subsystem
Set motor
to voltage
StuyPulse #11
Simulated Hardware Control
Subsystem logic
Ex. PID
SimulatedMotor shooterSim
Simulated shooter
Velocity read by subsystem
Voltage sent to simulation
StuyPulse #12
How Simulations Work
StuyPulse #13
Representing our Hardware
To simulate hardware:
StuyPulse #14
State Space Models
StuyPulse #15
State Space Models
StuyPulse #16
State Space Models
StuyPulse #17
State Space in Code
Simulation classes are based around State Space, and the main WPILib class that implements this is LinearSystemSim.
LinearSystemSim<States, Inputs, Outputs> represents a state space with a certain number of states, inputs, and outputs.
For a shooter, we would use a LinearSystemSim<N1, N1, N1> because it only has one state (velocity), one input (voltage), and one output (velocity).
StuyPulse #18
Simulation Code
StuyPulse #19
WPILib Sim Classes
WPILib has helpfully given us classes with state space models that can simulate a drivetrain.
DifferentialDrivetrainSim uses a state space model with the states: [x, y, rotation, left vel, right vel, left distance, right distance].
public class Drivetrain extends SubsystemBase {
// private CANSparkMax[] left, right;
// private DifferentialDrive drivetrain;
private DifferentialDrivetrainSim sim;
public Drivetrain() {}
public void tankDrive(double left, double right);
}
StuyPulse #20
Simulation Code
To integrate the simulation with the rest of the subsystem, we need to set the inputs (in volts) to the simulation.
We also need to update the simulation with a time step.
public class Drivetrain extends SubsystemBase {
private DifferentialDrivetrainSim sim;
public Drivetrain() {}
public void tankDrive(double left, double right) {
sim.setInputs(
left * RoboRioSim.getVInVoltage(),
right * RoboRioSim.getVInVoltage()
);
}
public void simulationPeriodic() {
sim.update(0.02);
}
}
StuyPulse #21
Displaying our Sim
public class Drivetrain extends SubsystemBase {
private DifferentialDrivetrainSim sim;
private Field2d field;
public Drivetrain() {
field = new Field2d();
SmartDashboard.putData(field);
}
public void tankDrive(double left, double right) {...}
public void periodic() {
field.setRobotPose(sim.getPose());
}
public void simulationPeriodic() {
sim.update(0.02);
}
}
Although our sim currently works, we can’t tell what’s happening.
WPILib’s Field2d can help display the position of a simulated drivetrain.
StuyPulse #22
Organizing Physical and Sim Code
StuyPulse #23
Physical & Sim
Right now, if we wanted to have both simulated and physical mechanisms at the same time in separate classes, we would have to replace which version we’re using every time in your commands.
We can fix this with inheritance!
StuyPulse #24
Simulation Interface
// IDrivetrain.java
public abstract class IDrivetrain extends SubsystemBase {
public abstract void tankDrive(double left, double right);
}
// DriveCommand.java
public class DriveCommand extends CommandBase {
private IDrivetrain drivetrain;
public void execute() {
drivetrain.tankDrive(...);
}
}
If we make an IDrivetrain class that the sim drivetrain and physical drivetrain both extend, commands can use it without needing to know if the robot is simulated.
StuyPulse #25
Implementing IDrivetrain
// SimDrivetrain.java
public class SimDrivetrain extends IDrivetrain {
private DifferentialDrivetrainSim sim;
@Override
public void tankDrive(double left, double right) {
drivetrain.tankDrive(left, right);
}
}
// Drivetrain.java
public class Drivetrain extends IDrivetrain {
private CANSparkMax[] left, right;
private DifferentialDrive drivetrain;
@Override
public void tankDrive(double left, double right) {
sim.setInputs(
RoboRioSim.getVInVoltage() * left,
RoboRioSim.getVInVoltage() * right
);
}
}
Although SimDrivetrain and Drivetrain have different internal workings, both implement tankDrive().
StuyPulse #26
More Sim Classes
StuyPulse #27
Elevator
public class SimElevator extends IElevator {
private ElevatorSim sim;
private Controller control;
private double targetHeight;
public SimElevator() {}
public void setTargetHeight(double height);
public void simulationPeriodic() {
sim.setInputVoltage(control.update(...));
sim.update(0.02);
}
}
Elevators are one of the only cases where a mechanism needs to simulate gravity.
StuyPulse #28
Displaying other Sims
// just Mechanism2d code
public class SimElevator extends IElevator {
private Mechanism2d mech;
private MechanismLigament2d elevatorLigament;
public SimElevator() {
mech = new Mechanism2d(2, 6);
MechanismRoot2d root = mech.getRoot(
"ElevatorRoot", 1, 0);
elevatorLigament = root.append(
new MechanismLigament2d("Elevator", 1, 90.0));
SmartDashboard.putData("Mech2d", mech);
}
public void periodic() {
// make the max height 5+0.5 and min height 0.5
elevatorLigament.setLength(
sim.getPositionMeters() * (5 / TOP) + 0.5);
}
}
Mechanism2d is another WPILib class that can display stick-figure representations of a robot mechanism.
StuyPulse #29
End
StuyPulse #30