quadcopter cufe
/
FollowWall
follow wall 1
main.cpp
- Committer:
- khaledelmadawi
- Date:
- 2014-04-08
- Revision:
- 0:b703833f6795
File content as of revision 0:b703833f6795:
/*/least updates: this program is based on exploration 7 which is PI controller on theta only. ##done steps: ------------- 1)avoiding obstacle. 2)switching modes must be in the navigation function. 3)case if Obs_opp=0. 4)line 413 if Robs=0 or Lobs=0 must be =1.5 ##next steps to be done: ----------------------- 1)position of Xtow. 2)dot product check line 431&459. 3)exiting conditions check. */// /** * this algorithm is Exploration algo. */ #include "Motor.h" #include "QEI.h" #include"math.h" #include "SpaceSensor.h" #include "PID.h" #include "SHARPIR.h" //#define PULSES_PER_REVOLUTION 624 #define RATE_PULSES 0.01 //PID loop timing //PID tuning constants. #define L_KC 1.5048 //Forward left motor Kc #define L_TI 0.1 //Forward left motor Ti #define L_TD 0.0 //Forward left motor Td #define R_KC 1.2716 //Forward right motor Kc #define R_TI 0.1 //Forward right motor Ti #define R_TD 0.0 //Forward right motor Td //PID input/output limits. #define L_INPUT_MIN 0 //Forward left motor minimum input #define L_INPUT_MAX 3000 //Forward left motor maximum input #define L_OUTPUT_MIN 0.0 //Forward left motor minimum output #define L_OUTPUT_MAX 0.9 //Forward left motor maximum output #define R_INPUT_MIN 0 //Forward right motor input minimum #define R_INPUT_MAX 3000 //Forward right motor input maximum #define R_OUTPUT_MIN 0.0 //Forward right motor output minimum #define R_OUTPUT_MAX 0.9 //Forward right motor output maximum #define PPDFactor 0.83638870665653517347249697967419 //pulse per Distance Factor #define LPPD 84.5374/PPDFactor //Left Pulse Per Distance #define RPPD 84.0804/PPDFactor //Left Pulse Per Distance #define delta 55 #define navigationtimeinterrupt 1 //functions identification void read_string(); void motor_intialize(); void initializePid(void); void PID_QEI_calculations(); void PWM_cal(); void Distance_calculation(); void Get_Window(int j,float *vec,float *vec2); void Energy_cal(float *Energy,float *W,float Threshold1[3]); void angle_cal(float *theta,float *theta_dot,float *E,float Threshold[3]); void move(float Lperiod,float Rperiod); void navigation_model(); void RotateLeft(float Lperiod,float Rperiod); void RotateRight(float Lperiod,float Rperiod); void Stop(); void rotation(); void errorfun(); void GTG_mode(); void FollowWall(); void First_FW(); void Wall_Following(); void min_Obs_opp(); DigitalOut myled(LED1); Serial xbee(p9, p10); //Creates a variable for serial comunication through pin 28 and 27 SHARPIR IR_front(p20); //the output of the sharpIR sensor is connected to the MBEDs pin 10. SHARPIR IR_right(p19); //the output of the sharpIR sensor is connected to the MBEDs pin 10. SHARPIR IR_left(p18); //the output of the sharpIR sensor is connected to the MBEDs pin 10. Motor leftFrontMotor(p21, p16, p15); //pwm, inB, inA Motor rightFrontMotor(p22, p12, p11); //pwm, inA, inB Motor leftBackMotor(p23, p8, p7); //pwm, inB, inA Motor rightBackMotor(p24, p6, p5); //pwm, inA, inB QEI leftBackQei(p25, p26, NC, 151); //chanA, chanB, index, ppr QEI rightBackQei(p29, p30, NC, 147); //chanB, chanA, index, ppr //Tuning parameters calculated from step tests; //see http://mbed.org/cookbook/PID for examples. PID leftPid(L_KC, L_TI, L_TD, RATE_PULSES); //Kc, Ti=0.01, Td=0.005 PID rightPid(R_KC, R_TI, R_TD, RATE_PULSES ); //Kc, Ti, Td SpaceSensor SS(p13, p14); //Left motor working variables. int leftPulses = 0; int leftPrevPulses = 0; float leftVelocity = 0.0; //Right motor working variables. int rightPulses = 0; int rightPrevPulses = 0; float rightVelocity = 0.0; //angle parameters. float sfgyro=2; float FIRtheta[3],FIR_W[3],FIR_E[3],sigmoid,error,errorI=0,LTP,RTP; float FIR_Thresh[3]= {0.00145,0.000674,0.006452}; float EulerAngles[3]; float compass[3]; bool theta_update; float LPWM,RPWM; float dis=0,rightDis,leftDis; // PID set points parameters. int LQEI_SetPoint=3000,RQEI_SetPoint=3000; //read string parameters float X,Y,R,Theta_base_st,Fobstacles_dis,Lobstacles_dis,Robstacles_dis; char Ar_st[30]; char X_coord[10]; char Y_coord[10]; char st[128]; //navigation model parameters float prev_dis[3]; float post_dis[3]; float current_theta,rot_theta; //Obasticle avoidance Parameters float current_Obs_dis,Obs_h,gradient_Obs; int Obs_factor; bool FWM,GTGM,ccw,cw; float Obs_adj,Obs_opp,Obs_hyp,Obs_dis,Obs_alpha_opp,Obs_alpha,FWtheta,Xtow; float GTGtheta,dot_product_cond; float h_Obsinv,error_inv; //time parameters Timer t; Ticker timer,timer2; void get_theta() { angle_cal(FIRtheta,FIR_W,FIR_E,FIR_Thresh); SS.ReadfilteredtaredEulerAngles(EulerAngles); SS.Readcompass(compass); Fobstacles_dis=IR_front.cm();//low range will return -1.0& for far range will return 0. Lobstacles_dis=IR_left.cm(); Robstacles_dis=IR_right.cm(); } int main() { motor_intialize(); initializePid(); //Velocity to mantain in pulses per second. leftPid.setSetPoint(LQEI_SetPoint);//set this number by experementally// rightPid.setSetPoint(RQEI_SetPoint);//set this number by experementally// FWM=false; GTGM=true; ccw=false; cw=false; while(1) { //big fat loop char datain; xbee.puts("press B to begin..."); datain=xbee.getc(); if(datain=='b'||datain=='B') { t.reset(); t.start(); xbee.puts("enter coordinates in the form of BX'x'Y'y'E...\r\n"); read_string(); rot_theta=Theta_base_st; navigation_model(); timer.attach(&get_theta, 0.2); timer2.attach(&navigation_model, navigationtimeinterrupt); if(theta_update) rotation(); rot_theta=0; FIRtheta[0]=FIRtheta[0]-Theta_base_st; errorI=0; while(1) {//////////////////////////////////////////////////////////////////////////////////////////////////////////////in diffrent modes GTG_mode(); if(abs(R-(dis+prev_dis[0]))<3)break; }//whileloop timer.detach(); timer2.detach(); Stop(); sprintf( st ,"final:distance%f lp:%d rp:%d %f\r\n",abs((dis+prev_dis[0])),leftPulses,rightPulses,t.read()); xbee.puts(st); }//if condition loop enter b }//big fat loop }//main loop void read_string() { int i=0; char c; c=xbee.getc(); if(c=='B') { while(1) { c=xbee.getc(); if(c=='E')break; Ar_st[i]=c; i++; myled != myled; } int count=0,j=0; if(Ar_st[j]=='X') { while(1) { j++; if(Ar_st[j]=='Y')break; X_coord[count]=Ar_st[j]; count++; } X=atof(X_coord); count=0; } if(Ar_st[j]=='Y') { while(1) { j++; Y_coord[count]=Ar_st[j]; count++; if(j==i)break; } Y=atof(Y_coord); count=0; } } xbee.puts("complete"); R=sqrt(X*X+Y*Y); Theta_base_st=atan2(X,Y); sprintf( st ,"Rover will move %f cm & %f cm \r\n",X,Y); xbee.puts(st); sprintf( st ,"Rover will move %f cm & %f Degree\r\n",R,Theta_base_st); xbee.puts(st); } void motor_intialize() { leftFrontMotor.period(0.00005); //Set motor PWM periods to 20KHz. rightFrontMotor.period(0.00005); leftBackMotor.period(0.00005); //Set motor PWM periods to 20KHz. rightBackMotor.period(0.00005); } void initializePid(void) { leftPid.setInputLimits(L_INPUT_MIN, L_INPUT_MAX); leftPid.setOutputLimits(L_OUTPUT_MIN, L_OUTPUT_MAX); leftPid.setMode(AUTO_MODE); rightPid.setInputLimits(R_INPUT_MIN, R_INPUT_MAX); rightPid.setOutputLimits(R_OUTPUT_MIN, R_OUTPUT_MAX); rightPid.setMode(AUTO_MODE); } void PID_QEI_calculations() { leftPulses = leftBackQei.getPulses(); leftVelocity = (leftPulses - leftPrevPulses) / RATE_PULSES; leftPrevPulses = leftPulses;//Use the absolute value of velocity as the PID controller works in the % (unsigned) domain and will get confused with -ve values. leftPid.setProcessValue(fabs(leftVelocity)); rightPulses = rightBackQei.getPulses(); rightVelocity = (rightPulses - rightPrevPulses) / RATE_PULSES; rightPrevPulses = rightPulses; rightPid.setProcessValue(fabs(rightVelocity)); } void errorfun() { error=current_theta-FIRtheta[0]; errorI=error+errorI; sprintf( st ,"error:%f errprI:%f\r\n",error,errorI); xbee.puts(st); float h; h=6*error+0.01*errorI; sigmoid=4/(1+exp(h))-2; if(sigmoid<0) { RTP=1+sigmoid; LTP=1; } else if(sigmoid>0) { RTP=1; LTP=1-sigmoid; } else { RTP=1; LTP=1; } } void PWM_cal() { errorfun(); RPWM=/*rightPid.compute()*/ 0.9*RTP; LPWM=/*leftPid.compute()*/ 0.9*LTP; } void Distance_calculation() { leftPulses = leftBackQei.getPulses(); rightPulses = rightBackQei.getPulses(); leftDis = (leftPulses)*2*3.14159*3/LPPD; rightDis = (rightPulses)*2*3.14159*3/RPPD; dis= (rightDis+leftDis)/2; } void Get_Window(int j,float *vec,float *vec2) { float vector1[3]= {0,0,0}; for(int i=0; i<3; i++) { vec[i]=0; vec2[i]=0; } int k=0; while(k<j) { SS.Readgyros(vector1); //SS.Readaccelerometer(vector2); for(int i=0; i<3; i++) { vec[i]=vector1[i]*vector1[i]+vec[i]; } for(int i=0; i<3; i++) { vec2[i]=vector1[i]+vec2[i]; } k++; } for(int i=0; i<3; i++) { vec[i]=vec[i]/(j); } for(int i=0; i<3; i++) { vec2[i]=vec2[i]/(j); } } void Energy_cal(float *Energy,float *W,float Threshold1[3]) { Get_Window(10,Energy,W); for(int i=0; i<3; i++) { if(Energy[i]<Threshold1[i]*sfgyro)W[i]=0; } for(int i=0; i<3; i++)W[i]=W[i]; } void angle_cal(float *theta,float *theta_dot,float *E,float Threshold[3]) { Energy_cal(E,theta_dot,Threshold); theta[0]=theta_dot[0]*(0.2)+theta[0]; theta[1]=theta_dot[1]*(0.2)+theta[1]; theta[2]=theta_dot[2]*(0.2)+theta[2]; } void move(float Lperiod,float Rperiod) { leftFrontMotor.speed(Lperiod); leftBackMotor.speed(Lperiod); rightFrontMotor.speed(Rperiod); rightBackMotor.speed(Rperiod); } void Stop() { leftFrontMotor.brake(); rightFrontMotor.brake(); leftBackMotor.brake(); rightBackMotor.brake(); } void RotateLeft(float Lperiod,float Rperiod) { leftFrontMotor.speed(-Lperiod); leftBackMotor.speed(-Lperiod); rightFrontMotor.speed(Rperiod); rightBackMotor.speed(Rperiod); } void RotateRight(float Lperiod,float Rperiod) { leftFrontMotor.speed(Lperiod); leftBackMotor.speed(Lperiod); rightFrontMotor.speed(-Rperiod); rightBackMotor.speed(-Rperiod); } //////////////////////////////////////////////////////////////////////////////////////not used yet void rotation() { timer2.detach(); if(current_theta-FIRtheta[0]>FIR_Thresh[0]) { xbee.puts("rotate right\r\n"); while(FIRtheta[0]<current_theta) { //new_disctrete.......................................................... PID_QEI_calculations(); RotateRight(leftPid.compute(),rightPid.compute()); Distance_calculation(); } } else if(current_theta-FIRtheta[0]<-1*FIR_Thresh[0]) { xbee.puts("rotate left\r\n"); while(FIRtheta[0]>current_theta) { PID_QEI_calculations(); RotateLeft(leftPid.compute(),rightPid.compute()); Distance_calculation(); } } else { xbee.puts("will not rotate\r\n"); } theta_update=false; timer2.attach(&navigation_model, navigationtimeinterrupt); } void GTG_mode() { if(FWM)Wall_Following(); PID_QEI_calculations(); PWM_cal(); move(LPWM,RPWM); Distance_calculation(); /* 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); xbee.puts(st); 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); xbee.puts(st); */ 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)); xbee.puts(st); 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); xbee.puts(st); } void FollowWall() { if(Fobstacles_dis<delta&&Fobstacles_dis>0&&!(cw&&Robstacles_dis==-1||ccw&&Lobstacles_dis==-1))First_FW(); //else if(FWM)Wall_Following(); } void Wall_Following() { xbee.puts("\r\nWF\r\n"); if(Obs_dis>0) { //clockwise current_Obs_dis=Robstacles_dis; cw=true; ccw=false; if(current_Obs_dis==0||current_Obs_dis==-1) current_Obs_dis=150;//check conditions in this line } else if(Obs_dis<0) { //anti clock wise current_Obs_dis=-1*Lobstacles_dis; cw=false; ccw=true; if(current_Obs_dis==0||current_Obs_dis==1)current_Obs_dis=-150;//check conditions in this line } Obs_h=current_Obs_dis-Obs_dis; gradient_Obs=2/(1+exp(-0.05*Obs_h))-1; //current_theta=current_theta+3.14*(gradient_Obs);//from here we shall begin. h_Obsinv=-log(4/(gradient_Obs+2)-1); error_inv=(h_Obsinv-0.01*errorI)/6; current_theta=error_inv+current_theta; //we should write here exiting condition. GTGtheta=atan2(post_dis[2],post_dis[1]); dot_product_cond=cos(current_theta-GTGtheta); if((dot_product_cond>0)&&(post_dis[0]<Xtow)&&(abs(current_Obs_dis)>60)) { FWM=false; GTGM=true; } } void First_FW() { xbee.puts("\r\nFFW\r\n"); Xtow=post_dis[0];//check if there is a proper plase to put Xtow more than this Obs_adj=Fobstacles_dis; min_Obs_opp(); Obs_hyp=sqrt(Obs_adj*Obs_adj+Obs_opp*Obs_opp); Obs_dis=Obs_adj*Obs_opp/Obs_hyp; Obs_alpha_opp=sqrt(Obs_adj*Obs_adj-Obs_dis*Obs_dis); Obs_alpha=atan2(Obs_alpha_opp,Obs_dis); if(Obs_alpha<3.1415/2) Obs_alpha= Obs_alpha-1.57079; else if(Obs_alpha>3.1415/2) Obs_alpha= 3.1415-Obs_alpha; if(Obs_alpha<0.0698&&Obs_alpha>-1*0.0698) Obs_alpha=1.57079; current_theta=current_theta+Obs_alpha;//FWtheta; if(Obs_dis==0)Obs_dis=Obs_adj;//done. FWM=true; GTGM=false; //exiting conditions. GTGtheta=atan2(post_dis[2],post_dis[1]); dot_product_cond=cos(current_theta-GTGtheta);//dot product check. if((dot_product_cond>0)&&(post_dis[0]<Xtow)&&(abs(current_Obs_dis)>60)) { FWM=false; GTGM=true; } } void min_Obs_opp() { if(Robstacles_dis<0&&Lobstacles_dis<0)Obs_opp=0; else if(Robstacles_dis<0&&Lobstacles_dis>0)Obs_opp=-1*Lobstacles_dis; else if(Robstacles_dis>0&&Lobstacles_dis<0)Obs_opp=Robstacles_dis; else if(Robstacles_dis>0&&Lobstacles_dis>0)Obs_opp = Lobstacles_dis>Robstacles_dis ? Robstacles_dis : -1*Lobstacles_dis; } void navigation_model() //edit here { prev_dis[1]=prev_dis[1]+dis*cos(FIRtheta[0]); prev_dis[2]=prev_dis[2]+dis*sin(FIRtheta[0]); prev_dis[0]=sqrt(prev_dis[1]*prev_dis[1]+prev_dis[2]*prev_dis[2]); post_dis[1]=R*cos(rot_theta)-prev_dis[1]; post_dis[2]=R*sin(rot_theta)-prev_dis[2]; post_dis[0]=sqrt(post_dis[1]*post_dis[1]+post_dis[2]*post_dis[2]); if(FWM&&!GTGM||Fobstacles_dis<delta&&Fobstacles_dis>0&&post_dis[0]>Fobstacles_dis)//check conditions in this line FollowWall(); if(GTGM&&!FWM) { current_theta=atan2(post_dis[2],post_dis[1]); Obs_alpha=0; ccw=false; cw=false; xbee.puts("\r\nGTG\r\n"); } leftBackQei.reset(); rightBackQei.reset(); dis=0; theta_update=true; }