quadcopter cufe / Mbed 2 deprecated FollowWall2

Go to goal robot with follow wall algo

Dependencies:   mbed

main.cpp@0:efef62b55c86, 2014-04-08 (annotated)

Committer:
khaledelmadawi
Date:
Tue Apr 08 13:21:17 2014 +0000
Revision:
0:efef62b55c86
Follow wall

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaledelmadawi 0:efef62b55c86 1 /*/least updates:
khaledelmadawi 0:efef62b55c86 2 this program is based on exploration 7 which is PI controller on theta only.
khaledelmadawi 0:efef62b55c86 3 ##done steps:
khaledelmadawi 0:efef62b55c86 4 -------------
khaledelmadawi 0:efef62b55c86 5 1)avoiding obstacle.
khaledelmadawi 0:efef62b55c86 6 2)switching modes must be in the navigation function.
khaledelmadawi 0:efef62b55c86 7 3)case if Obs_opp=0.
khaledelmadawi 0:efef62b55c86 8 4)line 413 if Robs=0 or Lobs=0 must be =1.5
khaledelmadawi 0:efef62b55c86 9
khaledelmadawi 0:efef62b55c86 10 ##next steps to be done:
khaledelmadawi 0:efef62b55c86 11 -----------------------
khaledelmadawi 0:efef62b55c86 12 1)position of Xtow.
khaledelmadawi 0:efef62b55c86 13 2)dot product check line 431&459.
khaledelmadawi 0:efef62b55c86 14 3)exiting conditions check.
khaledelmadawi 0:efef62b55c86 15 *///
khaledelmadawi 0:efef62b55c86 16 /**
khaledelmadawi 0:efef62b55c86 17 * this algorithm is Exploration algo.
khaledelmadawi 0:efef62b55c86 18 */
khaledelmadawi 0:efef62b55c86 19 #include "Motor.h"
khaledelmadawi 0:efef62b55c86 20 #include "QEI.h"
khaledelmadawi 0:efef62b55c86 21 #include"math.h"
khaledelmadawi 0:efef62b55c86 22 #include "SpaceSensor.h"
khaledelmadawi 0:efef62b55c86 23 #include "SHARPIR.h"
khaledelmadawi 0:efef62b55c86 24
khaledelmadawi 0:efef62b55c86 25
khaledelmadawi 0:efef62b55c86 26 #define PPDFactor 0.83638870665653517347249697967419*0.8349813227862008 //pulse per Distance Factor
khaledelmadawi 0:efef62b55c86 27 #define LPPD 84.5374/PPDFactor //Left Pulse Per Distance
khaledelmadawi 0:efef62b55c86 28 #define RPPD 84.0804/PPDFactor //Left Pulse Per Distance
khaledelmadawi 0:efef62b55c86 29
khaledelmadawi 0:efef62b55c86 30 #define delta 55
khaledelmadawi 0:efef62b55c86 31
khaledelmadawi 0:efef62b55c86 32 #define acquizitiontimeinterrupt 0.2
khaledelmadawi 0:efef62b55c86 33 #define navigationtimeinterrupt 0.6
khaledelmadawi 0:efef62b55c86 34 //functions identification
khaledelmadawi 0:efef62b55c86 35 void read_string();
khaledelmadawi 0:efef62b55c86 36 void motor_intialize();
khaledelmadawi 0:efef62b55c86 37 void PWM_cal();
khaledelmadawi 0:efef62b55c86 38 void Distance_calculation();
khaledelmadawi 0:efef62b55c86 39 void Get_Window(int j,float *vec,float *vec2);
khaledelmadawi 0:efef62b55c86 40 void Energy_cal(float *Energy,float *W,float Threshold1[3]);
khaledelmadawi 0:efef62b55c86 41 void angle_cal(float *theta,float *theta_dot,float *E,float Threshold[3]);
khaledelmadawi 0:efef62b55c86 42 void move(float Lperiod,float Rperiod);
khaledelmadawi 0:efef62b55c86 43 void navigation_model();
khaledelmadawi 0:efef62b55c86 44 void RotateLeft(float Lperiod,float Rperiod);
khaledelmadawi 0:efef62b55c86 45 void RotateRight(float Lperiod,float Rperiod);
khaledelmadawi 0:efef62b55c86 46 void Stop();
khaledelmadawi 0:efef62b55c86 47 void rotation();
khaledelmadawi 0:efef62b55c86 48 void errorfun();
khaledelmadawi 0:efef62b55c86 49 void GTG_mode();
khaledelmadawi 0:efef62b55c86 50 void FollowWall();
khaledelmadawi 0:efef62b55c86 51 void First_FW();
khaledelmadawi 0:efef62b55c86 52 void Wall_Following();
khaledelmadawi 0:efef62b55c86 53 void min_Obs_opp();
khaledelmadawi 0:efef62b55c86 54 void reset_parameters();
khaledelmadawi 0:efef62b55c86 55
khaledelmadawi 0:efef62b55c86 56 DigitalOut myled(LED1);
khaledelmadawi 0:efef62b55c86 57 Serial xbee(p9, p10); //Creates a variable for serial comunication through pin 28 and 27
khaledelmadawi 0:efef62b55c86 58 SHARPIR IR_front(p20); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
khaledelmadawi 0:efef62b55c86 59 SHARPIR IR_right(p19); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
khaledelmadawi 0:efef62b55c86 60 SHARPIR IR_left(p18); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
khaledelmadawi 0:efef62b55c86 61
khaledelmadawi 0:efef62b55c86 62
khaledelmadawi 0:efef62b55c86 63 Motor leftFrontMotor(p21, p16, p15); //pwm, inB, inA
khaledelmadawi 0:efef62b55c86 64 Motor rightFrontMotor(p22, p12, p11); //pwm, inA, inB
khaledelmadawi 0:efef62b55c86 65 Motor leftBackMotor(p23, p8, p7); //pwm, inB, inA
khaledelmadawi 0:efef62b55c86 66 Motor rightBackMotor(p24, p6, p5); //pwm, inA, inB
khaledelmadawi 0:efef62b55c86 67
khaledelmadawi 0:efef62b55c86 68 QEI leftBackQei(p25, p26, NC, 151); //chanA, chanB, index, ppr
khaledelmadawi 0:efef62b55c86 69 QEI rightBackQei(p29, p30, NC, 147); //chanB, chanA, index, ppr
khaledelmadawi 0:efef62b55c86 70 //Tuning parameters calculated from step tests;
khaledelmadawi 0:efef62b55c86 71 //see http://mbed.org/cookbook/PID for examples.
khaledelmadawi 0:efef62b55c86 72 SpaceSensor SS(p13, p14);
khaledelmadawi 0:efef62b55c86 73 //Left motor working variables.
khaledelmadawi 0:efef62b55c86 74 int leftPulses = 0;
khaledelmadawi 0:efef62b55c86 75 int leftPrevPulses = 0;
khaledelmadawi 0:efef62b55c86 76 float leftVelocity = 0.0;
khaledelmadawi 0:efef62b55c86 77
khaledelmadawi 0:efef62b55c86 78 //Right motor working variables.
khaledelmadawi 0:efef62b55c86 79 int rightPulses = 0;
khaledelmadawi 0:efef62b55c86 80 int rightPrevPulses = 0;
khaledelmadawi 0:efef62b55c86 81 float rightVelocity = 0.0;
khaledelmadawi 0:efef62b55c86 82
khaledelmadawi 0:efef62b55c86 83 //angle parameters.
khaledelmadawi 0:efef62b55c86 84 float sfgyro=2;
khaledelmadawi 0:efef62b55c86 85 float FIRtheta[3],FIR_W[3],FIR_E[3],sigmoid,error,errorI=0,LTP,RTP;
khaledelmadawi 0:efef62b55c86 86 float FIR_Thresh[3]= {0.00145,0.000674,0.006452};
khaledelmadawi 0:efef62b55c86 87 float EulerAngles[3];
khaledelmadawi 0:efef62b55c86 88 float compass[3];
khaledelmadawi 0:efef62b55c86 89
khaledelmadawi 0:efef62b55c86 90 bool theta_update;
khaledelmadawi 0:efef62b55c86 91 float LPWM,RPWM;
khaledelmadawi 0:efef62b55c86 92 float dis=0,rightDis,leftDis;
khaledelmadawi 0:efef62b55c86 93
khaledelmadawi 0:efef62b55c86 94
khaledelmadawi 0:efef62b55c86 95 //read string parameters
khaledelmadawi 0:efef62b55c86 96 float X,Y,R,Theta_base_st,Fobstacles_dis,Lobstacles_dis,Robstacles_dis;
khaledelmadawi 0:efef62b55c86 97 char Ar_st[30];
khaledelmadawi 0:efef62b55c86 98 char X_coord[10];
khaledelmadawi 0:efef62b55c86 99 char Y_coord[10];
khaledelmadawi 0:efef62b55c86 100 char st[128];
khaledelmadawi 0:efef62b55c86 101
khaledelmadawi 0:efef62b55c86 102 //navigation model parameters
khaledelmadawi 0:efef62b55c86 103 float prev_dis[3];
khaledelmadawi 0:efef62b55c86 104 float post_dis[3];
khaledelmadawi 0:efef62b55c86 105 float current_theta,rot_theta;
khaledelmadawi 0:efef62b55c86 106
khaledelmadawi 0:efef62b55c86 107 //Obasticle avoidance Parameters
khaledelmadawi 0:efef62b55c86 108 float current_Obs_dis,Obs_h,gradient_Obs;
khaledelmadawi 0:efef62b55c86 109 int Obs_factor;
khaledelmadawi 0:efef62b55c86 110 bool FWM,GTGM,ccw,cw;
khaledelmadawi 0:efef62b55c86 111 float Obs_adj,Obs_opp,Obs_hyp,Obs_dis,Obs_alpha_opp,Obs_alpha,FWtheta,Xtow;
khaledelmadawi 0:efef62b55c86 112 float GTGtheta,dot_product_cond;
khaledelmadawi 0:efef62b55c86 113 float h_Obsinv,error_inv;
khaledelmadawi 0:efef62b55c86 114 //time parameters
khaledelmadawi 0:efef62b55c86 115 Timer t;
khaledelmadawi 0:efef62b55c86 116 Ticker timer,timer2;
khaledelmadawi 0:efef62b55c86 117
khaledelmadawi 0:efef62b55c86 118 void get_theta()
khaledelmadawi 0:efef62b55c86 119 {
khaledelmadawi 0:efef62b55c86 120 angle_cal(FIRtheta,FIR_W,FIR_E,FIR_Thresh);
khaledelmadawi 0:efef62b55c86 121 SS.ReadfilteredtaredEulerAngles(EulerAngles);
khaledelmadawi 0:efef62b55c86 122 SS.Readcompass(compass);
khaledelmadawi 0:efef62b55c86 123 Fobstacles_dis=IR_front.cm();//low range will return -1.0& for far range will return 0.
khaledelmadawi 0:efef62b55c86 124 Lobstacles_dis=IR_left.cm();
khaledelmadawi 0:efef62b55c86 125 Robstacles_dis=IR_right.cm();
khaledelmadawi 0:efef62b55c86 126 }
khaledelmadawi 0:efef62b55c86 127
khaledelmadawi 0:efef62b55c86 128
khaledelmadawi 0:efef62b55c86 129 int main()
khaledelmadawi 0:efef62b55c86 130 {
khaledelmadawi 0:efef62b55c86 131 motor_intialize();
khaledelmadawi 0:efef62b55c86 132 FWM=false;
khaledelmadawi 0:efef62b55c86 133 GTGM=true;
khaledelmadawi 0:efef62b55c86 134 ccw=false;
khaledelmadawi 0:efef62b55c86 135 cw=false;
khaledelmadawi 0:efef62b55c86 136 while(1) { //big fat loop
khaledelmadawi 0:efef62b55c86 137
khaledelmadawi 0:efef62b55c86 138 char datain;
khaledelmadawi 0:efef62b55c86 139 // xbee.puts("press B to begin...");
khaledelmadawi 0:efef62b55c86 140 datain=xbee.getc();
khaledelmadawi 0:efef62b55c86 141 if(datain=='b'||datain=='B') {
khaledelmadawi 0:efef62b55c86 142 t.reset();
khaledelmadawi 0:efef62b55c86 143 t.start();
khaledelmadawi 0:efef62b55c86 144 // xbee.puts("enter coordinates in the form of BX'x'Y'y'E...\r\n");
khaledelmadawi 0:efef62b55c86 145 read_string();
khaledelmadawi 0:efef62b55c86 146 rot_theta=Theta_base_st;
khaledelmadawi 0:efef62b55c86 147 navigation_model();
khaledelmadawi 0:efef62b55c86 148 timer.attach(&get_theta, acquizitiontimeinterrupt);
khaledelmadawi 0:efef62b55c86 149 timer2.attach(&navigation_model, navigationtimeinterrupt);
khaledelmadawi 0:efef62b55c86 150
khaledelmadawi 0:efef62b55c86 151 if(theta_update)
khaledelmadawi 0:efef62b55c86 152 rotation();
khaledelmadawi 0:efef62b55c86 153 rot_theta=0;
khaledelmadawi 0:efef62b55c86 154 FIRtheta[0]=FIRtheta[0]-Theta_base_st;
khaledelmadawi 0:efef62b55c86 155 errorI=0;
khaledelmadawi 0:efef62b55c86 156 while(1) {//////////////////////////////////////////////////////////////////////////////////////////////////////////////in diffrent modes
khaledelmadawi 0:efef62b55c86 157 GTG_mode();
khaledelmadawi 0:efef62b55c86 158 if(abs(R-(dis+prev_dis[0]))<3)break;
khaledelmadawi 0:efef62b55c86 159 }//whileloop
khaledelmadawi 0:efef62b55c86 160 timer.detach();
khaledelmadawi 0:efef62b55c86 161 timer2.detach();
khaledelmadawi 0:efef62b55c86 162 Stop();
khaledelmadawi 0:efef62b55c86 163 reset_parameters();
khaledelmadawi 0:efef62b55c86 164 sprintf( st ,"final:distance%f lp:%d rp:%d %f\r\n",abs((dis+prev_dis[0])),leftPulses,rightPulses,t.read());
khaledelmadawi 0:efef62b55c86 165 xbee.puts(st);
khaledelmadawi 0:efef62b55c86 166
khaledelmadawi 0:efef62b55c86 167 }//if condition loop enter b
khaledelmadawi 0:efef62b55c86 168 }//big fat loop
khaledelmadawi 0:efef62b55c86 169 }//main loop
khaledelmadawi 0:efef62b55c86 170
khaledelmadawi 0:efef62b55c86 171 void read_string()
khaledelmadawi 0:efef62b55c86 172 {
khaledelmadawi 0:efef62b55c86 173 int i=0;
khaledelmadawi 0:efef62b55c86 174 char c;
khaledelmadawi 0:efef62b55c86 175 c=xbee.getc();
khaledelmadawi 0:efef62b55c86 176 if(c=='B') {
khaledelmadawi 0:efef62b55c86 177 while(1) {
khaledelmadawi 0:efef62b55c86 178 c=xbee.getc();
khaledelmadawi 0:efef62b55c86 179 if(c=='E')break;
khaledelmadawi 0:efef62b55c86 180 Ar_st[i]=c;
khaledelmadawi 0:efef62b55c86 181 i++;
khaledelmadawi 0:efef62b55c86 182 myled != myled;
khaledelmadawi 0:efef62b55c86 183
khaledelmadawi 0:efef62b55c86 184 }
khaledelmadawi 0:efef62b55c86 185
khaledelmadawi 0:efef62b55c86 186 int count=0,j=0;
khaledelmadawi 0:efef62b55c86 187 if(Ar_st[j]=='X') {
khaledelmadawi 0:efef62b55c86 188 while(1) {
khaledelmadawi 0:efef62b55c86 189 j++;
khaledelmadawi 0:efef62b55c86 190 if(Ar_st[j]=='Y')break;
khaledelmadawi 0:efef62b55c86 191 X_coord[count]=Ar_st[j];
khaledelmadawi 0:efef62b55c86 192 count++;
khaledelmadawi 0:efef62b55c86 193 }
khaledelmadawi 0:efef62b55c86 194 X=atof(X_coord);
khaledelmadawi 0:efef62b55c86 195 count=0;
khaledelmadawi 0:efef62b55c86 196 }
khaledelmadawi 0:efef62b55c86 197 if(Ar_st[j]=='Y') {
khaledelmadawi 0:efef62b55c86 198 while(1) {
khaledelmadawi 0:efef62b55c86 199 j++;
khaledelmadawi 0:efef62b55c86 200 Y_coord[count]=Ar_st[j];
khaledelmadawi 0:efef62b55c86 201 count++;
khaledelmadawi 0:efef62b55c86 202 if(j==i)break;
khaledelmadawi 0:efef62b55c86 203 }
khaledelmadawi 0:efef62b55c86 204 Y=atof(Y_coord);
khaledelmadawi 0:efef62b55c86 205 count=0;
khaledelmadawi 0:efef62b55c86 206 }
khaledelmadawi 0:efef62b55c86 207 }
khaledelmadawi 0:efef62b55c86 208 xbee.puts("complete");
khaledelmadawi 0:efef62b55c86 209 R=sqrt(X*X+Y*Y);
khaledelmadawi 0:efef62b55c86 210 Theta_base_st=atan2(X,Y);
khaledelmadawi 0:efef62b55c86 211 sprintf( st ,"Rover will move %f cm & %f cm \r\n",X,Y);
khaledelmadawi 0:efef62b55c86 212 xbee.puts(st);
khaledelmadawi 0:efef62b55c86 213 sprintf( st ,"Rover will move %f cm & %f Degree\r\n",R,Theta_base_st);
khaledelmadawi 0:efef62b55c86 214 xbee.puts(st);
khaledelmadawi 0:efef62b55c86 215
khaledelmadawi 0:efef62b55c86 216 }
khaledelmadawi 0:efef62b55c86 217 void motor_intialize()
khaledelmadawi 0:efef62b55c86 218 {
khaledelmadawi 0:efef62b55c86 219 leftFrontMotor.period(0.00005); //Set motor PWM periods to 20KHz.
khaledelmadawi 0:efef62b55c86 220 rightFrontMotor.period(0.00005);
khaledelmadawi 0:efef62b55c86 221 leftBackMotor.period(0.00005); //Set motor PWM periods to 20KHz.
khaledelmadawi 0:efef62b55c86 222 rightBackMotor.period(0.00005);
khaledelmadawi 0:efef62b55c86 223 }
khaledelmadawi 0:efef62b55c86 224
khaledelmadawi 0:efef62b55c86 225 void errorfun()
khaledelmadawi 0:efef62b55c86 226 {
khaledelmadawi 0:efef62b55c86 227 error=current_theta-FIRtheta[0];
khaledelmadawi 0:efef62b55c86 228 errorI=error+errorI;
khaledelmadawi 0:efef62b55c86 229 // sprintf( st ,"error:%f errprI:%f\r\n",error,errorI);
khaledelmadawi 0:efef62b55c86 230 // xbee.puts(st);
khaledelmadawi 0:efef62b55c86 231 float h;
khaledelmadawi 0:efef62b55c86 232 h=6*error;//+0.01*errorI;
khaledelmadawi 0:efef62b55c86 233 sigmoid=4/(1+exp(h))-2;
khaledelmadawi 0:efef62b55c86 234 if(sigmoid<0) {
khaledelmadawi 0:efef62b55c86 235 RTP=1+sigmoid;
khaledelmadawi 0:efef62b55c86 236 LTP=1;
khaledelmadawi 0:efef62b55c86 237 } else if(sigmoid>0) {
khaledelmadawi 0:efef62b55c86 238 RTP=1;
khaledelmadawi 0:efef62b55c86 239 LTP=1-sigmoid;
khaledelmadawi 0:efef62b55c86 240 } else {
khaledelmadawi 0:efef62b55c86 241 RTP=1;
khaledelmadawi 0:efef62b55c86 242 LTP=1;
khaledelmadawi 0:efef62b55c86 243 }
khaledelmadawi 0:efef62b55c86 244 }
khaledelmadawi 0:efef62b55c86 245 void PWM_cal()
khaledelmadawi 0:efef62b55c86 246 {
khaledelmadawi 0:efef62b55c86 247 errorfun();
khaledelmadawi 0:efef62b55c86 248 RPWM= 0.9*RTP;
khaledelmadawi 0:efef62b55c86 249 LPWM= 0.9*LTP;
khaledelmadawi 0:efef62b55c86 250 }
khaledelmadawi 0:efef62b55c86 251 void Distance_calculation()
khaledelmadawi 0:efef62b55c86 252 {
khaledelmadawi 0:efef62b55c86 253 leftPulses = leftBackQei.getPulses();
khaledelmadawi 0:efef62b55c86 254 rightPulses = rightBackQei.getPulses();
khaledelmadawi 0:efef62b55c86 255 leftDis = (leftPulses)*2*3.14159*3/LPPD;
khaledelmadawi 0:efef62b55c86 256 rightDis = (rightPulses)*2*3.14159*3/RPPD;
khaledelmadawi 0:efef62b55c86 257 dis= (rightDis+leftDis)/2;
khaledelmadawi 0:efef62b55c86 258 }
khaledelmadawi 0:efef62b55c86 259 void Get_Window(int j,float *vec,float *vec2)
khaledelmadawi 0:efef62b55c86 260 {
khaledelmadawi 0:efef62b55c86 261 float vector1[3]= {0,0,0};
khaledelmadawi 0:efef62b55c86 262 for(int i=0; i<3; i++) {
khaledelmadawi 0:efef62b55c86 263 vec[i]=0;
khaledelmadawi 0:efef62b55c86 264 vec2[i]=0;
khaledelmadawi 0:efef62b55c86 265 }
khaledelmadawi 0:efef62b55c86 266 int k=0;
khaledelmadawi 0:efef62b55c86 267 while(k<j) {
khaledelmadawi 0:efef62b55c86 268 SS.Readgyros(vector1);
khaledelmadawi 0:efef62b55c86 269 //SS.Readaccelerometer(vector2);
khaledelmadawi 0:efef62b55c86 270 for(int i=0; i<3; i++) {
khaledelmadawi 0:efef62b55c86 271 vec[i]=vector1[i]*vector1[i]+vec[i];
khaledelmadawi 0:efef62b55c86 272 }
khaledelmadawi 0:efef62b55c86 273 for(int i=0; i<3; i++) {
khaledelmadawi 0:efef62b55c86 274 vec2[i]=vector1[i]+vec2[i];
khaledelmadawi 0:efef62b55c86 275 }
khaledelmadawi 0:efef62b55c86 276 k++;
khaledelmadawi 0:efef62b55c86 277 }
khaledelmadawi 0:efef62b55c86 278 for(int i=0; i<3; i++) {
khaledelmadawi 0:efef62b55c86 279 vec[i]=vec[i]/(j);
khaledelmadawi 0:efef62b55c86 280 }
khaledelmadawi 0:efef62b55c86 281 for(int i=0; i<3; i++) {
khaledelmadawi 0:efef62b55c86 282 vec2[i]=vec2[i]/(j);
khaledelmadawi 0:efef62b55c86 283 }
khaledelmadawi 0:efef62b55c86 284 }
khaledelmadawi 0:efef62b55c86 285 void Energy_cal(float *Energy,float *W,float Threshold1[3])
khaledelmadawi 0:efef62b55c86 286 {
khaledelmadawi 0:efef62b55c86 287 Get_Window(10,Energy,W);
khaledelmadawi 0:efef62b55c86 288 for(int i=0; i<3; i++) {
khaledelmadawi 0:efef62b55c86 289 if(Energy[i]<Threshold1[i]*sfgyro)W[i]=0;
khaledelmadawi 0:efef62b55c86 290 }
khaledelmadawi 0:efef62b55c86 291 for(int i=0; i<3; i++)W[i]=W[i];
khaledelmadawi 0:efef62b55c86 292 }
khaledelmadawi 0:efef62b55c86 293 void angle_cal(float *theta,float *theta_dot,float *E,float Threshold[3])
khaledelmadawi 0:efef62b55c86 294 {
khaledelmadawi 0:efef62b55c86 295 Energy_cal(E,theta_dot,Threshold);
khaledelmadawi 0:efef62b55c86 296 theta[0]=theta_dot[0]*(0.2)+theta[0];
khaledelmadawi 0:efef62b55c86 297 theta[1]=theta_dot[1]*(0.2)+theta[1];
khaledelmadawi 0:efef62b55c86 298 theta[2]=theta_dot[2]*(0.2)+theta[2];
khaledelmadawi 0:efef62b55c86 299 }
khaledelmadawi 0:efef62b55c86 300 void move(float Lperiod,float Rperiod)
khaledelmadawi 0:efef62b55c86 301 {
khaledelmadawi 0:efef62b55c86 302 leftFrontMotor.speed(Lperiod);
khaledelmadawi 0:efef62b55c86 303 leftBackMotor.speed(Lperiod);
khaledelmadawi 0:efef62b55c86 304
khaledelmadawi 0:efef62b55c86 305 rightFrontMotor.speed(Rperiod);
khaledelmadawi 0:efef62b55c86 306 rightBackMotor.speed(Rperiod);
khaledelmadawi 0:efef62b55c86 307 }
khaledelmadawi 0:efef62b55c86 308 void Stop()
khaledelmadawi 0:efef62b55c86 309 {
khaledelmadawi 0:efef62b55c86 310 leftFrontMotor.brake();
khaledelmadawi 0:efef62b55c86 311 rightFrontMotor.brake();
khaledelmadawi 0:efef62b55c86 312 leftBackMotor.brake();
khaledelmadawi 0:efef62b55c86 313 rightBackMotor.brake();
khaledelmadawi 0:efef62b55c86 314 }
khaledelmadawi 0:efef62b55c86 315
khaledelmadawi 0:efef62b55c86 316 void RotateLeft(float Lperiod,float Rperiod)
khaledelmadawi 0:efef62b55c86 317 {
khaledelmadawi 0:efef62b55c86 318 leftFrontMotor.speed(-Lperiod);
khaledelmadawi 0:efef62b55c86 319 leftBackMotor.speed(-Lperiod);
khaledelmadawi 0:efef62b55c86 320
khaledelmadawi 0:efef62b55c86 321 rightFrontMotor.speed(Rperiod);
khaledelmadawi 0:efef62b55c86 322 rightBackMotor.speed(Rperiod);
khaledelmadawi 0:efef62b55c86 323 }
khaledelmadawi 0:efef62b55c86 324
khaledelmadawi 0:efef62b55c86 325 void RotateRight(float Lperiod,float Rperiod)
khaledelmadawi 0:efef62b55c86 326 {
khaledelmadawi 0:efef62b55c86 327 leftFrontMotor.speed(Lperiod);
khaledelmadawi 0:efef62b55c86 328 leftBackMotor.speed(Lperiod);
khaledelmadawi 0:efef62b55c86 329
khaledelmadawi 0:efef62b55c86 330 rightFrontMotor.speed(-Rperiod);
khaledelmadawi 0:efef62b55c86 331 rightBackMotor.speed(-Rperiod);
khaledelmadawi 0:efef62b55c86 332 }
khaledelmadawi 0:efef62b55c86 333 //////////////////////////////////////////////////////////////////////////////////////not used yet
khaledelmadawi 0:efef62b55c86 334 void rotation()
khaledelmadawi 0:efef62b55c86 335 {
khaledelmadawi 0:efef62b55c86 336 timer2.detach();
khaledelmadawi 0:efef62b55c86 337 if(current_theta-FIRtheta[0]>FIR_Thresh[0]) {
khaledelmadawi 0:efef62b55c86 338 // xbee.puts("rotate right\r\n");
khaledelmadawi 0:efef62b55c86 339 while(FIRtheta[0]<current_theta) { //new_disctrete..........................................................
khaledelmadawi 0:efef62b55c86 340 RotateRight(0.9,0.9);
khaledelmadawi 0:efef62b55c86 341 Distance_calculation();
khaledelmadawi 0:efef62b55c86 342 }
khaledelmadawi 0:efef62b55c86 343 } else if(current_theta-FIRtheta[0]<-1*FIR_Thresh[0]) {
khaledelmadawi 0:efef62b55c86 344 // xbee.puts("rotate left\r\n");
khaledelmadawi 0:efef62b55c86 345 while(FIRtheta[0]>current_theta) {
khaledelmadawi 0:efef62b55c86 346 RotateLeft(0.9,0.9);
khaledelmadawi 0:efef62b55c86 347 Distance_calculation();
khaledelmadawi 0:efef62b55c86 348 }
khaledelmadawi 0:efef62b55c86 349 } else {
khaledelmadawi 0:efef62b55c86 350 // xbee.puts("will not rotate\r\n");
khaledelmadawi 0:efef62b55c86 351 }
khaledelmadawi 0:efef62b55c86 352 theta_update=false;
khaledelmadawi 0:efef62b55c86 353 timer2.attach(&navigation_model, navigationtimeinterrupt);
khaledelmadawi 0:efef62b55c86 354 }
khaledelmadawi 0:efef62b55c86 355 void GTG_mode()
khaledelmadawi 0:efef62b55c86 356 {
khaledelmadawi 0:efef62b55c86 357 if(FWM)Wall_Following();
khaledelmadawi 0:efef62b55c86 358 PWM_cal();
khaledelmadawi 0:efef62b55c86 359 move(LPWM,RPWM);
khaledelmadawi 0:efef62b55c86 360 Distance_calculation();
khaledelmadawi 0:efef62b55c86 361 /* 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);
khaledelmadawi 0:efef62b55c86 362 xbee.puts(st);
khaledelmadawi 0:efef62b55c86 363 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);
khaledelmadawi 0:efef62b55c86 364 xbee.puts(st);
khaledelmadawi 0:efef62b55c86 365 */
khaledelmadawi 0:efef62b55c86 366 /* 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));
khaledelmadawi 0:efef62b55c86 367 xbee.puts(st);
khaledelmadawi 0:efef62b55c86 368 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);
khaledelmadawi 0:efef62b55c86 369 xbee.puts(st);
khaledelmadawi 0:efef62b55c86 370 */
khaledelmadawi 0:efef62b55c86 371 //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);
khaledelmadawi 0:efef62b55c86 372 //xbee.puts(st);
khaledelmadawi 0:efef62b55c86 373 }
khaledelmadawi 0:efef62b55c86 374 void FollowWall()
khaledelmadawi 0:efef62b55c86 375 {
khaledelmadawi 0:efef62b55c86 376 if(Fobstacles_dis<delta&&Fobstacles_dis>0&&!(cw&&Robstacles_dis==-1||ccw&&Lobstacles_dis==-1))First_FW();
khaledelmadawi 0:efef62b55c86 377 //else if(FWM)Wall_Following();
khaledelmadawi 0:efef62b55c86 378
khaledelmadawi 0:efef62b55c86 379 }
khaledelmadawi 0:efef62b55c86 380 void Wall_Following()
khaledelmadawi 0:efef62b55c86 381 {
khaledelmadawi 0:efef62b55c86 382 // xbee.puts("\r\nWF\r\n");
khaledelmadawi 0:efef62b55c86 383 if(Obs_dis>0) { //clockwise
khaledelmadawi 0:efef62b55c86 384 current_Obs_dis=Robstacles_dis;
khaledelmadawi 0:efef62b55c86 385 cw=true;
khaledelmadawi 0:efef62b55c86 386 ccw=false;
khaledelmadawi 0:efef62b55c86 387 if(current_Obs_dis==0||current_Obs_dis==-1)
khaledelmadawi 0:efef62b55c86 388 current_Obs_dis=150;//check conditions in this line
khaledelmadawi 0:efef62b55c86 389 } else if(Obs_dis<0) { //anti clock wise
khaledelmadawi 0:efef62b55c86 390 current_Obs_dis=-1*Lobstacles_dis;
khaledelmadawi 0:efef62b55c86 391 cw=false;
khaledelmadawi 0:efef62b55c86 392 ccw=true;
khaledelmadawi 0:efef62b55c86 393 if(current_Obs_dis==0||current_Obs_dis==1)current_Obs_dis=-150;//check conditions in this line
khaledelmadawi 0:efef62b55c86 394 }
khaledelmadawi 0:efef62b55c86 395
khaledelmadawi 0:efef62b55c86 396 Obs_h=current_Obs_dis-Obs_dis;
khaledelmadawi 0:efef62b55c86 397 gradient_Obs=2/(1+exp(-0.05*Obs_h))-1;
khaledelmadawi 0:efef62b55c86 398 //current_theta=current_theta+3.14*(gradient_Obs);//from here we shall begin.
khaledelmadawi 0:efef62b55c86 399 h_Obsinv=-log(4/(gradient_Obs+2)-1);
khaledelmadawi 0:efef62b55c86 400 error_inv=(h_Obsinv-0.01*errorI)/6;
khaledelmadawi 0:efef62b55c86 401 current_theta=error_inv+current_theta;
khaledelmadawi 0:efef62b55c86 402 //we should write here exiting condition.
khaledelmadawi 0:efef62b55c86 403 GTGtheta=atan2(post_dis[2],post_dis[1]);
khaledelmadawi 0:efef62b55c86 404 dot_product_cond=cos(current_theta-GTGtheta);
khaledelmadawi 0:efef62b55c86 405 if((dot_product_cond>0)&&(post_dis[0]<Xtow)&&(abs(current_Obs_dis)>60)) {
khaledelmadawi 0:efef62b55c86 406 FWM=false;
khaledelmadawi 0:efef62b55c86 407 GTGM=true;
khaledelmadawi 0:efef62b55c86 408 }
khaledelmadawi 0:efef62b55c86 409 }
khaledelmadawi 0:efef62b55c86 410 void First_FW()
khaledelmadawi 0:efef62b55c86 411 {
khaledelmadawi 0:efef62b55c86 412 // xbee.puts("\r\nFFW\r\n");
khaledelmadawi 0:efef62b55c86 413 Xtow=post_dis[0];//check if there is a proper plase to put Xtow more than this
khaledelmadawi 0:efef62b55c86 414 Obs_adj=Fobstacles_dis;
khaledelmadawi 0:efef62b55c86 415 min_Obs_opp();
khaledelmadawi 0:efef62b55c86 416 Obs_hyp=sqrt(Obs_adj*Obs_adj+Obs_opp*Obs_opp);
khaledelmadawi 0:efef62b55c86 417 Obs_dis=Obs_adj*Obs_opp/Obs_hyp;
khaledelmadawi 0:efef62b55c86 418
khaledelmadawi 0:efef62b55c86 419 Obs_alpha_opp=sqrt(Obs_adj*Obs_adj-Obs_dis*Obs_dis);
khaledelmadawi 0:efef62b55c86 420 Obs_alpha=atan2(Obs_alpha_opp,Obs_dis);
khaledelmadawi 0:efef62b55c86 421
khaledelmadawi 0:efef62b55c86 422 if(Obs_alpha<3.1415/2)
khaledelmadawi 0:efef62b55c86 423 Obs_alpha= Obs_alpha-1.57079;
khaledelmadawi 0:efef62b55c86 424 else if(Obs_alpha>3.1415/2)
khaledelmadawi 0:efef62b55c86 425 Obs_alpha= 3.1415-Obs_alpha;
khaledelmadawi 0:efef62b55c86 426 if(Obs_alpha<0.0698&&Obs_alpha>-1*0.0698)
khaledelmadawi 0:efef62b55c86 427 Obs_alpha=1.57079;
khaledelmadawi 0:efef62b55c86 428 current_theta=current_theta+Obs_alpha;//FWtheta;
khaledelmadawi 0:efef62b55c86 429 if(Obs_dis==0)Obs_dis=Obs_adj;//done.
khaledelmadawi 0:efef62b55c86 430
khaledelmadawi 0:efef62b55c86 431 FWM=true;
khaledelmadawi 0:efef62b55c86 432 GTGM=false;
khaledelmadawi 0:efef62b55c86 433 //exiting conditions.
khaledelmadawi 0:efef62b55c86 434 GTGtheta=atan2(post_dis[2],post_dis[1]);
khaledelmadawi 0:efef62b55c86 435 dot_product_cond=cos(current_theta-GTGtheta);//dot product check.
khaledelmadawi 0:efef62b55c86 436 if((dot_product_cond>0)&&(post_dis[0]<Xtow)&&(abs(current_Obs_dis)>60)) {
khaledelmadawi 0:efef62b55c86 437 FWM=false;
khaledelmadawi 0:efef62b55c86 438 GTGM=true;
khaledelmadawi 0:efef62b55c86 439 }
khaledelmadawi 0:efef62b55c86 440 }
khaledelmadawi 0:efef62b55c86 441 void min_Obs_opp()
khaledelmadawi 0:efef62b55c86 442 {
khaledelmadawi 0:efef62b55c86 443 if(Robstacles_dis<0&&Lobstacles_dis<0)Obs_opp=0;
khaledelmadawi 0:efef62b55c86 444 else if(Robstacles_dis<0&&Lobstacles_dis>0)Obs_opp=-1*Lobstacles_dis;
khaledelmadawi 0:efef62b55c86 445 else if(Robstacles_dis>0&&Lobstacles_dis<0)Obs_opp=Robstacles_dis;
khaledelmadawi 0:efef62b55c86 446 else if(Robstacles_dis>0&&Lobstacles_dis>0)Obs_opp = Lobstacles_dis>Robstacles_dis ? Robstacles_dis : -1*Lobstacles_dis;
khaledelmadawi 0:efef62b55c86 447 }
khaledelmadawi 0:efef62b55c86 448 void navigation_model() //edit here
khaledelmadawi 0:efef62b55c86 449 {
khaledelmadawi 0:efef62b55c86 450 prev_dis[1]=prev_dis[1]+dis*cos(FIRtheta[0]);
khaledelmadawi 0:efef62b55c86 451 prev_dis[2]=prev_dis[2]+dis*sin(FIRtheta[0]);
khaledelmadawi 0:efef62b55c86 452 prev_dis[0]=sqrt(prev_dis[1]*prev_dis[1]+prev_dis[2]*prev_dis[2]);
khaledelmadawi 0:efef62b55c86 453
khaledelmadawi 0:efef62b55c86 454 post_dis[1]=R*cos(rot_theta)-prev_dis[1];
khaledelmadawi 0:efef62b55c86 455 post_dis[2]=R*sin(rot_theta)-prev_dis[2];
khaledelmadawi 0:efef62b55c86 456 post_dis[0]=sqrt(post_dis[1]*post_dis[1]+post_dis[2]*post_dis[2]);
khaledelmadawi 0:efef62b55c86 457 if(FWM&&!GTGM||Fobstacles_dis<delta&&Fobstacles_dis>0&&post_dis[0]>Fobstacles_dis)//check conditions in this line
khaledelmadawi 0:efef62b55c86 458 FollowWall();
khaledelmadawi 0:efef62b55c86 459 if(GTGM&&!FWM) {
khaledelmadawi 0:efef62b55c86 460 current_theta=atan2(post_dis[2],post_dis[1]);
khaledelmadawi 0:efef62b55c86 461 Obs_alpha=0;
khaledelmadawi 0:efef62b55c86 462 ccw=false;
khaledelmadawi 0:efef62b55c86 463 cw=false;
khaledelmadawi 0:efef62b55c86 464 // xbee.puts("\r\nGTG\r\n");
khaledelmadawi 0:efef62b55c86 465 }
khaledelmadawi 0:efef62b55c86 466 leftBackQei.reset();
khaledelmadawi 0:efef62b55c86 467 rightBackQei.reset();
khaledelmadawi 0:efef62b55c86 468 dis=0;
khaledelmadawi 0:efef62b55c86 469 theta_update=true;
khaledelmadawi 0:efef62b55c86 470 }
khaledelmadawi 0:efef62b55c86 471 void reset_parameters()
khaledelmadawi 0:efef62b55c86 472 {
khaledelmadawi 0:efef62b55c86 473 }