quadcopter cufe
/
FollowWall2
Go to goal robot with follow wall algo
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?
User | Revision | Line number | New 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 | } |