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