Simulating a differential drivetrain, part 1 - If it's stupid but it works...

There’s a lot going on in a drivetrain.

On the surface, they seem pretty simple: “Rotary power comes from somewhere. It’s moved through mechanical links. The links connect to wheels. Wheels make thing go.” But there’s a lot of subtle complexity that underpins all of that. Drivetrains can actually be one of the more complicated things to simulated on a robot.

In a FIRST competition, every robot has some method of getting around, so it’s worth the time to invest in learning how to simulat them and what is involved in simulating them. Over the next few weeks of posts, we’re going to dig deep on this topic. I’m going to start from a simple simulation, then move onto a more complicated one, and then finally onto one using built-in WPILib classes that tries to capture many, many details of the drivetrain. Along the way, we’ll compare the approaches and the results we get and see what the tradeoffs are.

Full disclosure: I’m as exploring as I am explaining here, dear reader. At the moment, I have only implemented one of these approaches before, so I don’t really know what we’ll discover. It’ll be exciting to see where we end up.

Before we start in with our first model, let’s talk a bit about what a differential drive is and what we’ll be comparing.

What’s a differential drive?

A differential drive (“diff drive”) is a drivetrain where the motors and wheels are divided into two independently controlled sets: left and right. By varying the power to each half, you can get the robot to move forward and backward or rotate (generally, for convenience, we want to build the robot so that point is in the middle of the chassis when the left and right trains are driven with equal but opposite inputs).

Since there’s no way for a diff drive to “sideslip”, we say diff drive is a non-holonomic system (which is a fancy way of saying there are paths through space an air-hockey puck could take but the robot can’t; it can’t move in an arbitrary direction while continuously spinning). But since the robot can turn on a dime, it should still be able to get everywhere (as opposed to a robot using car-style steering, which has some parking spots it can’t reach).

The simplest model of a differential drive is just two big wheels controlled by two motors, so that’s what we’re going to use. We’ll assume our wheels have a diameter of 4" (so about 0.1016 meters) so the robot goes about π * 0.1016 ≈ 0.3192 meters per revolution if both wheels are driven at the same speed.

The comparison

For each simulation, we’ll compare the following:

• Driving straight for three rotations (about one meter total, more precisely 95.76cm)
• Driving in a circle for three rotations (one motor full forward, one full back). Assuming a track width (distance between wheels) of 50 centimeters (about 19.6 inches), three wheel rotations are a distance along an arc of 31.92 centimeters in a circle with diameter 50 centimeters. Since the circumference of a circle is π * diameter, we can find the angle by

    length of arc / total circumference
  = 31.92 / π * 50
  ≈ 31.92 / 157.07
  ≈ 20.32% of the circle
  

  … which is about (2π * 20%) radians = 2/5π radians = 72 degrees.
• driving in a lazy arc (one motor full power, one motor half power) for three rotations. This one, I’m not going to lay out the math to predict what will happen yet (future post); we’ll just see what it does.

We’ll consider for each kind of simulator

• What results it gives us for our inputs
• How we set it up
• Velocity curves, acceleration curves, and final positions

And we’ll discuss some pros and cons.

Having established what we’re doing here, let’s start off with a very simple model.

The simple interpolator

So at first glance, the question of simulating a diff drive doesn’t feel too complicated. We know that

• If both motors go full-forward, the robot goes max-speed forward
• If both motors go full-backward, the robot goes max-speed backward
• If one motor is full forward and one full backward, the robot turns on a dime at max speed

The interpolation simulator just assumes that any motion the robot could be doing between these extremes is a simple linear interpolation of these motions. We establish a top speed for the wheel based on motor input (something simple, like “The robot goes at most 1 meter / second forward and rotates twice a second at max speed”). Then we split the motion in question into linear motion and turning motion.

• To figure out linear, we take the average of the sum of the left and right motor power and multiply that by top linear speed
• To figure out rotation, we take the average of the difference of the left and right motor (i.e. (left - right) / 2)) and multiply that by the top rotational speed

Then we update the robot’s speed and then update the robot’s position and heading based on that speed. Updating the position involves changing simulated encoder values, and updating the rotation involves changing a simulated gyro value.

There’s an implementation of this approach split between PoseEstimator.java and SimpleSimulatedChassis.java in this simulator demo.

Comparison and results

The end result feels okay; when hand-controlled; it’s not obviously broken. Here’s what it looks like on the tests of motion:

