quadcopter cufe
/
FollowWall
follow wall 1
Diff: main.cpp
- Revision:
- 0:b703833f6795
diff -r 000000000000 -r b703833f6795 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Apr 08 13:23:06 2014 +0000 @@ -0,0 +1,520 @@ +/*/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; +}