Previous Lesson

By the end of this lesson you should be able to:

If you were able to finish the homework on lesson 3 and feel pretty confident in how ya did it, feel free to skip this lesson. This is just going over

**Lesson Purpose:**By the end of this lesson you should be able to:

- Send our rover Geometry/Twist messages
- Have it steer and drive according to those messages

If you were able to finish the homework on lesson 3 and feel pretty confident in how ya did it, feel free to skip this lesson. This is just going over

*a*(not*the*) solution.## Lesson 4: Programming our Rover

That homework in lesson 3 was one hefty assignment. It's a pretty big jump from copy-pasting code to "now go make this work".

So, here's how I did it in a sorta jerry-rigged way.

You can download the project off of my Github here

(https://github.com/wrbernardoni/robot_tutorial_2 if ya want to git clone that stuff)

First thing's first we want to move away from skid-steering into something more car-like by allowing the front two wheels to rotate, so our rover will look like this:

So, here's how I did it in a sorta jerry-rigged way.

You can download the project off of my Github here

(https://github.com/wrbernardoni/robot_tutorial_2 if ya want to git clone that stuff)

**1. Updating the Rover Model**First thing's first we want to move away from skid-steering into something more car-like by allowing the front two wheels to rotate, so our rover will look like this:

First let's add the rotating legs that we will attach the front wheels to.

Here's our left leg and joint:

Here's our left leg and joint:

And the right one is basically the same:

Now the last thing we've gotta change in our *.xacro is we've got to attach the front two wheels to the rotating legs instead of the body.

And now we can rotate our front two wheels!

Now it's time to make some code.

First things first, it'll be a lot easier to debug if we can put it in different situations, so let's have it subscribe to our command topic so we can give it some orders.

Now it's time to make some code.

**2. Let's get some steering**First things first, it'll be a lot easier to debug if we can put it in different situations, so let's have it subscribe to our command topic so we can give it some orders.

Now our rover subscribes to a cmd topic, and when something is posted in that topic it will save it to some private variables for later use.

Now let's actually get those wheels moving.

First things first, let's set up our angular control.

**3. Using those commands**Now let's actually get those wheels moving.

First things first, let's set up our angular control.

These are just some useful constants we will use later.

These are just some useful functions we will end up using later.

zeroToTwoPi just takes any angle in radians and turns it into it's equivalent form in the interval [0,2pi)

minAngleAdjust takes two angles, and figures out the smallest magnitude adjustment to get from the first to the second, it just uses a little trig trick to do this.

pLA and pRA are going to be the previous angles of our differential legs, this will be used to figure out the angular velocities of those legs.

Now let's actually implement our control system in our onUpdate function

zeroToTwoPi just takes any angle in radians and turns it into it's equivalent form in the interval [0,2pi)

minAngleAdjust takes two angles, and figures out the smallest magnitude adjustment to get from the first to the second, it just uses a little trig trick to do this.

pLA and pRA are going to be the previous angles of our differential legs, this will be used to figure out the angular velocities of those legs.

Now let's actually implement our control system in our onUpdate function

That formula looks a bit weird:

(((lDif/PI) * MAX_DIF_V - lAV)/MAX_DIF_V) * DIF_MAX_EFFORT

All it is doing is:

(((lDif/PI) * MAX_DIF_V - lAV)/MAX_DIF_V) creates a value (normally) in [-1, 1] that we will scale by the max effort we want to assert.

((lDif/PI) * MAX_DIF_V - lAV) Finds the difference between our desired velocity, and our current velocity (so how much acceleration we need to apply)

(lDif/PI) * MAX_DIF_V Just normalizes the difference in angle between the desired and the current, and then uses that to scale according to our max allowed velocity.

So really all we are doing is figuring out our desired velocity, and seeing how much acceleration we need to apply to the joint to get it to our desired velocity.

The tricky thing here is that what we are doing is trying to control our velocity, instead of purely basing our controls off of our current position. The reason we do this is because joint effort controls our angular acceleration on that joint, so to figure out how much acceleration we need we need to figure out what velocity we want to be at. You get a much tighter control on the joint if you control the velocity in addition to the position.

Now we can control where our wheels point, let's make them move!

Our linear control system will work almost the same:

(((lDif/PI) * MAX_DIF_V - lAV)/MAX_DIF_V) * DIF_MAX_EFFORT

All it is doing is:

(((lDif/PI) * MAX_DIF_V - lAV)/MAX_DIF_V) creates a value (normally) in [-1, 1] that we will scale by the max effort we want to assert.

((lDif/PI) * MAX_DIF_V - lAV) Finds the difference between our desired velocity, and our current velocity (so how much acceleration we need to apply)

(lDif/PI) * MAX_DIF_V Just normalizes the difference in angle between the desired and the current, and then uses that to scale according to our max allowed velocity.

So really all we are doing is figuring out our desired velocity, and seeing how much acceleration we need to apply to the joint to get it to our desired velocity.

The tricky thing here is that what we are doing is trying to control our velocity, instead of purely basing our controls off of our current position. The reason we do this is because joint effort controls our angular acceleration on that joint, so to figure out how much acceleration we need we need to figure out what velocity we want to be at. You get a much tighter control on the joint if you control the velocity in addition to the position.

Now we can control where our wheels point, let's make them move!

**4. Linear Control**Our linear control system will work almost the same:

You'll note that other than some trig to figure out our current velocity, this is pretty similar to how we control our differentials angles.

Our final rover.cc now looks like:

**All together now**Our final rover.cc now looks like:

**Homework:**Take some time to look over the solution I've given you, then take another shot at making your own rover.cc, trying not to copy or reference this solution on your first attempt.

If you end up with something similar try and add some improvements, some things you can think about adding:

- "Ghosting" protection. See how recent the last command was, and if it wasn't recent then tell the rover to stop, so that if our controller crashes our rover won't go flying off.
- Better linear control when turning. Right now if we are turning our linear controller speeds us up, we might not want that.
- Spinning in place. We can rotate the wheels if we aren't moving forward so that we can almost turn in place. Add to the controller so we can spin in place.
- And anything else you can think of adding!

Next Lesson