Before moving on to defining the go to goal function.
One thing I must reinforce that is all the information that we gathered, the current force of the board
and the part to go.
Everything was done considering the image of the mes computer in the mapping stage as the frame of reference.
So in the go to go function, the board will just iteratively try to reach those many goals.
Define on that frame of reference.
And eventually reaching them is exit.
To add the go to go functionality to our board motion class, we first need to add a few instance variable
inside the init function so we head on over to the init function.
Scroll down and then add in the first instance variable.
That is basically a state variable that tells us about whether the ME's exit has been reached or not.
And because this is in this state and state, the goal not reach back what we set to do.
And then we add in the next instance, we label that basically containers that stores the current when
you go x and y axis.
And then we add in the final instance variable, that is basically an iterator that tells us about the
current minimal iteration.
And because this is the initializing step, the pot iterator is set to zero because iteration starts
from zero and then we head on over to the navigate path function.
We previously computed the bot angle in image from the bot and the relation that we previously computed.
So after this, because we have the complete force computed, we can now call in the go to go function
to make the robot exit.
But before calling the go to go function we first need to define it.
So we head back before the navigate bot function and define the go to go function and that would be
named as go to go and its arguments are exactly the same as navigate function that is both location.
But we lost the object and velocity probably should.
And then we define the go to go function.
Now inside it, the very first thing that we need to do is basically compute the angle to go and the
distance to go.
And that can be done by simply using the angle and distance function that we defined earlier and passing
in the current smart location and that many go x and y axis.
So we assessed angular distance function for the point where we pass in the current port location and
for the point B we pass passing the tuple that access the goalpost x.
And the goal was why?
So once we have provided this information, what we receive as an output are two outputs.
The first is the angle to boot, and the second is the distance to go.
So once we have the angle and the distance to go, we can now compute the actual angle the car needs
to take to orient itself towards that meaning goal and to compute the angle to turn.
What we need to do.
We need to simply.
Subtract and go to go from the car orientation.
So got orientation is basically the bot angle.
So once we subtract the boat angle from the angle to go, we get the angle to turn or the angle with
the card needs to turn so that it can orient itself towards the winning goal.
So once we have the information about the distance to go, angle to turn, we can now compute the actual
angle of the angle and the speed of the car that it needs to take to make the exit.
So that can be done by simply using a function of inter that is a function of an numbers library.
So we compute speed for that is basically using the inter function of number library.
We pass in the variable that is basically the distance to go and we basically use the distance to go
and the speed would be just proportional to distance to go.
So the greater the distance to the goal, the greater the speed of the car.
So basically how this works is that our distance to go either has a range of 0 to 100 and we interpolate
this list down from 0 to 100 for distance to go to get the speed that will be basically be from 0.2
to 1.5.
So for the hundred distance, the maximum speed would be given that is 1.5.
And for the zero distance, the minimum speed would be given that is 0.2.
And then be adding a compute angle or steering angle which to God needs to orient itself towards the
exit.
And that is again using the inter function we provide in.
The angle to turn.
And go through June as a variable.
And then it was in its range that is basically -360.
To Boston 360.
And then we pass in the new range that we want to interpolate to.
That is basically negative for two, positive four or basically the range of the steering angle.
Once we have computed the speed and the angle of the car, which it needs to take to reach the miss
or the meaning goal, then what we need to do, we need to simply print out this information to the
user and also print out a distance to go.
Now, once we have this information, the next thing that we need to do, we need to start.
Start checking if the car is far away, then what we need to do, we need to turn towards the goal.
So if the distance of to go is greater than two, then we set angular component of the velocity object
to set equal to the angle.
So basically the distance to go is greater than some value.
Then we start or we turn the car towards the exit or towards the mini go and then we address the second
case.
That is because our car is a differential drive, but it cannot turn at high speeds and it cannot run
more than a certain limit.
So we need to address that limit.
And what we need to do, we simply check if the angle of the car angle the car needs to take.
When a steering angle is greater than a certain value, then we either stop the car or if it is less
than a certain value, then we move at a very low speed.
Otherwise, if it is within a reasonable value, then we run with a speed computer earlier.
So once you've completed this stuff, we then proceed on to simply pass on the velocity component or
velocity object that we have just updated using the velocity publisher.
And if we have not reached the final goal, so once you're done with this is simply checked.
If we have reached the winning goal and that is done by simply checking if the distance to goal is less
than eight pixels.
So if the distance to goal is within a certain threshold, then we have reached the winning goal and
we need to simply stop the card.
We're starting the linear and angular component of the car to zero and publishing this velocity using
the velocity publisher.
And then we simply check if we have reached the final goal, and that is done by simply checking the
sift or path.
Iteration is equal to the length of port minus one.
So if we have reached a five minute goal, then we simply set the goal, not reach back to false, indicating
that we have reached the final means exit and we simply play the party music and then we address the
other case.
For the other case, if we have not reached the final goal, then we need to iterate over the next winning
goal in the list and that is done by simply writing an L statement where we iterate over the next part,
iterate to get the next goal, post X and the goal post y and simply play the music of that.
We have reached the next winning goal.
Once we are done with this, our port to go or go to goal has been completed.
So we head back for the navigate port function.
But before this.
We need to do a few imports.
The first import is basically the internal function that is part of the library.
And the second function and second import that we need to do is basically the importing of the by name
library.
And this is necessary to play the music that we want.
Once we have reached the winning goal or the final exit.
So we head over to the top.
Add in the new imports that are the into and the buying game library.
And once we are done with this we head back to the navigate bot function where we simply check or address
the case for if the path iterator is at the very beginning.
At this point, the Google equals x will set to zero and zero.
We don't want zero and zero, but rather we would want to tell it that it needs to reach the first reading
in the path list and that is done by simply searching.
If the path is equal to zero, then we simply set the port first variable.
A path iteration to the goal was X and the goal was Y and then we head on over down where we have computed
the bot angle inside the image case, we simply now call in the go to goal function, which is simply
now calling the go to go function.
So you provide in the vault location, report, provide in the port computer, and then you providing
the velocity object and then you provided the velocity publisher.
So this basically tells the car to reach the next me to go iteratively until it reaches a final exit.
So this means that we have completed our go to go functionality.
We have provided it to the motion planning class and now we can simply integrated to our Ms..
Salvador Pi and see if it does the job of reaching the exit or not.
To perform the analysis of the go to go function for the motion planner class.
We are adding a new function that basically displays a control mechanism in action and that is done
before the angle and distance function.
So right over here we have the display control mechanism in action.
This takes in the input argument of what location the port to go.
It was taught to spot the localize and the frame to display.
Then we head back by saving this to Miss Solver.
Scroll down.
We previously called the bot motion planner function of Navigate Path to navigate to go.
We then call in the function that is basically display the control mechanism in action.
We passing the board location and part previously computed and then image shows the spot that would
basically be the bot path planner instance variable.
We have not defined this instance variable, so let's define it inside the board for class.
So we head back to the board planner, create the new instance variable that is in the shortest path.
Assign this to an empty list and then we head back to the drop bot on Miss Function where we have the
Miss video.
And this Miss BGR would become the value of the newly assigned image's shortest path.
So we assign the value of Miss VTR to this instance variable.
You save this, we head back to the Ms. solver, maybe boss in this image.
Shortest path.
And the third argument and then we boss in the bot localize the object.
And finally the frame to the.
Then we have we simply, I am sure, the same display which shows us the life made solving in action.
Now we can simply head on over to our terminal.
I have already started a simulation.
Build the project.
Run the missile over.
By writing rules to run.
Mays bought it.
So.
To start the localizing state and then we get this output of the mail solving bought life.
So oops, we have landed on an error and the error is that we have a value error that is FP and XB are
not of the same length.
So FP and SBY are basically the initial range that we wanted to interpolate from.
And this is occurring on the line 14 to basically underline not fitting to our line one to do one and
go to go function where we have incorrectly passing the information of negative four to positive four.
And here if you can see this should be a comma representing a range of a list.
So we head back to the go to go function inside the motions class.
Inside the go to go function.
Really we have basically defined the range and we here we have the range, the new range that we wanted
the angle to be interpreted to.
So this should be -4 to 4, but rather it is written as -4.4.
So this is why it is causing an error.
Once we fix this error, we head back, close the simulation, run the project again.
Launch the simulation.
Once a simulation starts, I would run the Maze Solver.
Then we get the first output of the localizing stage dressing space for.
The output of them is now.
The car is moving.
It's trying to compute the correct robust pause or the complete robot pause, and then it's heading
somewhere very long.
So it has collided.
And if you look in the simulation.
Our car has just collided to the to the to the wall.
There seems to be some problem with the algorithm and is this problem is more complex.
So if you look.
The minute is shown very wrong.
So there is something wrong with the mini gold being printed, so we need to fix that.
So the bottle's computer inside the bottling algorithm.
So let's see if we can find the error.
So we head back to the board then.
Good bye.
Maybe have the board part, plan a class.
And here we were, computing or saving the fourth to go inside this instance variable and it is initialized
inside the fine part in display function where we are initializing the part to display value for the
part to go.
But the problem is we required the coordinates to point, so we required the point to be saved inside
the particle instead of the coordinates.
So that can be done by simply going deleting this going after the parts point to display has been computed
using the coordinates, the point function and simply assigning the port to go the value of what point
to display.
So hopefully this fixes the error and we can now get the correct location the robot needs to move to
reach the main exit.
So we re rebuild our main solver, run the simulation.
And once the simulation starts up, it will run the myth solver.
You get the first output of the localise and stage dressing space for.
Figured out of the man's life.
This time the car is moving and it's moving towards.
Computing is complete robot pose and then it goes back and tries to raise the very first meeting goal
that was of our path computed from the path then anchored by.
So once it has reached the first minute goal, it makes a sound that it has reached the winning goal,
just like Pac-Man.
And then it tries to approach the next minute goal, which is shown right here.
So there should be another window displays this all action live.
And as you can see, this red ball basically represents the location of a robot.
The green is basically the winning goal that we have already reached, and this is basically the winning
goal that the robot is about to reach.
So if you see right over here on the left window, the red represents the car moving towards the orange
mini goal and the green is the winning goal that it has already reached.
So the farther it was from the winning goal, the faster it move and the more angle the car needs to
doing so, the slower the car's speed is so that they can turn easily.
So it tries to raise one meaning goal at a time it attractively until it reaches the music.
At this moment you are seeing that it is reaching the winning goal that is in front of it.
So at this moment it reaches its minimum goals and then it needs to make a turn so its force makes Leyton
orient itself towards the winning goal and then it reaches that winning goal.
And once each segment go, it tries to re-orient towards the next minute goal that is on its right side.
So you can see it's not moving.
And once it reorients itself, then it moves toward that when you go.
And keep this whole process going until it reaches all the mini goals, until the final and when you
go.
So on the top you could see the length of the path is basically 78 to 78.
Many goals are what our go to go function has to try to reach until it reaches the final winning goal.
And the current iteration is basically 48 mini goals.
So it is right now moving in this direction.
And if you look on the left hand side, this is basically what is considered as a frame of reference,
this left image, because the entry is on the top of the image and the exit is on the bottom.
So car is about to reach the maze exit as it approaches this winning goal.
That is basically the winning goal is number 60.
It reorients itself towards this winning goal by rotating itself, making the angle to turn, and then
moving towards the final winning goal.
So right now is approaching is why I'm going to go.
And this is a large screen when you go.
And we'll love it as rich is finally going to go.
So in this way we have conformed that our go to function indeed helps car solve the maze by reaching
the maze exit.
So you can now proceed on to testing it out on a more complex scenario that is Maze two and see if it
does the same job or the same correct job of reaching the muse exit on that particular case.
Until then, happy coding.
And by.