Lab 13

Net ID: pp386

Objective: Path Planning and Execution

Finally after months of work, the day has come to gather all the concepts that we have learned and to execute towards building a path planning robot. This lab will serve as a culmination of the course. The robot will start at a location on the map, move through certain waypoints and end at another endpoint. The endpoints are given in the image below which also shows the shape of the trajectory.

For this lab, I had to first start with integrating the two PID controllers, one used in Lab 6 to avoid hitting the wall using a TOF sensor and one using the gyroscope as used in Lab 12. I decided to create 2 functions, one for rotation and one for translation along with their own PID controllers.

Starting point (-4,-3)

I tried to implement PID control on the original trajectory shown in the green line above but there was a problem. The distance sensor wasnt able to detect the far off wall at the starting point nor the second point. Therefore, I decided to follow a principle I made (I think). Always point the way closest to the wall. Therefore I turned my car the other way and tried to execute from the first point. However there was a problem. The TOF sensor doesn’t work well around corners as it also get the direct reflection of the laser beam and the indirect reflection of the laser beam. Therefore I decided to use a city block distance technique as shown in the figure below.

I found some advantages of using the city block based distance measurement and trajectory. Firstly, my angle PID rotation is pretty accurate, and I could turn almost 90 degrees or 180 or 270 degrees accurately. Using city block, the walls become flat to the TOF sensor and there are no diagonal reflections. It is the easiest way for a TOF sensor to work. So, I figured if I could move accurately in both distance and angle, my errors become less and the possibility of a failure ceases to exist.

On the Python side, I used the distances measured in the figure above to execute each part of the trajectory. I decided to keep control on the PC side because it made the debugging much easier and we had control over the trajectory if the robot went haywire, like spinning on its own axis due to slipping and so on.

PID runs with city block distance

Reason for not using Localization

I ran the localization code from Lab 12 for the starting point (-4,-3) but it was already off by 1 foot tile (-3,-3). Seeing this, I decided not to implement Localization along with PID. This is because lets assume we get a 1 tile error for each localization. For so many waypoints, the cumulative error will become too large and the robot would practically not know where it is.
While I was using PID, I was getting accuracy of around half a tile from the points at least. This was much lesser than the localization error for each step, which would just accumulate over time. I therefore, made the hard choice of not using localization.

Ghost detection by TOF at point (1,-1)

At point (1,-1) my TOF picked up something many times and would actually move backward rather than moving forward, it might be because the sensor got the reflected ray from the box rather than the wall. You can see that in the first video.

Flowchart of the code

THe basic structure of the code is just 2 PID controllers working alternatively to achieve the trajectory.

Sensor positioning

One thing I noticed in this lab was that sensor positioning was very crucial to the TOF sensor reliability. For some waypoints the sensor seemed to perform well but for some waypoints the sensor readings were off and the car would overshoot by a lot or just hit the wall.

Problems faced during the lab

I would say I was dealt the worst luck when it came to hardware, my car banged the wall and a motor wire connected to Artemis came off and this caused one of my motors to work only in one direction. This took some time to fix. Sensor values sometimes froze midway, meaning the PID wouldnt update and the car would just ram into the wall. I rectified this issue by adding a check for data ready condition before the sensor values were taken and only execute the PID thereafter if the sensor was ready.

Conclusion

This lab is a culmination of the course and I am quite happy and feel accomplished that I was able to make the robot do the full execution of the map without any collaboration with anyone except asking the TAs (Vivek, Jonathan and Jade) some doubts. In an ideal world, a PID controller sounds like not a good enough method to do this lab, rather seems like a repetition of lab 6 and lab 12, but localization performed pretty poorly sometimes meaning that the robot would go way off the required waypoints if we localized at each step. Maybe only localizing at lesser waypoints could have worked better for the purpose but due to lack of time during the finals week, I was not able to try out localization. I would definitely want to try out some cooler algorithms with my robot over the summer as I genuinely enjoyed this course even though it was my first time taking a robotics class and it has inspired me to take more robotics courses in the Fall semester to maybe work on more concept building and building more robots on the way. Thank you!

Appendix

Code functions for rotation and translation:

void rotate(double angle)
{ 
  myICM.getAGMT();         // The values are only updated when you call 'getAGMT'
  float timegyro=millis();
  current_point_rot=myICM.gyrX();
  degreess+=(current_point_rot)*(timegyro-prev_t_rot)/1000;
          //prev_gyr=current_point;
  
  PID_Controller_rot(Setpoint_rot,current_point_rot);
  prev_t_rot=timegyro;
  if (degreess<angle){
  motor=Output_rot;
  if(motor>115){
    motor=115;
  }
  analogWrite(16, motor); 
  analogWrite(14, 0);
  analogWrite(7, 0);
  analogWrite(5, motor);
  }
else
{
  analogWrite(16, 255); 
  analogWrite(14, 255);
  analogWrite(7, 255);
  analogWrite(5, 255);

  flag=0;
}
}
double distance=0;
double distmoved=0;
void translate(double dist)
{ 
  distanceSensor2.startRanging();
  if (distanceSensor2.checkForDataReady()){
  
  if (flagone==0){
  
  Setpoint=distanceSensor2.getDistance();
  distance=Setpoint;
  distanceSensor2.clearInterrupt();
  distanceSensor2.stopRanging();
  Setpoint=Setpoint-dist;
  flagone=1;
}
else if (flagone==1)
  {
  
  current_point = distanceSensor2.getDistance(); 
  Serial.println(current_point);
  sendingtime=millis();
  distanceSensor2.clearInterrupt();
  distanceSensor2.stopRanging();
  
  sendingval=current_point;
  if (current_point<Setpoint+2 && current_point>Setpoint-2){
   analogWrite(16, 255); 
  analogWrite(14, 255);
  analogWrite(7, 255);
  analogWrite(5, 255);
  translationdone=1;
  flagone=0;
  }
  else{
  PID_Controller(Setpoint,current_point);
  motor=Output;
  if(motor>=127)
  {
    motor=127;
  }
  else if(motor<-100)
  {
    motor=-100;
  }
  else if(motor>0 && motor<40)
  {
    motor=40;
  }
  else if(motor>-40 && motor<0)
  {
    motor=-40;
  }
  if(motor<0)
  { 
  motor=-motor; 
  analogWrite(16, 0); 
  analogWrite(14, motor);
  analogWrite(7, 0);
  analogWrite(5, motor);
  }
  else
  {
  analogWrite(16, motor); 
  analogWrite(14, 0);
  analogWrite(7, motor);
  analogWrite(5, 0);
  }
  

  }
  
  }
}

}