Simulating Drive Train accurately and Limelight for Autonomous

I had some autonomous code but haven’t had the chance to try it out on our robot yet and was wondering if it would work. I saw RobotPy’s drivetrain physics simulation and was wondering if I could do something similar. I used simulated wheel speeds to keep track of left and right wheel cumulative distances to feed into odometry, and used DifferentialDriveKinematics to figure out rotation rate and then multiplied by a constant to slow it down (not sure how to simulate our robot’s weight). I also simulated the Limelight target x and y by doing some trig calculations. The funny thing is that the angles are still properly reported even if the Limelight wouldn’t normally be able to see the target. It works so much better than I expected!

Drive Train simulation code: https://github.com/FRC-4277/2020InfiniteRecharge/blob/simulate-autonomous/src/main/java/frc/robot/subsystems/DriveTrain.java#L314-L352
Limelight simulation code: https://github.com/FRC-4277/2020InfiniteRecharge/blob/simulate-autonomous/src/main/java/frc/robot/util/limelight/Limelight.java#L57-L109

Two autonomous commands simulated:

I was wondering if anyone had any suggestion on how to properly simulate the drive train, considering robot weight.

6 Likes

The first thing I would try to do is have the simulated wheel speeds not jump around. Basically, the minimum time it takes for each wheel to get to its maximum speed shouldn’t be 0 seconds (like it is right now). So if you’re at 0% speed and you want to go to 50% speed, let’s say that might take .5 seconds. This would mean you would basically have an acceleration of 100% per second: It takes you 1 second to get to 100% speed.

This year with our custom simulation, we used the Box2D physics library with libGDX. Although we used swerve, it can also be applied to tank: Define where each wheel is and constrain each wheel to the robot’s main body. Periodically update the velocity and angle of each wheel (for tank it would only be velocity). The box2d library took care of realistically moving and rotating the body.

I’m sure there’s a reasonable was to do this with hopefully simple math, but a full on physics engine would take some of that work away and might even give you more options.

Setup part of our code
Setting the velocity of a single wheel
Acceleration logic
Sorry that the simulation part of our code is in Kotlin, hopefully you can understand it

I’m not sure how well box2d works without using libGDX, but it might be worth trying. However, there is a learning curve for it.

I’m guessing that more people will want to try what you’re doing next year as the Field2d thing was added in the middle of the season. I’d be interested to see if someone will eventually make a PR with an example for a simulated drivetrain. I might try and contribute to the WPI simulation stuff, I’m not sure yet.

2 Likes

Interesting! I’ll look into using Box2D. Seems like it would be somewhat overkill but an amazing solution.

The tankmodel uses the robot characterization constants, it feels more real.

1 Like

You shouldn’t need to slow down the calculated turn rate by a constant determined by your robot’s weight. The reason it turns slower when your robot is heavier is actually that it takes longer for the wheels to accelerate, so thus it takes longer to get it to turn. If you’re interested in the math and physics behind it, I recommend reading section 14.5 of the FRC Controls Engineering book. The result is equations 14.29 and .30, which show the acceleration of a wheel depends on the speeds of both wheels and voltages applied to both sides (to my knowledge, drive train characterization is a good approximation of this when the magnitude of both voltages are approximately equal), and both the robot’s weight and moment of inertia determine the exact coefficients.

Speaking of overkill, I ran some linear regressions on the raw characterization data .json files to determine the coefficients in 14.29 and .30 and then created a MATLAB simulator using their ODE solvers and its been giving us pretty nice results. I’m honestly not sure how advantageous it is to use this overly complicated method to simulate drive train behavior though, characterization + odometry is probably good enough! But either way definitely use empirically derived values and an accurate physics model, or else as said above you’ll get motors that instantly accelerate which sadly don’t exist : (

2 Likes