quadcopter cufe
Go to goal robot with follow wall algo
Diff: main.cpp
- Revision:
- 0:efef62b55c86
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 08 13:21:17 2014 +0000
@@ -0,0 +1,473 @@
+/*/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 "SHARPIR.h"
+
+
+#define PPDFactor 0.83638870665653517347249697967419*0.8349813227862008 //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 acquizitiontimeinterrupt 0.2
+#define navigationtimeinterrupt 0.6
+//functions identification
+void read_string();
+void motor_intialize();
+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();
+void reset_parameters();
+
+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.
+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;
+
+
+//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();
+ 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, acquizitiontimeinterrupt);
+ 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();
+ reset_parameters();
+ 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 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= 0.9*RTP;
+ LPWM= 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..........................................................
+ RotateRight(0.9,0.9);
+ Distance_calculation();
+ }
+ } else if(current_theta-FIRtheta[0]<-1*FIR_Thresh[0]) {
+// xbee.puts("rotate left\r\n");
+ while(FIRtheta[0]>current_theta) {
+ RotateLeft(0.9,0.9);
+ 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();
+ 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);
+*/
+//sprintf( st ,"\r\n\r\nR:%f prev:%f theta:%f FIR:%f Base:%f S:%f LTP:%f RTP:%f\r\n",R,prev_dis[0],current_theta*180/3.14,FIRtheta[0]*180/3.14,Theta_base_st*180/3.14,sigmoid,leftDis,rightDis);
+//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;
+}
+void reset_parameters()
+{
+}