So, now that we have established that we

need the go-to-goal behavior and possibly an avoid

obstacle behavior in order to go between points A and

B and we have figured

out that if we have a differential drive robot,

that we can model as a unicycle,

with constant velocity v naught,

then what we can really control is the heading

and the way we control it is Phi is equal to Omega.

Now, let's define the error.

Error equals to Phi desired minus Phi.

So, the desired heading minus

the heading we're in and then we can use

a PID controller acting on E and we should

remember that we may not want to

act directly on E because it's an angle.

So, this little trick with arc tangents two will

allow us to ensure that E stays within minus Pi and Pi.

Okay. Let's use this to actually build

a behavior that takes us to a goal.

So, the question then is,

the only unknown here which is what's Phi desired.

Well, let's say that we located it at x and y.

We know what the goal is x goal and

y goal and the way we know it

is either because of

the sensor skirt disc obstruction we talked about,

or some other way of knowing the goal location.

Well, it's not very hard

to compute what the desired angle is.

It's simply y goal minus y

divided by x goal minus x and an arc tangent of that.

So, Phi desired in this case, of course,

is this angle here.

So, that's given by this arc tangent formula.

So now, all we do is we plug it in to get the error.

Then we plugged the error in to

get the controller and then we

hook the controller in to get the update on the heading.

So, without further ado, let's do it.

Okay. Here's my first attempt.

Look at is we're getting there to.

What? All right.

What just happened here? This was attempt one

and well the robot started out.

It started out looking in wrong direction and then it

was started to turn nicely maybe

not enough and then something

happened and it seemed

to happen roughly when the angle here

was close to minus Pi over two,

but in fact, this is exactly what happened.

The problem here is that I

forgot that I was dealing with angles.

So, the issue is angles.

As we talked about let's not plug in angles right away.

Let's always make sure that they are within minus Pi and

Pi and by the way

Omega in this case was just K times the error.

So, it was a pure P regulator.

No I and no D part.

But this is what happens. Even if

you do a nice design and you

forget that you're dealing with

angles instead of other entities.

So, let's go to attempt two.

Same controller.

Now, I'm putting Omega equal to within quotations here.

This simply means that it's not

exactly this because I'm doing

the arc tangent two on this

and let's see what the robot is doing.

It's spiraling around the story error a little bit.

Not that great I must say.

So, what seems to be the matter?