quadcopter cufe / Mbed 2 deprecated FollowWall

follow wall 1

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }