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? Well, the problem is the following. I am driving the car at a constant speed, V naught and then I'm turning based on this equation up here and it doesn't seem like I'm turning fast enough. You know, if I'm going really fast, then I need to turn a lot to actually get to where I want. So, the problem simply is that I'm not turning quickly enough or maybe I should slow down when I get closer to an obstacle and in truth, if you're controlling V and Omega at the same time, you need to be more smart in your control design and just a PID regulator on the heading error. But since this is what we're doing right now, simply what we should observe is the gain is not high enough. K is not high enough to steer us towards where we would like to go. So, let's do the same thing, but let's make K bigger. So, this is attempt three. We're doing the same thing. We have K big and again, we have the quotations there because it's not exactly this we're doing. What we're really doing is this arc tan two trick on the error on the angles to make sure that we get something between minus Pi and Pi and this is just right. This was a successful control design and in fact pure P part and I don't know if you remember this video that I showed you that showed how the metric drift was happening. It was this video. Well, let's look at it again and this time I have the volume on because it's really exciting. This is a competition and what these robots are doing is exactly what we just saw. The obstacle goal using a P regulator and this is a competition of the P games and in this case one robot did well and the other one was forced out alone. But this was exactly the controller that was running on this kind of robot. Okay.