Note: one spot of trouble I got into testing this: you can change the refresh rate for graphs in Shuffleboard. If you set it too high, Shuffleboard becomes unresponseive and cannot edit the preference again. The fix is to delete the prefs from Java’s user prefs store (in my case, it’s stored in the system prefs in ~/.java/userPrefs/edu/api/first/shuffleboard/plugin/base/prefs.xml.

Linear motion

The “Drive” command (which causes the robot to drive forward until its encoder passes three rotations) causes the robot to drive forward and then stop; no problem. It gets pretty close to spot-on for the final value; the overage is likely due to the fact that the simulation happens in steps of 0.02 seconds; if the robot passes the target encoder rotation in one of those time steps, it can’t stop on a dime with simple on-off control and so stops a bit after the target.

The “Turn” command (which attempts to cause the robot to turn precisely 72 degrees) is similarly accurate. It also overshoots because turning can’t stop in the middle of a time step.

Worth noting is the acceleration and velocity result. Because the robot accelerates instantaneously, they are very spiky. This is unrealistic motion; real robots don’t immediately stair-step from zero velocity to some high velocity; they have to overcome inertia and static friction (and similarly, once moving, inertia will keep them moving and motors have to fight that to stop the machine). So our simulation isn’t particularly realistic.

Finally, we take a look at our lazy arc, which drives with the right motor full power and the left half power until we pass 72 degrees. The robot does trace a little arc, and we find our final position to be about a quarter-meter out in the x and y position with a heading of 76 degrees, closer to the 72-degree target than in raw turn (this makes sense; the rate of turn is lower on the lazy-turn mode).

Analysis

While this is good enough for practicing, it does not reflect how an actual robot behaves. For one thing, the motors cut on and off immediately, which doesn’t really reflect how motors work in the real world (see the discussion of the flywheel for details). For another, the robot’s path through space looks like a bunch of small line segments, since the facing is only allowed to update between steps and the robot always moves straight along its current facing. If the steps are small enough, this might be fine, but it doesn’t reflect well the curves that the robot actually travels in space.

Next week’s simulation approach will try to follow those curves.

Simulating a Flywheel part 1: overview and simulating a motor

Well, the off-season of FIRST Robotics is upon us. A time when young people’s minds turn away from robots and towards other things. Like simulating robots!

To give the team something to play with on the software side, I’m putting together a couple of simulators of robot subsystems to experiment with. The first one is a simple flywheel connected to one motor and encoder.

Interface for the simulator, showing the graph of flywheel RPM over time

I’m going to break the discussion of how I did this into two parts. Here, I’ll go over the top-level architecture of the program and how it tries to simulate, and talk about the equations to simulate a DC electric motor. In a follow-up post, I’ll go over the physics of friction simulation.

Full disclosure: I’m a rank amateur at this, putting together half-remembered high school physics, a barely-used robotics minor, and some hazy memories of graphs from my first years playing FIRST Robotics. Definitely room for improvement, and I welcome feedback on this.

You can get the simulator from the GitHub repsitory.

Overview

Program structure

The program is a simple TimedRobot operating at the default frequency of 50 times a second. It includes one automatically-running Flywheel subsystem that lets a motor be turned on and off (at max power or 0 power), which is controlled by a single joystick button.

In addition to Flywheel, a SimulatedFlywheel class is constructed if the robot is operating in simulation mode. This class takes in the motor from the Flywheel and a simulation wrapper around the encoder, allowing us to explicitly set the encoder’s position and velocity values.

The simulation

At a high level of abstraction, the torque on a motor-driven flywheel at any moment is simple:

Opposed forces acting on a spinning flywheel

Once we have calculated this torque, we can update the flywheel’s angular velocity by adding total torque / moment of inertia to the flywheel’s previous velocity. Then we update the flywheel’s position as well (accounting, in both cases, for the step of time over which this change is occurring, in our case 20ms between updates).

That’s the high-level approach; let’s drill down on the details.

Simulating motors

A DC motor is a relatively simple system, but it has a bit of complexity making simulating one interesting.

The basic principle by which a DC motor operates is that half of the motor uses electromagnets, while the other half uses permanent magnets. Varying the current through the motor’s electromagnets creates moving magnetic fields that the permanent magnets then “chase,” turning electricity into motion. The highest torque such a motor can give is when it’s “stalled” (i.e. not moving) and the voltage across it is maximized. At this configuration, the motor is pushing with its “stall torque” (note: It’s generally a bad idea to leave a motor stalled like this; many DC motors aren’t designed to run current through with no motion, and doing so for long periods of time can heat up and damage components).

So the total torque a motor can give is equal to some constant times the current through the motor:

Where I is current through the motor and k_M is a torque constant. Note: equations for motor behavior are mostly sourced from here. We can quickly calculate k_M as just the stall torque divided by the current, but for the purposes of the simulation i find it easier to just consider a relationship between stall torque and voltage by subsituting current through V = IR, then considering the motor with 12 volts across it and finding the value of a constant q which is resistance divided by K_M:

What’s interesting about electric motors is that when I say “some loops of wire acting as electromagnets near some permanent magnets,” I’m also describing a type of generator. And indeed, a spinning electric motor tries to induce a current in the direction opposite the spin! This opposing force, known as “back-EMF” (EMF: electromotive force), creates a counter-voltage to the voltage across the motor, resulting in a drop in current through the circuit. The physics of why this happens can be a little confusing, but there’s (I think) an elegant way to remember it: the fact that motors have back-EMF sort of falls naturally out of conservation of energy once we remember that motors with a voltage across them don’t spin up to infinite speed. Since a motor spinning at top speed only needs a little bit of energy to overcome resistance and maintain that speed, and since energy lost across a component of a circuit is the power (P=IV), we know that when the motor is spinning near top speed, P across the motor must be small to keep it spinning, so current and/or voltage diminished.

An electric motor and electric generator differ mostly in where the energy comes from. (Images sourced from Wikimedia)

The back-EMF on the motor is proportional to the motor’s rotational speed and some back-EMF constant k_e.

A motor allowed to spin up to full speed under no load and with maximum voltage across it will spin at a “free speed” which varies from motor to motor. At free speed, the back-EMF is approximately equal to the forward voltage, so we can set the voltage and speed in the above equation and re-arrange to solve for the back-EMF constant:

We’re closing in on simulating the torque. The remaining ingredient is the full torque equation.

To approach the final torque equation, we start by considering that the total voltage across the motor is the V=IR relationship through the motor plus the back-EMF value.

Substituting in the relationship between torque and current, we find

Now we have voltage, motor speed, and motor torque related. Doing the algebra to make torque the dependent variable and substituting our q constant for resistance divided by k_M, we get:

That gives us the motor torque in almost all circumstances, save one: what happens when the voltage is set to zero?

Brake or coast?

It’s a subtle bit of architecture, but an important one: as we’ve established, a motor acts like a generator. So what happens when the motor should be applying no power?

Motor controllers used in the FIRST Robotics competition are generally configurable to “brake” or “coast” mode. In “brake” mode, the controller short-circuits the motor to itself, allowing the back-EMF to try and drive the motor in reverse. This will resist motion in the motor and drag it to a halt much quicker. The mathematics of this is, honestly, something I don’t know.

“Coast” mode is much easier to model: in “coast” mode, the controller opens the circuit. With no ability for electricity to flow from one end of the motor to the other, the motor basically contributes no torque and the flywheel will spin down normally. So we special-case the zero-voltage condition to eliminate the motor torque, simulating “coast mode.”

Next steps

This gives us half the flywheel simulation. Next time, we’ll talk about the friction counter-torque (and how annoying friction subtleties are!) and tie it all together with the acceleration and velocity-update logic.

Another Year of FIRST Robotics

The FIRST Robotics regional has wrapped up in Pittsburgh, PA. I didn't really have time to participate as a mentor or volunteer this year (although I got a chance to present for my company as a sponsor, which was really nice). But I did get a chance to watch a couple of rounds. One I caught really summed up why I love FIRST.

Tem 3955 (blue, center) built a robot that could climb the pyramid rung-by-rung by repeatedly extending a two-claw gripping arm and then hooking a lower grip onto the bar below, a bit like a rock climber. The machine was a beautiful thing to watch.

But in this match, they slipped; only one hook "bit" on the highest bar of the pyramid and they were left hanging. In that position, they're worth 20 points at the end of the match, but they'd be worth 10 points more if they could get the lower chassis above the middle bar. You can hear the crowd shouting "Pull yourself up!" But if they do and the chassis bumps the middle bar, they could tumble right off and lose all 20 points (and possibly damage their robot or a teammate's robot on impact).

Scoring for the climbing pyramid. Source: http://frc-manual.usfirst.org/viewItem/3#3

Rather than risk what they already had, they decided to hold on. And in the end, it proved to be the right choice; they won the round 69-55; the twenty points they earned were instrumental to their team's victory.

This is a machine that these students built themselves in six weeks, from scratch. But it's not about the machine; it's never really about the machine. It's about the moment-by-moment decisions made by the students on and off the field. I can imagine the pressure the three-student driving team felt trying to decide how to handle the situation; at the end of the day, the moment was in their hands alone, and it's their choice that will decide whether they win or lose.

Teamwork. Quick thinking. Focus. Strategy. And also control. Knowing how to do as much as you possibly can... And no more.

And no less.

This is why I keep coming back to FIRST.