yi li / Mbed 2 deprecated 3W_wifi_odo_p2p_0722

20160725

Dependencies:   QEI mbed

main.cpp@0:926cd923a480, 2016-07-22 (annotated)

Committer:
xx123456987
Date:
Fri Jul 22 07:57:56 2016 +0000
Revision:
0:926cd923a480
Child:
1:a78e5f6154d3
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xx123456987 0:926cd923a480 1 /* 版本說明: 可1~9任選，但scanf有問題
xx123456987 0:926cd923a480 2 實驗說明:
xx123456987 0:926cd923a480 3 利用運動學模型並以靜止的virtual leader當作輸入命令，達到point to point控制，來驗證里程計的正確性。
xx123456987 0:926cd923a480 4 =八個方位的測試=
xx123456987 0:926cd923a480 5 7 8 9
xx123456987 0:926cd923a480 6 ↖ ↑ ↗
xx123456987 0:926cd923a480 7 4← . →6
xx123456987 0:926cd923a480 8 ↙ ↓ ↘
xx123456987 0:926cd923a480 9 1 2 3
xx123456987 0:926cd923a480 10 07/07測試結果:
xx123456987 0:926cd923a480 11 實驗成功但略有誤差
xx123456987 0:926cd923a480 12 */
xx123456987 0:926cd923a480 13 #include "mbed.h"
xx123456987 0:926cd923a480 14 #include "math.h"
xx123456987 0:926cd923a480 15 #include "QEI.h"
xx123456987 0:926cd923a480 16 // definition
xx123456987 0:926cd923a480 17 #define pi 3.1416
xx123456987 0:926cd923a480 18 #define pwm_period_us 500
xx123456987 0:926cd923a480 19 #define r 0.05 // wheel radius [m]
xx123456987 0:926cd923a480 20 #define L 0.125 // distance between wheel center & geometric center [m]
xx123456987 0:926cd923a480 21 #define pulsesPerRev 1024
xx123456987 0:926cd923a480 22 #define c0 0.06// control gain
xx123456987 0:926cd923a480 23
xx123456987 0:926cd923a480 24 // initial pose
xx123456987 0:926cd923a480 25 #define x0 0 //[m]
xx123456987 0:926cd923a480 26 #define y0 0 //[m]
xx123456987 0:926cd923a480 27 #define theta0 0//[deg.]
xx123456987 0:926cd923a480 28 // desied pose
xx123456987 0:926cd923a480 29 #define xd 0.5 //[cm]
xx123456987 0:926cd923a480 30 #define yd 0.5 //[cm]
xx123456987 0:926cd923a480 31 #define thetad 0//[deg.]
xx123456987 0:926cd923a480 32 // error threshold
xx123456987 0:926cd923a480 33 #define ex 0.5 //[cm]
xx123456987 0:926cd923a480 34 #define ey 0.5 //[cm]
xx123456987 0:926cd923a480 35 #define etheta 1//[deg.]
xx123456987 0:926cd923a480 36
xx123456987 0:926cd923a480 37 PwmOut pwm_1(D9);
xx123456987 0:926cd923a480 38 PwmOut pwm_2(D10);
xx123456987 0:926cd923a480 39 PwmOut pwm_3(D11);
xx123456987 0:926cd923a480 40
xx123456987 0:926cd923a480 41 Timer t; // timer for printing
xx123456987 0:926cd923a480 42
xx123456987 0:926cd923a480 43 QEI w1 (D2, D3, NC, pulsesPerRev, QEI::X4_ENCODING);
xx123456987 0:926cd923a480 44 QEI w2 (D4, D5, NC, pulsesPerRev, QEI::X4_ENCODING);
xx123456987 0:926cd923a480 45 QEI w3 (D14, D15, NC, pulsesPerRev, QEI::X4_ENCODING);
xx123456987 0:926cd923a480 46 // odomerty para.
xx123456987 0:926cd923a480 47 float c1 = r/(3*L);
xx123456987 0:926cd923a480 48 float c2 = 2*r/3;
xx123456987 0:926cd923a480 49 float Current_X[3] = {x0,y0,(theta0*pi/180)+pi/3}; // X[0] = x; X[1] = y; X[2] = theta; shift pi/3 : origin to new
xx123456987 0:926cd923a480 50 float Next_X[3] = {0,0,0};
xx123456987 0:926cd923a480 51 double d_theta1,d_theta2,d_theta3,d_theta;
xx123456987 0:926cd923a480 52 int qei1_new, qei2_new, qei3_new;
xx123456987 0:926cd923a480 53 int qei1_old, qei2_old, qei3_old;
xx123456987 0:926cd923a480 54 // control law para.
xx123456987 0:926cd923a480 55 float f1[3] = {0}; // formation vector
xx123456987 0:926cd923a480 56 float X1[3],X1_b[3];// X1_b(X_BAR) is defined as X1-f1
xx123456987 0:926cd923a480 57 float XL[3] = {xd,yd,(thetad*pi/180)+pi/3};// pose of virtual leader [m m rad.]
xx123456987 0:926cd923a480 58 float u[3] = {0};// control law
xx123456987 0:926cd923a480 59 float omg1,omg2,omg3;// velocity of wheels
xx123456987 0:926cd923a480 60 // PWM
xx123456987 0:926cd923a480 61 float pwm_w1,pwm_w2,pwm_w3;
xx123456987 0:926cd923a480 62 // Others
xx123456987 0:926cd923a480 63 DigitalIn mybutton(USER_BUTTON);
xx123456987 0:926cd923a480 64 DigitalOut myled(LED1);
xx123456987 0:926cd923a480 65 int c = 3;
xx123456987 0:926cd923a480 66 float err[3]= {0};
xx123456987 0:926cd923a480 67 bool s0 = false;
xx123456987 0:926cd923a480 68 char dire;
xx123456987 0:926cd923a480 69
xx123456987 0:926cd923a480 70
xx123456987 0:926cd923a480 71 int main()
xx123456987 0:926cd923a480 72 {
xx123456987 0:926cd923a480 73
xx123456987 0:926cd923a480 74 t.start();
xx123456987 0:926cd923a480 75
xx123456987 0:926cd923a480 76 pwm_1.period_us(pwm_period_us);
xx123456987 0:926cd923a480 77 pwm_2.period_us(pwm_period_us);
xx123456987 0:926cd923a480 78 pwm_3.period_us(pwm_period_us);
xx123456987 0:926cd923a480 79
xx123456987 0:926cd923a480 80 pwm_1.write(0.50);
xx123456987 0:926cd923a480 81 pwm_2.write(0.50);
xx123456987 0:926cd923a480 82 pwm_3.write(0.50);
xx123456987 0:926cd923a480 83
xx123456987 0:926cd923a480 84 qei1_old = 0;
xx123456987 0:926cd923a480 85 qei2_old = 0;
xx123456987 0:926cd923a480 86 qei3_old = 0;
xx123456987 0:926cd923a480 87 printf("===================Reset===================\n");
xx123456987 0:926cd923a480 88 aa: {
xx123456987 0:926cd923a480 89
xx123456987 0:926cd923a480 90 printf("Which direction to go?\n");
xx123456987 0:926cd923a480 91 printf(" 7 8 9 \n");
xx123456987 0:926cd923a480 92 printf(" | \n");
xx123456987 0:926cd923a480 93 printf(" 4- 5 -6 \n");
xx123456987 0:926cd923a480 94 printf(" | \n");
xx123456987 0:926cd923a480 95 printf(" 1 2 3 \n");
xx123456987 0:926cd923a480 96 myled = 1;
xx123456987 0:926cd923a480 97 }
xx123456987 0:926cd923a480 98
xx123456987 0:926cd923a480 99 scanf("%c", &dire);
xx123456987 0:926cd923a480 100 switch (dire) {
xx123456987 0:926cd923a480 101 case '1':
xx123456987 0:926cd923a480 102 XL[0]=-0.4;
xx123456987 0:926cd923a480 103 XL[1]=-0.4;
xx123456987 0:926cd923a480 104 XL[2]=0+pi/3;
xx123456987 0:926cd923a480 105 break;
xx123456987 0:926cd923a480 106 case '2':
xx123456987 0:926cd923a480 107 XL[0]=0;
xx123456987 0:926cd923a480 108 XL[1]=-0.4;
xx123456987 0:926cd923a480 109 XL[2]=0+pi/3;
xx123456987 0:926cd923a480 110 break;
xx123456987 0:926cd923a480 111 case '3':
xx123456987 0:926cd923a480 112 XL[0]=0.4;
xx123456987 0:926cd923a480 113 XL[1]=-0.4;
xx123456987 0:926cd923a480 114 XL[2]=0+pi/3;
xx123456987 0:926cd923a480 115 break;
xx123456987 0:926cd923a480 116 case '4':
xx123456987 0:926cd923a480 117 XL[0]=-0.4;
xx123456987 0:926cd923a480 118 XL[1]=0;
xx123456987 0:926cd923a480 119 XL[2]=0+pi/3;
xx123456987 0:926cd923a480 120 break;
xx123456987 0:926cd923a480 121 case '5':
xx123456987 0:926cd923a480 122 XL[0]=0;
xx123456987 0:926cd923a480 123 XL[1]=0;
xx123456987 0:926cd923a480 124 XL[2]=0+pi/3;
xx123456987 0:926cd923a480 125 break;
xx123456987 0:926cd923a480 126 case '6':
xx123456987 0:926cd923a480 127 XL[0]=0.4;
xx123456987 0:926cd923a480 128 XL[1]=0;
xx123456987 0:926cd923a480 129 XL[2]=0+pi/3;
xx123456987 0:926cd923a480 130 break;
xx123456987 0:926cd923a480 131 case '7':
xx123456987 0:926cd923a480 132 XL[0]=-0.4;
xx123456987 0:926cd923a480 133 XL[1]=0.4;
xx123456987 0:926cd923a480 134 XL[2]=0+pi/3;
xx123456987 0:926cd923a480 135 break;
xx123456987 0:926cd923a480 136 case '8':
xx123456987 0:926cd923a480 137 XL[0]=0;
xx123456987 0:926cd923a480 138 XL[1]=0.4;
xx123456987 0:926cd923a480 139 XL[2]=0+pi/3;
xx123456987 0:926cd923a480 140 break;
xx123456987 0:926cd923a480 141 case '9':
xx123456987 0:926cd923a480 142 XL[0]=0.4;
xx123456987 0:926cd923a480 143 XL[1]=0.4;
xx123456987 0:926cd923a480 144 XL[2]=0+pi/3;
xx123456987 0:926cd923a480 145 break;
xx123456987 0:926cd923a480 146 }
xx123456987 0:926cd923a480 147
xx123456987 0:926cd923a480 148 printf("PWM Reset: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100);
xx123456987 0:926cd923a480 149 printf("Initial Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", Current_X[0]*100, Current_X[1]*100 ,Current_X[2]*180/pi-60);
xx123456987 0:926cd923a480 150 printf("Desired Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60);
xx123456987 0:926cd923a480 151 //printf("Press USER_BUTTON to START (Go to Direction '%c')\n", dire);
xx123456987 0:926cd923a480 152 printf("Go to Direction '%c')\n", dire);
xx123456987 0:926cd923a480 153 //while (mybutton == 1) {}
xx123456987 0:926cd923a480 154
xx123456987 0:926cd923a480 155 while(c>0) {
xx123456987 0:926cd923a480 156 wait(1);
xx123456987 0:926cd923a480 157 printf("%d\n",c--);
xx123456987 0:926cd923a480 158 myled = !myled;
xx123456987 0:926cd923a480 159 }
xx123456987 0:926cd923a480 160
xx123456987 0:926cd923a480 161 while(1) {
xx123456987 0:926cd923a480 162 //
xx123456987 0:926cd923a480 163 myled = 0;
xx123456987 0:926cd923a480 164 X1[0] = Current_X[0];
xx123456987 0:926cd923a480 165 X1[1] = Current_X[1];
xx123456987 0:926cd923a480 166 X1[2] = Current_X[2];
xx123456987 0:926cd923a480 167
xx123456987 0:926cd923a480 168 // odometry beginning //
xx123456987 0:926cd923a480 169 qei1_new = w1.getPulses();
xx123456987 0:926cd923a480 170 qei2_new = w2.getPulses();
xx123456987 0:926cd923a480 171 qei3_new = w3.getPulses();
xx123456987 0:926cd923a480 172
xx123456987 0:926cd923a480 173 double qei1 = qei1_new - qei1_old;
xx123456987 0:926cd923a480 174 double qei2 = qei2_new - qei2_old;
xx123456987 0:926cd923a480 175 double qei3 = qei3_new - qei3_old;
xx123456987 0:926cd923a480 176
xx123456987 0:926cd923a480 177 //wait_ms(50);
xx123456987 0:926cd923a480 178
xx123456987 0:926cd923a480 179 d_theta1 = qei1*360*pi/(4096*6*180); //DEG to RAD
xx123456987 0:926cd923a480 180 d_theta2 = qei2*360*pi/(4096*6*180);
xx123456987 0:926cd923a480 181 d_theta3 = qei3*360*pi/(4096*6*180);
xx123456987 0:926cd923a480 182 d_theta = c1*( d_theta1 + d_theta2 + d_theta3 );
xx123456987 0:926cd923a480 183
xx123456987 0:926cd923a480 184 // compute theta
xx123456987 0:926cd923a480 185 Next_X[2] = Current_X[2] + d_theta;
xx123456987 0:926cd923a480 186 double theta = Current_X[2];
xx123456987 0:926cd923a480 187 float c3 = Current_X[2] + d_theta/2;
xx123456987 0:926cd923a480 188 // compute y
xx123456987 0:926cd923a480 189 Next_X[1] = Current_X[1] + c2*(+d_theta1*cos(c3)-d_theta2*cos(pi/3-c3)-d_theta3*cos(pi/3+c3));
xx123456987 0:926cd923a480 190 // compute x
xx123456987 0:926cd923a480 191 Next_X[0] = Current_X[0] + c2*(-d_theta1*sin(c3)-d_theta2*sin(pi/3-c3)+d_theta3*sin(pi/3+c3));
xx123456987 0:926cd923a480 192
xx123456987 0:926cd923a480 193 // transition
xx123456987 0:926cd923a480 194 Current_X[2] = Next_X[2];
xx123456987 0:926cd923a480 195 Current_X[1] = Next_X[1];
xx123456987 0:926cd923a480 196 Current_X[0] = Next_X[0];
xx123456987 0:926cd923a480 197
xx123456987 0:926cd923a480 198 qei1_old = qei1_new;
xx123456987 0:926cd923a480 199 qei2_old = qei2_new;
xx123456987 0:926cd923a480 200 qei3_old = qei3_new;
xx123456987 0:926cd923a480 201 // odometry end //
xx123456987 0:926cd923a480 202
xx123456987 0:926cd923a480 203 // control law beginning //
xx123456987 0:926cd923a480 204 //#define r 0.05
xx123456987 0:926cd923a480 205 //#define L 0.125
xx123456987 0:926cd923a480 206
xx123456987 0:926cd923a480 207 X1_b[0] = X1[0]-f1[0];
xx123456987 0:926cd923a480 208 X1_b[1] = X1[1]-f1[1];
xx123456987 0:926cd923a480 209 X1_b[2] = X1[2]-f1[2];
xx123456987 0:926cd923a480 210 u[0] = -c0*(X1_b[0]-XL[0]);
xx123456987 0:926cd923a480 211 u[1] = -c0*(X1_b[1]-XL[1]);
xx123456987 0:926cd923a480 212 u[2] = -c0*(X1_b[2]-XL[2]);
xx123456987 0:926cd923a480 213 //
xx123456987 0:926cd923a480 214 omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta);
xx123456987 0:926cd923a480 215 omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta);
xx123456987 0:926cd923a480 216 omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta);
xx123456987 0:926cd923a480 217 // control law end //
xx123456987 0:926cd923a480 218
xx123456987 0:926cd923a480 219 //theta = theta + pi/3; // shift: origin to new
xx123456987 0:926cd923a480 220
xx123456987 0:926cd923a480 221 // omega to PWM //
xx123456987 0:926cd923a480 222 pwm_w1 = 0.5 + omg1/2;
xx123456987 0:926cd923a480 223 pwm_w2 = 0.5 + omg2/2;
xx123456987 0:926cd923a480 224 pwm_w3 = 0.5 + omg3/2;
xx123456987 0:926cd923a480 225
xx123456987 0:926cd923a480 226 pwm_1.write(pwm_w1);
xx123456987 0:926cd923a480 227 pwm_2.write(pwm_w2);
xx123456987 0:926cd923a480 228 pwm_3.write(pwm_w3);
xx123456987 0:926cd923a480 229 // END //
xx123456987 0:926cd923a480 230
xx123456987 0:926cd923a480 231 // define error // not abs() yet
xx123456987 0:926cd923a480 232 err[0] = Current_X[0]-XL[0];
xx123456987 0:926cd923a480 233 err[1] = Current_X[1]-XL[1];
xx123456987 0:926cd923a480 234 err[2] = Current_X[2]-XL[2];
xx123456987 0:926cd923a480 235 // END //
xx123456987 0:926cd923a480 236 if ( t.read_ms() > 200) { // print
xx123456987 0:926cd923a480 237
xx123456987 0:926cd923a480 238 t.reset();
xx123456987 0:926cd923a480 239 t.start();
xx123456987 0:926cd923a480 240 if (s0 == true) printf ("=================Homing====================\n");
xx123456987 0:926cd923a480 241 else printf ("===========================================\n");
xx123456987 0:926cd923a480 242 printf ("Current Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", Current_X[0]*100, Current_X[1]*100, Current_X[2]*180/pi-60);
xx123456987 0:926cd923a480 243 printf ("Error : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", (err[0])*100, (err[1])*100, (err[2])*180/pi);
xx123456987 0:926cd923a480 244 /*
xx123456987 0:926cd923a480 245 printf ("\nWheel[1] counts: %d | revolutions: % .2f", qei1_new, (float)qei1_new/(4096*6));
xx123456987 0:926cd923a480 246 printf ("\nWheel[2] counts: %d | revolutions: % .2f", qei2_new, (float)qei2_new/(4096*6));
xx123456987 0:926cd923a480 247 printf ("\nWheel[3] counts: %d | revolutions: % .2f", qei3_new, (float)qei3_new/(4096*6));
xx123456987 0:926cd923a480 248 */
xx123456987 0:926cd923a480 249 //printf("Wheel Vel. [%3.2f %3.2f %3.2f]\n",pwm_w1,pwm_w2,pwm_w3);
xx123456987 0:926cd923a480 250 printf("OMEGA: [%.2f %.2f %.2f]\n",omg1,omg2,omg3);
xx123456987 0:926cd923a480 251 printf("PWM: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100);
xx123456987 0:926cd923a480 252 //printf("X: [%.3f] Y:[%.3f]\n",px,py);
xx123456987 0:926cd923a480 253 //printf("px+py=%.2f\n", px+py);
xx123456987 0:926cd923a480 254
xx123456987 0:926cd923a480 255 }
xx123456987 0:926cd923a480 256 if ( abs(err[0])<=(ex/100) && abs(err[1])<=(ey/100) && abs(err[2])<=(etheta*pi/180) ) {
xx123456987 0:926cd923a480 257 printf("The robot Arrived!\n");
xx123456987 0:926cd923a480 258 pwm_1.write(0.50);
xx123456987 0:926cd923a480 259 pwm_2.write(0.50);
xx123456987 0:926cd923a480 260 pwm_3.write(0.50);
xx123456987 0:926cd923a480 261 //s0 = true;
xx123456987 0:926cd923a480 262 c = 3;
xx123456987 0:926cd923a480 263 goto aa;
xx123456987 0:926cd923a480 264 /*
xx123456987 0:926cd923a480 265 printf("Press USER_BUTTON to go HOME\n");
xx123456987 0:926cd923a480 266 while (mybutton == 1) {
xx123456987 0:926cd923a480 267 wait(0.25);
xx123456987 0:926cd923a480 268 myled = !myled;
xx123456987 0:926cd923a480 269
xx123456987 0:926cd923a480 270 }
xx123456987 0:926cd923a480 271
xx123456987 0:926cd923a480 272 while(c>0) {
xx123456987 0:926cd923a480 273 wait(1);
xx123456987 0:926cd923a480 274 printf("%d\n",c--);
xx123456987 0:926cd923a480 275 myled = !myled;
xx123456987 0:926cd923a480 276 }
xx123456987 0:926cd923a480 277 XL[0]=0;
xx123456987 0:926cd923a480 278 XL[1]=0;
xx123456987 0:926cd923a480 279 XL[2]=0+pi/3;
xx123456987 0:926cd923a480 280 */
xx123456987 0:926cd923a480 281 }
xx123456987 0:926cd923a480 282
xx123456987 0:926cd923a480 283 wait_ms(10);// 會有什麼影響?
xx123456987 0:926cd923a480 284 }
xx123456987 0:926cd923a480 285 }