main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "Servo.h"
00004 #include "LSM9DS1.h"
00005 #include <math.h>
00006 
00007 #define TIMEOUT_VAL_MS 15
00008 #define PI 3.14159
00009 #define DECLINATION -4.94
00010 #define DIST_FACTOR 0.107475 //cm/tick Conversion between a tick from Hall effect sensor and linear distance.
00011                             //Approximately 190 ticks for a full wheel rotation.
00012                             //Approximately 6.5cm wheel diameter
00013                             //pi*diameter (cm) / 190(ticks)
00014 volatile float heading;
00015 float prevHeading[3];
00016 void getHeading(float ax, float ay, float az, float mx, float my, float mz);
00017 float PI_cont(float err);
00018 
00019 //MOTOR PINS
00020 Motor rwheel(p22, p18, p17); // pwmA, fwd / Ain2, rev / Ain1
00021 Motor lwheel(p23, p15, p16); // pwmB, fwd / Bin2, rev / 
00022 //BLUETOOTH PINS
00023 Serial bt(p28,p27);
00024 //LEFT SONAR
00025 DigitalOut triggerL(p14);
00026 DigitalIn echoL(p12);
00027 DigitalOut echo1RiseWait(LED1);
00028 DigitalOut echo1FallWait(LED2);
00029 //RIGHT SONAR
00030 DigitalOut triggerR(p13);
00031 DigitalIn echoR(p11);
00032 DigitalOut echo2RiseWait(LED3);
00033 DigitalOut echo2FallWait(LED4);
00034 int correction = 0; //Used to adjust software delay for sonar measurement
00035 Timer sonar;
00036 Timer timeout;
00037 
00038 //SERVO PINS
00039 Servo angle(p21);
00040 //IMU PINS
00041 LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
00042 Ticker nav;
00043 float posX = 0;
00044 float posY = 0;
00045 int tickDistL = 0;
00046 int tickDistR = 0;
00047 
00048 Serial pc(USBTX, USBRX);
00049 Timer timestamp;
00050 
00051 // Set up Hall-effect control
00052 InterruptIn EncR(p25);
00053 InterruptIn EncL(p24);
00054 int ticksR = 0; // Encoder wheel state change counts
00055 int ticksL = 0;
00056 float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
00057 float speedR = 0; //Same for right wheel
00058 int dirL = 1; //1 for fwd, -1 for reverse
00059 int dirR = 1; //1 for fwd, -1 for reverse
00060 Ticker Sampler; //Interrupt Routine to sample encoder ticks.
00061 int tracking = 0; //Flag used to indicate right wheel correction
00062 int sweeping = 0;
00063 
00064 //PI controller
00065 //global variables
00066 float kp=12.0f;
00067 float ki=4.0f;
00068 float kd=1.0f;
00069 float P=0;
00070 float I=0;
00071 float D=0;
00072 float dt=0.1f;
00073 float out=0.0f;
00074 float max=0.5f;
00075 float min=-0.5f;
00076 float prerr=0.0f;
00077 
00078 //SLAM variables
00079 float angle_list[64];
00080 float dist_list[64];
00081 int slam_ind = 0;
00082 float slam_dist = 0.0f;
00083 //Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
00084 void sampleEncoder()
00085 {
00086     float E; // Error  between the 2 wheel speeds
00087     if(tracking) { //Right wheel "tracks" left wheel if enabled. 
00088 
00089         E = ticksL - ticksR; //Find error
00090         //E = (ticksL+1)/(ticksR+1);
00091         if (dirL == 1)
00092         {
00093             speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
00094             //speedR = E*speedR;
00095             if (speedR < 0) speedR = 0;
00096         }
00097         else if (dirL == -1)
00098         {
00099             speedR = speedR - float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
00100             //speedR = E*speedR;
00101             if (speedR > 0) speedR = 0;
00102         }
00103         rwheel.speed(speedR);
00104     }
00105     //pc.printf("tickL: %i   tickR: %i  Error: %i\n\r ",ticksL, ticksR, ticksL - ticksR);
00106     ticksR = 0; //Restart the counters
00107     ticksL = 0;
00108 }
00109 
00110 float angleAdjust(float a)
00111 {
00112     while(a<0 || a>360)
00113     {
00114         if (a<0) a+=360;
00115         if (a>360) a-=360;
00116     }
00117     return a;
00118 }
00119 void getHeading(float ax, float ay, float az, float mx, float my, float mz)
00120 {
00121     float roll = atan2(ay, az);
00122     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
00123     //Shift previous headings
00124     prevHeading[2] = prevHeading[1];
00125     prevHeading[1] = prevHeading[0];
00126     prevHeading[0] = heading;
00127 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00128     mx = -mx;
00129     if (my == 0.0)
00130         heading = (mx < 0.0) ? 180.0 : 0.0;
00131     else
00132         heading = atan2(mx, my)*360.0/(2.0*PI);
00133     //pc.printf("heading atan=%f \n\r",heading);
00134     heading -= DECLINATION; //correct for geo location
00135     heading += 90;
00136     //if(heading>180.0) heading = heading - 360.0;
00137     //else if(heading<-180.0) heading = 360.0 + heading;
00138     //else if(heading<0.0) heading = 360.0  + heading;
00139     heading = angleAdjust(heading);
00140     heading = (heading + prevHeading[0] + prevHeading[1] + prevHeading[2])/4.0;
00141     
00142 }
00143 
00144 float PI_cont(float err)
00145 {    
00146      float Pout=kp*err;
00147      P=Pout;
00148      //pc.printf("P is: %f \n\r",P);
00149      
00150      float Iout=Iout+err*dt;
00151      I=ki*Iout;
00152      //pc.printf("I is: %f \n\r",I);
00153      
00154      float Dout = (err - prerr)/dt;
00155      D=kd*Dout;
00156      
00157      out=P+I+D;
00158      
00159      prerr=err;
00160      
00161      //pc.printf("out is: %f \n\r",out); // basically pwm out
00162      if (out>max)out=max;
00163      else if (out<min)out=min;        
00164      return out;
00165 }
00166 
00167 void turn(float angle)
00168 {
00169         float s =0.05;//speed by which the robot should turn
00170         while(!imu.magAvailable(X_AXIS));
00171         imu.readMag();
00172         while(!imu.accelAvailable());
00173         imu.readAccel();
00174         while(!imu.gyroAvailable());
00175         imu.readGyro();
00176         getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
00177                       imu.calcMag(imu.my), imu.calcMag(imu.mz));
00178         float PV = heading;//the original heading
00179         float SP = heading - angle;//the new required heading
00180         if(SP>=360){//reseting after 360 degrees to 0
00181             SP = SP-360;
00182             }
00183         pc.printf("PV: %f\n",PV);
00184         pc.printf("SV: %f\n",SP);
00185         wait(2);
00186         //pc.printf("required: %f\n",requiredHeading);
00187         float error = angle;
00188          pc.printf("error: %f\n",error);
00189          int reached = 0;
00190          while(reached ==0){
00191          
00192                 while(!imu.magAvailable(ALL_AXIS));
00193                 imu.readMag();
00194                 getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
00195                       imu.calcMag(imu.my), imu.calcMag(imu.mz));
00196                 PV = heading;//calculate the current heading
00197                 
00198                 error=(PV-SP)/360;//find error between current heading and required heading
00199                 float correction=PI_cont(error);//correction to motor speed
00200                      if (error> 1.5/360 || error<-1.5/360) { //an error limit of 1.5 degrees allowed.. checking if the robot needs to turn more
00201                             rwheel.speed((s+correction)/2);
00202                             lwheel.speed((s-correction)/2);
00203                             pc.printf("error in 1st if: %f\n\r",error);
00204                             pc.printf("error in 1st if: %f\n\r",error);
00205                             }
00206                     else if (error>=-1.5/360 && error<=1.5/360) { //within within satisfying angular range 
00207                              rwheel.speed(0);
00208                              lwheel.speed(0);//stop moving and exit the loop
00209                              pc.printf("error in 2nd if: %f\n\r",error);
00210                              break;
00211                               }       
00212                               wait(0.01);                  
00213                         }
00214    
00215     rwheel.speed(0);
00216     lwheel.speed(0);
00217 }
00218 
00219 
00220 void sendCmd(char cmd, float arg)
00221 {
00222     bt.printf("!%c%.2f",cmd, arg);
00223     //unsigned char c[sizeof arg];
00224     //memcpy(c, &arg, sizeof arg);
00225     //bt.putc('!');
00226     //bt.putc(cmd);
00227     //bt.putc(c[0]);
00228     //bt.putc(c[1]);
00229     //bt.putc(c[2]);
00230     //bt.putc(c[3]);
00231 }
00232 
00233 int ping(int i)
00234 {
00235     float distance = 0;
00236     switch(i)
00237     {
00238         
00239         case(1): //Ping Left Sonar
00240         // trigger sonar to send a ping
00241         //pc.printf("Pinging sonar 1...\n\r");
00242         triggerL = 1;
00243         sonar.reset();
00244         //wait_us(10.0);
00245         //wait for echo high
00246         echo1RiseWait = 1;
00247         timeout.reset();
00248         timeout.start();
00249         while (echoL==0){
00250             if (timeout.read_ms() > TIMEOUT_VAL_MS) return 0;
00251             };
00252         echo1RiseWait = 0;
00253         //echo high, so start timer
00254         sonar.start();
00255         triggerL = 0;
00256         //wait for echo low
00257         echo1FallWait = 1;
00258         timeout.reset();
00259         timeout.start();
00260         while (echoL==1) {
00261             if (timeout.read_ms() > TIMEOUT_VAL_MS*5) return 0;
00262             };
00263         echo1FallWait = 0;
00264         //stop timer and read value
00265         sonar.stop();
00266         //subtract software overhead timer delay and scale to cm
00267         distance = (sonar.read_us()-correction)/58.0;
00268         //pc.printf("Got distance %f cm back.\n\r", distance);
00269         //wait so that any echo(s) return before sending another ping
00270         wait(0.015);
00271         break;
00272         case(2): //Ping Right Sonar
00273         // trigger sonar to send a ping
00274         //pc.printf("Pinging sonar 2...\n\r");
00275         triggerR = 1;
00276         sonar.reset();
00277         //wait_us(10.0);
00278         //wait for echo high
00279         echo2RiseWait = 1;
00280         timeout.reset();
00281         timeout.start();
00282         while (echoR==0) {
00283             if (timeout.read_ms() > TIMEOUT_VAL_MS) return 0;
00284             };
00285         echo2RiseWait = 0;
00286         //echo high, so start timer
00287         sonar.start();
00288         triggerR = 0;
00289         //wait for echo low
00290         echo2FallWait = 1;
00291         timeout.reset();
00292         timeout.start();
00293         while (echoR==1) {
00294             if (timeout.read_ms() > TIMEOUT_VAL_MS*5) return 0;
00295             };
00296         echo2FallWait = 0;
00297         //stop timer and read value
00298         sonar.stop();
00299         //subtract software overhead timer delay and scale to cm
00300         distance = (sonar.read_us()-correction)/58.0;
00301         //pc.printf("Got distance %f cm back.\n\r", distance);
00302         //wait so that any echo(s) return before sending another ping
00303         wait(0.015);
00304         break;
00305         default:
00306         break;
00307     } 
00308     return distance;
00309 }
00310 
00311 void sweep()
00312 {
00313     __disable_irq();
00314     float AL = 0.0;
00315     float AR = 0.0;
00316     int dL = 0;
00317     int dR = 0;
00318     sweeping = 1;
00319     if (angle <=0.5) //Set to min angle and execute forward sweep.
00320     {
00321         angle = 0;
00322         wait(.2);
00323         while( angle >= 0 && angle < 1)
00324         {
00325             dL = ping(1);
00326             dR = ping(2);
00327             AL = angleAdjust(heading + (angle-0.5)*55 - 45);
00328             AR = angleAdjust(heading + (angle-0.5)*55 + 45);
00329             if (dL > 20 && dL < 200)
00330             {
00331                 sendCmd('A',AL);
00332                 sendCmd('D',dL);
00333                 dist_list[slam_ind] = dL;
00334                 angle_list[slam_ind] = AL-heading;
00335                 if (slam_ind < 63) slam_ind++;
00336                 pc.printf("(%.1f,%.1f)\n\r",AL-heading,dL);
00337             }
00338             if (dR > 20 && dR < 200)
00339             {    
00340                 sendCmd('A',AR);
00341                 sendCmd('D',dR);
00342                 dist_list[slam_ind] = dL;
00343                 angle_list[slam_ind] = AR-heading;
00344                 if (slam_ind < 63) slam_ind++;
00345                 pc.printf("(%.1f,%.1f)\n\r",AR-heading,dR);
00346             }
00347             angle = angle + 0.05;
00348         }
00349         angle = 1;
00350     }
00351     else if (angle > 0.5) //Set to max angle and execute backward sweep.
00352     {
00353         angle = 1;
00354         wait(.2);
00355         while( angle > 0 && angle <= 1)
00356         {
00357             dL = ping(1);
00358             dR = ping(2);
00359             AL = angleAdjust(heading + (angle-0.5)*55 - 45);
00360             AR = angleAdjust(heading + (angle-0.5)*55 + 45);
00361             if (dL > 20 && dL < 200)
00362             {
00363                 sendCmd('A',AL);
00364                 sendCmd('D',dL);
00365                 dist_list[slam_ind] = dL;
00366                 angle_list[slam_ind] = AL-heading;
00367                 if (slam_ind < 63) slam_ind++;
00368                 pc.printf("(%.1f,%.1f)\n\r",AL-heading,dL);
00369             }
00370             if (dR > 20 && dR < 200)
00371             {    
00372                 sendCmd('A',AR);
00373                 sendCmd('D',dR);
00374                 dist_list[slam_ind] = dL;
00375                 angle_list[slam_ind] = AR-heading;
00376                 if (slam_ind < 63) slam_ind++;
00377                 pc.printf("(%.1f,%.1f)\n\r",AR-heading,dR);
00378             }
00379             angle = angle - 0.05;
00380         }
00381         angle = 0;
00382     }
00383     sweeping = 0;
00384     __enable_irq();
00385 }
00386 
00387 void updatePos()
00388 {
00389     if(!sweeping)
00390     {
00391         float deltaX;
00392         float deltaY;
00393         float dist;
00394         while(!imu.magAvailable(X_AXIS));
00395         imu.readMag();
00396         while(!imu.accelAvailable());
00397         imu.readAccel();
00398         while(!imu.gyroAvailable());
00399         imu.readGyro();
00400         getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
00401                       imu.calcMag(imu.my), imu.calcMag(imu.mz));
00402         dist = (tickDistR + tickDistL)/2.0f*DIST_FACTOR;
00403         dist = (dirL + dirR)*dist/2.0; //Average the ticks and convert to linear distance.
00404         slam_dist += dist;
00405        // pc.printf("Magnetic Heading: %f degress    ",heading);
00406         //pc.printf("L ticks: %d   R ticks: %d       ",tickDistL, tickDistR);
00407         tickDistR = 0;//Reset the ticks.
00408         tickDistL = 0;
00409         deltaX = dist*(float)sin((double)heading*PI/180.0f); //Do trig.
00410         deltaY = dist*(float)cos((double)heading*PI/180.0f);
00411         posX = posX + deltaX;
00412         posY = posY + deltaY;
00413         //pc.printf("deltaX: %f     deltaY: %f       ",deltaX,deltaY);
00414         //pc.printf("posX: %f     posY:%f      \n\r",posX,posY);    
00415         sendCmd('X',posX);
00416         sendCmd('Y',posY);     
00417         sendCmd('H',heading);
00418     }  
00419 }
00420 
00421 void incTicksR()
00422 {
00423     ticksR++;
00424     tickDistR++;
00425 }
00426 
00427 void incTicksL()
00428 {
00429     ticksL++;
00430     tickDistL++;
00431 }
00432 
00433 void moveFwd()
00434 {
00435     dirL = 1;
00436     dirR = 1;
00437     speedL = 0.9f;
00438     speedR = 0.9f;
00439     rwheel.speed(speedR);
00440     lwheel.speed(speedL);
00441     tracking = 1; //Turn on wheel feedback updates
00442     tickDistL = 0; //Reset the distance ticks.
00443     tickDistR = 0;
00444 }
00445 
00446 void moveBwd()
00447 {
00448     dirL = -1;
00449     dirR = -1;
00450     speedL = -0.9f;
00451     speedR = -0.9f;
00452     rwheel.speed(speedR);
00453     lwheel.speed(speedL);
00454     tracking = 1;
00455     tickDistL = 0; //Reset the distance ticks.
00456     tickDistR = 0;
00457 }
00458 
00459 void moveStp()
00460 {
00461     tracking = 0; //Turn off wheel feedback updates
00462     speedL = 0;
00463     speedR = 0;
00464     lwheel.speed(0);
00465     rwheel.speed(0);
00466 }
00467 
00468 void turnL()
00469 {
00470     tracking = 0;
00471     dirL = -1;
00472     dirR = 1;
00473     speedL = -0.9;
00474     speedR = .9;
00475     lwheel.speed(speedL);
00476     rwheel.speed(speedR);
00477 }
00478 
00479 void turnR()
00480 {
00481     tracking = 0;
00482     dirL = 1;
00483     dirR = -1;
00484     speedL = 0.9;
00485     speedR = -0.9;
00486     lwheel.speed(speedL);
00487     rwheel.speed(speedR);
00488 }
00489 
00490 int slam_sweep()
00491 {
00492     //Reset the SLAM variables;
00493     memset(angle_list,0,sizeof(angle_list));
00494     memset(dist_list,0,sizeof(dist_list));
00495     slam_ind = 0;
00496     //And sweep
00497     sweep();
00498     //Iterate to determine if detection in "danger zone"
00499     for (int i = 0; i<64; i++)
00500     {
00501         if ((angle_list[i]) < 80 && (angle_list[i]) > -80)
00502         {
00503             if (dist_list[i] < 50 && dist_list[i] > 1)
00504             {
00505                 return 1;
00506                 
00507             }
00508         }
00509     }
00510     return 0;
00511 }
00512 
00513     
00514 void slam()
00515 {
00516     //Initialize arrays
00517     int detect = 0;
00518     wait(1);
00519     while(1)
00520     {
00521         while(!detect)//while an object is not within 30cm of front 60 degrees of robot...
00522         {
00523             //move forward 20cm and scan
00524             moveFwd();
00525             //while(slam_dist < 20)pc.printf("SlamDist: %f\n\r",slam_dist);
00526             wait(.8);
00527             moveStp();
00528             slam_dist = 0;
00529             detect = slam_sweep();
00530         }
00531         //Else, turn left slightly.
00532         detect = 0;
00533         turnL();
00534         wait(.8);
00535         moveStp();
00536         //and scan
00537         detect = slam_sweep();
00538     }
00539     
00540 }
00541 
00542 int main() 
00543 {
00544     char cmd;
00545     timestamp.start(); 
00546     angle = 0.0f;
00547     //Calibrate the IMU
00548     if (!imu.begin()) {
00549         pc.printf("Failed to communicate with LSM9DS1.\n");
00550     }
00551     imu.calibrate();
00552     turnR();
00553     imu.calibrateMag(0);
00554     moveStp();
00555     // Initialize hall-effect control
00556     Sampler.attach(&sampleEncoder, 0.2); //Sampler uses sampleEncoder function every 200ms
00557     EncL.mode(PullUp); // Use internal pullups
00558     EncR.mode(PullUp);
00559     EncR.rise(&incTicksR);
00560     EncL.rise(&incTicksL);
00561     memset(prevHeading, 0, sizeof(prevHeading));
00562     nav.attach(&updatePos,1);//Update X,Y,H every second
00563     while(1) 
00564     {
00565         //Check if char is ready to be read and put into command buffer;
00566         if(bt.getc() =='!')
00567         {
00568             cmd = bt.getc();
00569             switch (cmd)
00570             {
00571                 case 'B':
00572                 //pc.printf("A button was pressed!\n\r");
00573                 cmd = bt.getc();
00574                 switch(cmd)
00575                 {
00576                     case '5': //Up
00577                     cmd = bt.getc();
00578                     switch(cmd)
00579                     {
00580                         case '1': //Pressed
00581                         moveFwd();
00582                         break;
00583                         
00584                         case '0': //Released
00585                         moveStp();
00586                         sweep();
00587                         break;
00588                     }
00589                     break;
00590                     
00591                     case '6': //Down
00592                     cmd = bt.getc();
00593                     switch(cmd)
00594                     {
00595                         case '1': //Pressed
00596                         moveBwd();
00597                         break;
00598                         
00599                         case '0': //Released
00600                         moveStp();
00601                         sweep();
00602                         break;
00603                     }
00604                     break;
00605                     
00606                     case '7': //Left
00607                     cmd = bt.getc();
00608                     switch(cmd)
00609                     {
00610                         case '1': //Pressed
00611                         turnL();
00612                         break;
00613                         
00614                         case '0': //Released
00615                         moveStp();
00616                         sweep();
00617                         break;
00618                     }
00619                     break;
00620                     
00621                     case '8': //Right
00622                     cmd = bt.getc();
00623                     switch(cmd)
00624                     {
00625                         case '1': //Pressed
00626                         turnR();
00627                         break;
00628                         
00629                         case '0': //Released
00630                         moveStp();
00631                         sweep();
00632                         break;
00633                     }
00634                     break;
00635                     
00636                     case '9': //Right
00637                     cmd = bt.getc();
00638                     switch(cmd)
00639                     {
00640                         case '1': //Pressed
00641                         slam();
00642                         break;
00643                         
00644                         case '0': //Released
00645                         break;
00646                     }
00647                     break;
00648                 }
00649                 
00650                 break;
00651                 
00652                 case 'S': //Stop moving
00653                 pc.printf("Got Command STOP!\n\r");
00654                 tracking = 0; //Turn off wheel feedback updates
00655                 speedL = 0;
00656                 speedR = 0;
00657                 lwheel.speed(0);
00658                 rwheel.speed(0);
00659                 break;
00660                 
00661                 case 'F': //Forward
00662                 pc.printf("Got Command FORWARD!\n\r");
00663                 dirL = 1;
00664                 dirR = 1;
00665                 speedL = 0.9f;
00666                 speedR = 0.9f;
00667                 rwheel.speed(speedR);
00668                 lwheel.speed(speedL);
00669                 tracking = 1; //Turn on wheel feedback updates
00670                 tickDistL = 0; //Reset the distance ticks.
00671                 tickDistR = 0;
00672                 break;
00673                 
00674                 case 'R': //Reverse
00675                 pc.printf("Got Command REVERSE!\n\r");
00676                 dirL = -1;
00677                 dirR = -1;
00678                 speedL = -0.9f;
00679                 speedR = -0.9f;
00680                 rwheel.speed(speedR);
00681                 lwheel.speed(speedL);
00682                 tracking = 1;
00683                 tickDistL = 0; //Reset the distance ticks.
00684                 tickDistR = 0;
00685                 break;
00686                 
00687                 case 'P': //Sweep
00688                 pc.printf("Got Command SWEEP!\n\r");
00689                 sweep();
00690                 break;
00691                 
00692                 default:
00693                 pc.printf("Unknown Command!\n\r");
00694                 break;
00695                 
00696             }
00697         }
00698     }
00699 }