Lab Description
The purpose of this lab is to navigate through a set of waypoints within a maze. I used PID control throughout the maze to accomplish this.
Lab Sections
Part 1: Rotations
The path that needed to be followed can be seen below.
I split each section up into a rotation followed by a forward movement. The rotations are controlled through PID using the gyroscope the same way as in previous labs. First, I had to find the angle the robot must turn. I do this in the code below.
command[0] = (atan((slope_p2-slope_p1)/(1+slope_p1*slope_p2)))*180/3.14159;
This code implements an equation to find the angle between two lines in radians, then converts that to degrees. command[0] is passed as the setpoint to the rotational PID controller. Before moving onto adding linear movement, I made sure all the rotations
worked well in sequence, this led me to an issue where 90 degrees turns cause a division by 0. I added simple conditionals to deal with this.
if(isinf(slope_p1) && isinf(slope_p2))
command[0] = 90;
With this, the robot would execute the turns extremely well when in one spot.
Part 1: Linear movement
With rotations in place, I began working on linear movements. First, I used the equation to find the distance between two points.
command[1] = sqrt(pow(next[0]-curr[0],2) + pow(next[1]-curr[1],2))*304.8;
To this, I add a single front sensor reading.
command[1] += car.getFrontDist();
This will make the robot go the distance it needs to when driving in reverse. If going forward, the fron sensor reading must instead be subtracted.
With is in place, I was able to send my robot through the maze.
Besides hitting the wall, the robot navigates extremely well. I also included a victory localization at the end.
It was a bit off, but still very close.
Lab Description
Lab Sections
I split each section up into a rotation followed by a forward movement. The rotations are controlled through PID using the gyroscope the same way as in previous labs. First, I had to find the angle the robot must turn. I do this in the code below.
command[0] = (atan((slope_p2-slope_p1)/(1+slope_p1*slope_p2)))*180/3.14159;
This code implements an equation to find the angle between two lines in radians, then converts that to degrees. command[0] is passed as the setpoint to the rotational PID controller. Before moving onto adding linear movement, I made sure all the rotations
worked well in sequence, this led me to an issue where 90 degrees turns cause a division by 0. I added simple conditionals to deal with this.
if(isinf(slope_p1) && isinf(slope_p2))
command[0] = 90;
With this, the robot would execute the turns extremely well when in one spot.
With rotations in place, I began working on linear movements. First, I used the equation to find the distance between two points.
command[1] = sqrt(pow(next[0]-curr[0],2) + pow(next[1]-curr[1],2))*304.8;
To this, I add a single front sensor reading.
command[1] += car.getFrontDist();
This will make the robot go the distance it needs to when driving in reverse. If going forward, the fron sensor reading must instead be subtracted.With is in place, I was able to send my robot through the maze.
Besides hitting the wall, the robot navigates extremely well. I also included a victory localization at the end.
It was a bit off, but still very close.