main.cpp

00001 /*/least updates:
00002 this program is based on exploration 7 which is PI controller on theta only.
00003 ##done steps:
00004 -------------
00005 1)avoiding obstacle.
00006 2)switching modes must be in the navigation function.
00007 3)case if Obs_opp=0.
00008 4)line 413 if Robs=0 or Lobs=0 must be =1.5
00009 
00010 ##next steps to be done:
00011 -----------------------
00012 1)position of Xtow.
00013 2)dot product check line 431&459.
00014 3)exiting conditions check.
00015 *///
00016 /**
00017  * this algorithm is Exploration algo.
00018  */
00019 #include "Motor.h"
00020 #include "QEI.h"
00021 #include"math.h"
00022 #include "SpaceSensor.h"
00023 #include "PID.h"
00024 #include "SHARPIR.h"
00025 
00026 //#define PULSES_PER_REVOLUTION 624
00027 #define RATE_PULSES 0.01        //PID loop timing
00028 
00029 //PID tuning constants.
00030 #define L_KC 1.5048 //Forward left motor Kc
00031 #define L_TI 0.1    //Forward left motor Ti
00032 #define L_TD 0.0    //Forward left motor Td
00033 #define R_KC 1.2716 //Forward right motor Kc
00034 #define R_TI 0.1    //Forward right motor Ti
00035 #define R_TD 0.0    //Forward right motor Td
00036 
00037 //PID input/output limits.
00038 #define L_INPUT_MIN 0     //Forward left motor minimum input
00039 #define L_INPUT_MAX 3000  //Forward left motor maximum input
00040 #define L_OUTPUT_MIN 0.0  //Forward left motor minimum output
00041 #define L_OUTPUT_MAX 0.9  //Forward left motor maximum output
00042 
00043 #define R_INPUT_MIN 0     //Forward right motor input minimum
00044 #define R_INPUT_MAX 3000  //Forward right motor input maximum
00045 #define R_OUTPUT_MIN 0.0  //Forward right motor output minimum
00046 #define R_OUTPUT_MAX 0.9  //Forward right motor output maximum
00047 
00048 #define PPDFactor 0.83638870665653517347249697967419 //pulse per Distance Factor
00049 #define LPPD 84.5374/PPDFactor //Left Pulse Per Distance
00050 #define RPPD 84.0804/PPDFactor //Left Pulse Per Distance
00051 
00052 #define delta 55
00053 
00054 #define navigationtimeinterrupt 1
00055 //functions identification
00056 void read_string();
00057 void motor_intialize();
00058 void initializePid(void);
00059 void PID_QEI_calculations();
00060 void PWM_cal();
00061 void Distance_calculation();
00062 void Get_Window(int j,float *vec,float *vec2);
00063 void Energy_cal(float *Energy,float *W,float Threshold1[3]);
00064 void angle_cal(float *theta,float *theta_dot,float *E,float Threshold[3]);
00065 void move(float Lperiod,float Rperiod);
00066 void navigation_model();
00067 void RotateLeft(float Lperiod,float Rperiod);
00068 void RotateRight(float Lperiod,float Rperiod);
00069 void Stop();
00070 void rotation();
00071 void errorfun();
00072 void GTG_mode();
00073 void FollowWall();
00074 void First_FW();
00075 void Wall_Following();
00076 void min_Obs_opp();
00077 
00078 DigitalOut myled(LED1);
00079 Serial xbee(p9, p10); //Creates a variable for serial comunication through pin 28 and 27
00080 SHARPIR IR_front(p20); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
00081 SHARPIR IR_right(p19); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
00082 SHARPIR IR_left(p18); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
00083 
00084 Motor leftFrontMotor(p21, p16, p15);  //pwm, inB, inA
00085 Motor rightFrontMotor(p22, p12, p11); //pwm, inA, inB
00086 Motor leftBackMotor(p23, p8, p7);  //pwm, inB, inA
00087 Motor rightBackMotor(p24, p6, p5); //pwm, inA, inB
00088 
00089 QEI leftBackQei(p25, p26, NC, 151);  //chanA, chanB, index, ppr
00090 QEI rightBackQei(p29, p30, NC, 147); //chanB, chanA, index, ppr
00091 //Tuning parameters calculated from step tests;
00092 //see http://mbed.org/cookbook/PID for examples.
00093 PID leftPid(L_KC, L_TI, L_TD, RATE_PULSES);  //Kc, Ti=0.01, Td=0.005
00094 PID rightPid(R_KC, R_TI, R_TD, RATE_PULSES ); //Kc, Ti, Td
00095 SpaceSensor SS(p13, p14);
00096 //Left motor working variables.
00097 int leftPulses     = 0;
00098 int leftPrevPulses = 0;
00099 float leftVelocity = 0.0;
00100 
00101 //Right motor working variables.
00102 int rightPulses     = 0;
00103 int rightPrevPulses = 0;
00104 float rightVelocity = 0.0;
00105 
00106 //angle parameters.
00107 float sfgyro=2;
00108 float FIRtheta[3],FIR_W[3],FIR_E[3],sigmoid,error,errorI=0,LTP,RTP;
00109 float FIR_Thresh[3]= {0.00145,0.000674,0.006452};
00110 float EulerAngles[3];
00111 float compass[3];
00112 
00113 bool theta_update;
00114 float LPWM,RPWM;
00115 float dis=0,rightDis,leftDis;
00116 
00117 // PID set points parameters.
00118 int LQEI_SetPoint=3000,RQEI_SetPoint=3000;
00119 
00120 //read string parameters
00121 float X,Y,R,Theta_base_st,Fobstacles_dis,Lobstacles_dis,Robstacles_dis;
00122 char Ar_st[30];
00123 char X_coord[10];
00124 char Y_coord[10];
00125 char st[128];
00126 
00127 //navigation model parameters
00128 float prev_dis[3];
00129 float post_dis[3];
00130 float current_theta,rot_theta;
00131 
00132 //Obasticle avoidance Parameters
00133 float current_Obs_dis,Obs_h,gradient_Obs;
00134 int Obs_factor;
00135 bool FWM,GTGM,ccw,cw;
00136 float Obs_adj,Obs_opp,Obs_hyp,Obs_dis,Obs_alpha_opp,Obs_alpha,FWtheta,Xtow;
00137 float GTGtheta,dot_product_cond;
00138 float h_Obsinv,error_inv;
00139 //time parameters
00140 Timer t;
00141 Ticker timer,timer2;
00142 
00143 void get_theta()
00144 {
00145     angle_cal(FIRtheta,FIR_W,FIR_E,FIR_Thresh);
00146     SS.ReadfilteredtaredEulerAngles(EulerAngles);
00147     SS.Readcompass(compass);
00148     Fobstacles_dis=IR_front.cm();//low range will return -1.0& for far range will return 0.
00149     Lobstacles_dis=IR_left.cm();
00150     Robstacles_dis=IR_right.cm();
00151 }
00152 
00153 int main()
00154 {
00155     motor_intialize();
00156     initializePid();
00157     //Velocity to mantain in pulses per second.
00158     leftPid.setSetPoint(LQEI_SetPoint);//set this number by experementally//
00159     rightPid.setSetPoint(RQEI_SetPoint);//set this number by experementally//
00160     FWM=false;
00161     GTGM=true;
00162     ccw=false;
00163     cw=false;
00164     while(1) { //big fat loop
00165         char datain;
00166         xbee.puts("press B to begin...");
00167         datain=xbee.getc();
00168         if(datain=='b'||datain=='B') {
00169             t.reset();
00170             t.start();
00171             xbee.puts("enter coordinates in the form of BX'x'Y'y'E...\r\n");
00172             read_string();
00173             rot_theta=Theta_base_st;
00174             navigation_model();
00175             timer.attach(&get_theta, 0.2);
00176             timer2.attach(&navigation_model, navigationtimeinterrupt);
00177 
00178             if(theta_update)
00179                 rotation();
00180             rot_theta=0;
00181             FIRtheta[0]=FIRtheta[0]-Theta_base_st;
00182             errorI=0;
00183             while(1) {//////////////////////////////////////////////////////////////////////////////////////////////////////////////in diffrent modes
00184                 GTG_mode();
00185                 if(abs(R-(dis+prev_dis[0]))<3)break;
00186             }//whileloop
00187             timer.detach();
00188             timer2.detach();
00189             Stop();
00190             sprintf( st ,"final:distance%f lp:%d rp:%d %f\r\n",abs((dis+prev_dis[0])),leftPulses,rightPulses,t.read());
00191             xbee.puts(st);
00192 
00193         }//if condition loop enter b
00194     }//big fat loop
00195 }//main loop
00196 
00197 void read_string()
00198 {
00199     int i=0;
00200     char c;
00201     c=xbee.getc();
00202     if(c=='B') {
00203         while(1) {
00204             c=xbee.getc();
00205             if(c=='E')break;
00206             Ar_st[i]=c;
00207             i++;
00208             myled != myled;
00209 
00210         }
00211 
00212         int count=0,j=0;
00213         if(Ar_st[j]=='X') {
00214             while(1) {
00215                 j++;
00216                 if(Ar_st[j]=='Y')break;
00217                 X_coord[count]=Ar_st[j];
00218                 count++;
00219             }
00220             X=atof(X_coord);
00221             count=0;
00222         }
00223         if(Ar_st[j]=='Y') {
00224             while(1) {
00225                 j++;
00226                 Y_coord[count]=Ar_st[j];
00227                 count++;
00228                 if(j==i)break;
00229             }
00230             Y=atof(Y_coord);
00231             count=0;
00232         }
00233     }
00234     xbee.puts("complete");
00235     R=sqrt(X*X+Y*Y);
00236     Theta_base_st=atan2(X,Y);
00237     sprintf( st ,"Rover will move %f cm & %f cm \r\n",X,Y);
00238     xbee.puts(st);
00239     sprintf( st ,"Rover will move %f cm & %f Degree\r\n",R,Theta_base_st);
00240     xbee.puts(st);
00241 
00242 }
00243 void motor_intialize()
00244 {
00245     leftFrontMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
00246     rightFrontMotor.period(0.00005);
00247     leftBackMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
00248     rightBackMotor.period(0.00005);
00249 }
00250 
00251 void initializePid(void)
00252 {
00253     leftPid.setInputLimits(L_INPUT_MIN, L_INPUT_MAX);
00254     leftPid.setOutputLimits(L_OUTPUT_MIN, L_OUTPUT_MAX);
00255     leftPid.setMode(AUTO_MODE);
00256     rightPid.setInputLimits(R_INPUT_MIN, R_INPUT_MAX);
00257     rightPid.setOutputLimits(R_OUTPUT_MIN, R_OUTPUT_MAX);
00258     rightPid.setMode(AUTO_MODE);
00259 
00260 }
00261 
00262 void PID_QEI_calculations()
00263 {
00264     leftPulses = leftBackQei.getPulses();
00265     leftVelocity = (leftPulses - leftPrevPulses) / RATE_PULSES;
00266     leftPrevPulses = leftPulses;//Use the absolute value of velocity as the PID controller works in the % (unsigned) domain and will get confused with -ve values.
00267     leftPid.setProcessValue(fabs(leftVelocity));
00268 
00269     rightPulses = rightBackQei.getPulses();
00270     rightVelocity = (rightPulses - rightPrevPulses) / RATE_PULSES;
00271     rightPrevPulses = rightPulses;
00272     rightPid.setProcessValue(fabs(rightVelocity));
00273 }
00274 void errorfun()
00275 {
00276     error=current_theta-FIRtheta[0];
00277     errorI=error+errorI;
00278     sprintf( st ,"error:%f errprI:%f\r\n",error,errorI);
00279     xbee.puts(st);
00280     float h;
00281     h=6*error+0.01*errorI;
00282     sigmoid=4/(1+exp(h))-2;
00283     if(sigmoid<0) {
00284         RTP=1+sigmoid;
00285         LTP=1;
00286     } else if(sigmoid>0) {
00287         RTP=1;
00288         LTP=1-sigmoid;
00289     } else {
00290         RTP=1;
00291         LTP=1;
00292     }
00293 }
00294 void PWM_cal()
00295 {
00296     errorfun();
00297     RPWM=/*rightPid.compute()*/ 0.9*RTP;
00298     LPWM=/*leftPid.compute()*/ 0.9*LTP;
00299 }
00300 void Distance_calculation()
00301 {
00302     leftPulses  = leftBackQei.getPulses();
00303     rightPulses = rightBackQei.getPulses();
00304     leftDis  = (leftPulses)*2*3.14159*3/LPPD;
00305     rightDis = (rightPulses)*2*3.14159*3/RPPD;
00306     dis= (rightDis+leftDis)/2;
00307 }
00308 void Get_Window(int j,float *vec,float *vec2)
00309 {
00310     float vector1[3]= {0,0,0};
00311     for(int i=0; i<3; i++) {
00312         vec[i]=0;
00313         vec2[i]=0;
00314     }
00315     int k=0;
00316     while(k<j) {
00317         SS.Readgyros(vector1);
00318         //SS.Readaccelerometer(vector2);
00319         for(int i=0; i<3; i++) {
00320             vec[i]=vector1[i]*vector1[i]+vec[i];
00321         }
00322         for(int i=0; i<3; i++) {
00323             vec2[i]=vector1[i]+vec2[i];
00324         }
00325         k++;
00326     }
00327     for(int i=0; i<3; i++) {
00328         vec[i]=vec[i]/(j);
00329     }
00330     for(int i=0; i<3; i++) {
00331         vec2[i]=vec2[i]/(j);
00332     }
00333 }
00334 void Energy_cal(float *Energy,float *W,float Threshold1[3])
00335 {
00336     Get_Window(10,Energy,W);
00337     for(int i=0; i<3; i++) {
00338         if(Energy[i]<Threshold1[i]*sfgyro)W[i]=0;
00339     }
00340     for(int i=0; i<3; i++)W[i]=W[i];
00341 }
00342 void angle_cal(float *theta,float *theta_dot,float *E,float Threshold[3])
00343 {
00344     Energy_cal(E,theta_dot,Threshold);
00345     theta[0]=theta_dot[0]*(0.2)+theta[0];
00346     theta[1]=theta_dot[1]*(0.2)+theta[1];
00347     theta[2]=theta_dot[2]*(0.2)+theta[2];
00348 }
00349 void move(float Lperiod,float Rperiod)
00350 {
00351     leftFrontMotor.speed(Lperiod);
00352     leftBackMotor.speed(Lperiod);
00353 
00354     rightFrontMotor.speed(Rperiod);
00355     rightBackMotor.speed(Rperiod);
00356 }
00357 void Stop()
00358 {
00359     leftFrontMotor.brake();
00360     rightFrontMotor.brake();
00361     leftBackMotor.brake();
00362     rightBackMotor.brake();
00363 }
00364 
00365 void RotateLeft(float Lperiod,float Rperiod)
00366 {
00367     leftFrontMotor.speed(-Lperiod);
00368     leftBackMotor.speed(-Lperiod);
00369 
00370     rightFrontMotor.speed(Rperiod);
00371     rightBackMotor.speed(Rperiod);
00372 }
00373 
00374 void RotateRight(float Lperiod,float Rperiod)
00375 {
00376     leftFrontMotor.speed(Lperiod);
00377     leftBackMotor.speed(Lperiod);
00378 
00379     rightFrontMotor.speed(-Rperiod);
00380     rightBackMotor.speed(-Rperiod);
00381 }
00382 //////////////////////////////////////////////////////////////////////////////////////not used yet
00383 void rotation()
00384 {
00385     timer2.detach();
00386     if(current_theta-FIRtheta[0]>FIR_Thresh[0]) {
00387         xbee.puts("rotate right\r\n");
00388         while(FIRtheta[0]<current_theta) { //new_disctrete..........................................................
00389             PID_QEI_calculations();
00390             RotateRight(leftPid.compute(),rightPid.compute());
00391             Distance_calculation();
00392         }
00393     } else if(current_theta-FIRtheta[0]<-1*FIR_Thresh[0]) {
00394         xbee.puts("rotate left\r\n");
00395         while(FIRtheta[0]>current_theta) {
00396             PID_QEI_calculations();
00397             RotateLeft(leftPid.compute(),rightPid.compute());
00398             Distance_calculation();
00399         }
00400     } else {
00401         xbee.puts("will not rotate\r\n");
00402     }
00403     theta_update=false;
00404     timer2.attach(&navigation_model, navigationtimeinterrupt);
00405 }
00406 void GTG_mode()
00407 {
00408     if(FWM)Wall_Following();
00409     PID_QEI_calculations();
00410     PWM_cal();
00411     move(LPWM,RPWM);
00412     Distance_calculation();
00413 /*    sprintf( st ,"\r\n\r\nR:%f prev:%f theta:%f GTG:%f FIR:%f Base:%f S:%f LTP:%f RTP:%f\r\n",R,prev_dis[0],current_theta*180/3.14,(current_theta-GTGtheta)*180/3.14,FIRtheta[0]*180/3.14,Theta_base_st*180/3.14,sigmoid,LTP,RTP);
00414     xbee.puts(st);
00415     sprintf( st ,"\r\n\r\nF:%f R:%f L:%f OD:%f OA:%f GO:%f ccw:%i cw:%i dot:%f Xtow:%f\r\n",Fobstacles_dis,Robstacles_dis,Lobstacles_dis,Obs_dis,Obs_alpha*180/3.14,gradient_Obs,(int)(dot_product_cond>0),(int)(post_dis[0]<Xtow),dot_product_cond,Xtow);
00416     xbee.puts(st);
00417 */
00418     sprintf( st ," theta:%f FIR:%f OA:%f GO:%f 1st:%i 2nd:%i 3rd:%i\r\n",current_theta*180/3.14,FIRtheta[0]*180/3.14,Obs_alpha*180/3.14,gradient_Obs,(int)(dot_product_cond>0),(int)(post_dis[0]<Xtow),(int)(abs(current_Obs_dis)>60));
00419     xbee.puts(st);
00420     sprintf( st ,"DA:%f GTGA:%f OD:%f F:%f R:%f L:%f COD:%f\r\n",(current_theta-GTGtheta)*180/3.14,GTGtheta*180/3.14,Obs_dis,Fobstacles_dis,Robstacles_dis,Lobstacles_dis,current_Obs_dis);
00421     xbee.puts(st);
00422 
00423 }
00424 void FollowWall()
00425 {
00426     if(Fobstacles_dis<delta&&Fobstacles_dis>0&&!(cw&&Robstacles_dis==-1||ccw&&Lobstacles_dis==-1))First_FW();
00427     //else if(FWM)Wall_Following();
00428 
00429 }
00430 void Wall_Following()
00431 {
00432     xbee.puts("\r\nWF\r\n");
00433     if(Obs_dis>0) { //clockwise
00434         current_Obs_dis=Robstacles_dis;
00435         cw=true;
00436         ccw=false;
00437         if(current_Obs_dis==0||current_Obs_dis==-1)
00438             current_Obs_dis=150;//check conditions in this line
00439     } else if(Obs_dis<0) { //anti clock wise
00440         current_Obs_dis=-1*Lobstacles_dis;
00441         cw=false;
00442         ccw=true;
00443         if(current_Obs_dis==0||current_Obs_dis==1)current_Obs_dis=-150;//check conditions in this line
00444     }
00445 
00446     Obs_h=current_Obs_dis-Obs_dis;
00447     gradient_Obs=2/(1+exp(-0.05*Obs_h))-1;
00448     //current_theta=current_theta+3.14*(gradient_Obs);//from here we shall begin.
00449     h_Obsinv=-log(4/(gradient_Obs+2)-1);
00450     error_inv=(h_Obsinv-0.01*errorI)/6;
00451     current_theta=error_inv+current_theta;
00452 //we should write here exiting condition.
00453     GTGtheta=atan2(post_dis[2],post_dis[1]);
00454     dot_product_cond=cos(current_theta-GTGtheta);
00455     if((dot_product_cond>0)&&(post_dis[0]<Xtow)&&(abs(current_Obs_dis)>60)) {
00456         FWM=false;
00457         GTGM=true;
00458     }
00459 }
00460 void First_FW()
00461 {
00462     xbee.puts("\r\nFFW\r\n");
00463     Xtow=post_dis[0];//check if there is a proper plase to put Xtow more than this
00464     Obs_adj=Fobstacles_dis;
00465     min_Obs_opp();
00466     Obs_hyp=sqrt(Obs_adj*Obs_adj+Obs_opp*Obs_opp);
00467     Obs_dis=Obs_adj*Obs_opp/Obs_hyp;
00468 
00469     Obs_alpha_opp=sqrt(Obs_adj*Obs_adj-Obs_dis*Obs_dis);
00470     Obs_alpha=atan2(Obs_alpha_opp,Obs_dis);
00471 
00472     if(Obs_alpha<3.1415/2)
00473         Obs_alpha= Obs_alpha-1.57079;
00474     else if(Obs_alpha>3.1415/2)
00475         Obs_alpha= 3.1415-Obs_alpha;
00476     if(Obs_alpha<0.0698&&Obs_alpha>-1*0.0698)
00477         Obs_alpha=1.57079;
00478     current_theta=current_theta+Obs_alpha;//FWtheta;
00479     if(Obs_dis==0)Obs_dis=Obs_adj;//done.
00480 
00481     FWM=true;
00482     GTGM=false;
00483     //exiting conditions.
00484     GTGtheta=atan2(post_dis[2],post_dis[1]);
00485     dot_product_cond=cos(current_theta-GTGtheta);//dot product check.
00486     if((dot_product_cond>0)&&(post_dis[0]<Xtow)&&(abs(current_Obs_dis)>60)) {
00487         FWM=false;
00488         GTGM=true;
00489     }
00490 }
00491 void min_Obs_opp()
00492 {
00493     if(Robstacles_dis<0&&Lobstacles_dis<0)Obs_opp=0;
00494     else if(Robstacles_dis<0&&Lobstacles_dis>0)Obs_opp=-1*Lobstacles_dis;
00495     else if(Robstacles_dis>0&&Lobstacles_dis<0)Obs_opp=Robstacles_dis;
00496     else if(Robstacles_dis>0&&Lobstacles_dis>0)Obs_opp = Lobstacles_dis>Robstacles_dis ? Robstacles_dis : -1*Lobstacles_dis;
00497 }
00498 void navigation_model()   //edit here
00499 {       
00500     prev_dis[1]=prev_dis[1]+dis*cos(FIRtheta[0]);
00501     prev_dis[2]=prev_dis[2]+dis*sin(FIRtheta[0]);
00502     prev_dis[0]=sqrt(prev_dis[1]*prev_dis[1]+prev_dis[2]*prev_dis[2]);
00503 
00504     post_dis[1]=R*cos(rot_theta)-prev_dis[1];
00505     post_dis[2]=R*sin(rot_theta)-prev_dis[2];
00506     post_dis[0]=sqrt(post_dis[1]*post_dis[1]+post_dis[2]*post_dis[2]);
00507     if(FWM&&!GTGM||Fobstacles_dis<delta&&Fobstacles_dis>0&&post_dis[0]>Fobstacles_dis)//check conditions in this line
00508         FollowWall();
00509     if(GTGM&&!FWM) {
00510         current_theta=atan2(post_dis[2],post_dis[1]);
00511         Obs_alpha=0;
00512         ccw=false;
00513         cw=false;
00514         xbee.puts("\r\nGTG\r\n");
00515     }
00516     leftBackQei.reset();
00517     rightBackQei.reset();
00518     dis=0;
00519     theta_update=true;
00520 }