main.cpp

00001 #include "mbed.h"
00002 #include "btbee.h"
00003 #include "m3pi_ng.h"
00004 #include <vector>
00005 #include <string>
00006 m3pi robot;
00007 Timer timer;
00008 Timer time_wait;
00009 #define MAX 0.5
00010 #define MIN 0
00011 
00012 #define P_TERM 3
00013 #define I_TERM 0
00014 #define D_TERM 27
00015 
00016 void turnRight();
00017 void turnLeft();
00018 void goStraight();
00019 
00020 int main()
00021 {
00022 
00023     robot.sensor_auto_calibrate();
00024     wait(2.0);
00025 
00026     float right;
00027     float left;
00028     //float current_pos[5];
00029     float current_pos = 0.0;
00030     float previous_pos =0.0;
00031     float derivative, proportional, integral = 0;
00032     float power;
00033     float speed = MAX;
00034     string turns;
00035     int direction = 0;
00036     int x [5];
00037     bool passed = false;
00038 
00039 
00040     
00041     int Run = 1;
00042     string blah[36] = {"", "LRRL", "SSRL", "RLSRL", "RLLLRSRLLR", "LRLL",
00043                     "RLLR", "", "SSR", "RLRR", "RLLRLSR", "LRSLR",
00044                     "RLSS", "LLS", "", "RR", "RLSLR", "LRSSS",
00045                     "RLSRL", "LLRL", "LL", "", "RLSS", "RLRLS",
00046                     "RRSRR", "LSRLRRL", "LRSRL", "SSRL", "", "RLRRRL",
00047                     "RRR", "LRSLR", "SSSLR", "SRLL", "RLLLRL", ""};
00048     //vector< vector<string> > Lookup(6, vector<string>(6));
00049     
00050     //Lookup[1][2] = "LLS";
00051     
00052 
00053     
00054 
00055 
00056     time_wait.start();
00057      turns = blah[(6-1)*6+(5-1)];  
00058 
00059     while(Run) {
00060         time_wait.reset();
00061         //Get raw sensor values
00062         
00063         robot.calibrated_sensor(x);
00064 
00065         //insert end and beginning locations
00066 
00067   
00068 
00069         //Check to make sure battery isn't low
00070         if (robot.battery() < 2.4) {
00071             timer.stop();
00072             break;
00073         }
00074 
00075         
00076         
00077         if(((x[0]>300 && (turns.at(direction) == 'L')) || (x[4]>300 && (turns.at(direction) == 'R')) || ((x[0] > 300 || x[4] > 300) && turns.at(direction) == 'S')) && (direction < turns.size()) && !passed) {
00078             
00079             
00080             if(turns.at(direction) == 'L'){
00081                 while(x[0] > 300){
00082                     goStraight();
00083                     robot.calibrated_sensor(x);
00084                     }
00085                 previous_pos = 0;
00086                 robot.stop();
00087                 //wait(1);
00088                  while(x[2] > 300){
00089                     turnLeft();
00090                     robot.calibrated_sensor(x);
00091                 }
00092             
00093                 while((x[0] > 300) || (x[2] < 300)){
00094                     turnLeft();
00095                     robot.calibrated_sensor(x);
00096                     }
00097                 robot.stop();
00098                 //continue;
00099                 /*wait(1);
00100                 goStraight();
00101                 wait(1);*/
00102             }
00103             else if(turns.at(direction) == 'R'){
00104                 previous_pos = 0;
00105                 robot.stop();
00106                 while(x[4] > 300){
00107                     goStraight();
00108                     robot.calibrated_sensor(x);
00109                 }
00110                 //wait(1);
00111                 while(x[2] > 300){
00112                     turnRight();
00113                     robot.calibrated_sensor(x);
00114                 }
00115                 
00116                 while((x[4] > 300) || (x[2] < 300)){
00117                     turnRight();
00118                     robot.calibrated_sensor(x);
00119                     }
00120                 robot.stop();
00121                 
00122                 /*wait(1);
00123                 goStraight();
00124                 wait(1);*/
00125             }
00126             else{
00127                 robot.stop();
00128                 //wait(1);
00129                 while(x[0] > 300 || x[4] > 300){
00130                     goStraight();
00131                     robot.calibrated_sensor(x);
00132                 }
00133                 //wait(1);
00134             }
00135             robot.cls();
00136             robot.printf("Size %d", direction);
00137             ++direction;
00138             passed = true;
00139             continue;
00140         }
00141         else if (x[0]>300 || x[4]>300 && direction >= turns.size()){
00142             robot.stop();
00143             robot.cls();
00144             robot.printf("Try again");
00145             while(x[2] < 300 || x[0] < 300 || x[4] < 300){
00146                 turnRight();
00147                 robot.calibrated_sensor(x);
00148             }
00149             break;
00150         }
00151         else if (x[0]>300 || x[4]>300 && passed)
00152             passed = true;
00153         else
00154             passed = false;
00155 
00156 
00157         // Get the position of the line.
00158         current_pos = robot.line_position();
00159         proportional = current_pos;
00160 
00161         derivative = current_pos - previous_pos;
00162 
00163 
00164         //compute the integral
00165         integral =+ proportional;
00166 
00167         //remember the last position.
00168         previous_pos = current_pos;
00169 
00170         // compute the power
00171         power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
00172         //computer new speeds
00173         right = speed+power;
00174         left = speed-power;
00175 
00176         //limit checks
00177         if(right<MIN)
00178             right = MIN;
00179         else if (right > MAX)
00180             right = MAX;
00181 
00182         if(left<MIN)
00183             left = MIN;
00184         else if (left>MIN)
00185             left = MAX;
00186 
00187         //set speed
00188 
00189         robot.left_motor(left);
00190         robot.right_motor(right);
00191 
00192         wait((5-time_wait.read_ms())/1000);
00193     }
00194 
00195 
00196 
00197     robot.stop();
00198 
00199     /*char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
00200                   ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
00201                   ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
00202                   ,'G','8','E','8','D','8','C','4'
00203                  };
00204     int numb = 59;
00205 
00206     robot.playtune(hail,numb);*/
00207 
00208 
00209 }
00210 
00211 
00212 void turnRight(){
00213     //Timer turner;
00214     //turner.start();
00215     //while (turner.read_ms() < 10){
00216             robot.right(.5);
00217     //}
00218     //turner.stop();
00219 }
00220 
00221 void turnLeft(){
00222 //    Timer turner;
00223   //  turner.start();
00224   //  while (turner.read_ms() < 10){
00225             robot.left(.5);
00226    // }
00227     //turner.stop();
00228 }
00229 
00230 void goStraight(){
00231    // Timer turner;
00232    // turner.start();
00233    // while (turner.read_ms() < 5){
00234             robot.forward(.5);
00235    // }
00236    // turner.stop();
00237 }