yi li
/
3W_wifi_odo_p2p_0722
20160725
Diff: main.cpp
- Revision:
- 0:926cd923a480
- Child:
- 1:a78e5f6154d3
diff -r 000000000000 -r 926cd923a480 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jul 22 07:57:56 2016 +0000 @@ -0,0 +1,285 @@ +/* 版本說明: 可1~9任選,但scanf有問題 +實驗說明: + 利用運動學模型並以靜止的virtual leader當作輸入命令,達到point to point控制,來驗證里程計的正確性。 + =八個方位的測試= + 7 8 9 + ↖ ↑ ↗ + 4← . →6 + ↙ ↓ ↘ + 1 2 3 +07/07測試結果: + 實驗成功但略有誤差 +*/ +#include "mbed.h" +#include "math.h" +#include "QEI.h" +// definition +#define pi 3.1416 +#define pwm_period_us 500 +#define r 0.05 // wheel radius [m] +#define L 0.125 // distance between wheel center & geometric center [m] +#define pulsesPerRev 1024 +#define c0 0.06// control gain + +// initial pose +#define x0 0 //[m] +#define y0 0 //[m] +#define theta0 0//[deg.] +// desied pose +#define xd 0.5 //[cm] +#define yd 0.5 //[cm] +#define thetad 0//[deg.] +// error threshold +#define ex 0.5 //[cm] +#define ey 0.5 //[cm] +#define etheta 1//[deg.] + +PwmOut pwm_1(D9); +PwmOut pwm_2(D10); +PwmOut pwm_3(D11); + +Timer t; // timer for printing + +QEI w1 (D2, D3, NC, pulsesPerRev, QEI::X4_ENCODING); +QEI w2 (D4, D5, NC, pulsesPerRev, QEI::X4_ENCODING); +QEI w3 (D14, D15, NC, pulsesPerRev, QEI::X4_ENCODING); +// odomerty para. +float c1 = r/(3*L); +float c2 = 2*r/3; +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 +float Next_X[3] = {0,0,0}; +double d_theta1,d_theta2,d_theta3,d_theta; +int qei1_new, qei2_new, qei3_new; +int qei1_old, qei2_old, qei3_old; +// control law para. +float f1[3] = {0}; // formation vector +float X1[3],X1_b[3];// X1_b(X_BAR) is defined as X1-f1 +float XL[3] = {xd,yd,(thetad*pi/180)+pi/3};// pose of virtual leader [m m rad.] +float u[3] = {0};// control law +float omg1,omg2,omg3;// velocity of wheels +// PWM +float pwm_w1,pwm_w2,pwm_w3; +// Others +DigitalIn mybutton(USER_BUTTON); +DigitalOut myled(LED1); +int c = 3; +float err[3]= {0}; +bool s0 = false; +char dire; + + +int main() +{ + + t.start(); + + pwm_1.period_us(pwm_period_us); + pwm_2.period_us(pwm_period_us); + pwm_3.period_us(pwm_period_us); + + pwm_1.write(0.50); + pwm_2.write(0.50); + pwm_3.write(0.50); + + qei1_old = 0; + qei2_old = 0; + qei3_old = 0; + printf("===================Reset===================\n"); +aa: { + + printf("Which direction to go?\n"); + printf(" 7 8 9 \n"); + printf(" | \n"); + printf(" 4- 5 -6 \n"); + printf(" | \n"); + printf(" 1 2 3 \n"); + myled = 1; + } + + scanf("%c", &dire); + switch (dire) { + case '1': + XL[0]=-0.4; + XL[1]=-0.4; + XL[2]=0+pi/3; + break; + case '2': + XL[0]=0; + XL[1]=-0.4; + XL[2]=0+pi/3; + break; + case '3': + XL[0]=0.4; + XL[1]=-0.4; + XL[2]=0+pi/3; + break; + case '4': + XL[0]=-0.4; + XL[1]=0; + XL[2]=0+pi/3; + break; + case '5': + XL[0]=0; + XL[1]=0; + XL[2]=0+pi/3; + break; + case '6': + XL[0]=0.4; + XL[1]=0; + XL[2]=0+pi/3; + break; + case '7': + XL[0]=-0.4; + XL[1]=0.4; + XL[2]=0+pi/3; + break; + case '8': + XL[0]=0; + XL[1]=0.4; + XL[2]=0+pi/3; + break; + case '9': + XL[0]=0.4; + XL[1]=0.4; + XL[2]=0+pi/3; + break; + } + + printf("PWM Reset: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100); + printf("Initial Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", Current_X[0]*100, Current_X[1]*100 ,Current_X[2]*180/pi-60); + printf("Desired Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60); + //printf("Press USER_BUTTON to START (Go to Direction '%c')\n", dire); + printf("Go to Direction '%c')\n", dire); + //while (mybutton == 1) {} + + while(c>0) { + wait(1); + printf("%d\n",c--); + myled = !myled; + } + + while(1) { + // + myled = 0; + X1[0] = Current_X[0]; + X1[1] = Current_X[1]; + X1[2] = Current_X[2]; + + // odometry beginning // + qei1_new = w1.getPulses(); + qei2_new = w2.getPulses(); + qei3_new = w3.getPulses(); + + double qei1 = qei1_new - qei1_old; + double qei2 = qei2_new - qei2_old; + double qei3 = qei3_new - qei3_old; + + //wait_ms(50); + + d_theta1 = qei1*360*pi/(4096*6*180); //DEG to RAD + d_theta2 = qei2*360*pi/(4096*6*180); + d_theta3 = qei3*360*pi/(4096*6*180); + d_theta = c1*( d_theta1 + d_theta2 + d_theta3 ); + + // compute theta + Next_X[2] = Current_X[2] + d_theta; + double theta = Current_X[2]; + float c3 = Current_X[2] + d_theta/2; + // compute y + Next_X[1] = Current_X[1] + c2*(+d_theta1*cos(c3)-d_theta2*cos(pi/3-c3)-d_theta3*cos(pi/3+c3)); + // compute x + Next_X[0] = Current_X[0] + c2*(-d_theta1*sin(c3)-d_theta2*sin(pi/3-c3)+d_theta3*sin(pi/3+c3)); + + // transition + Current_X[2] = Next_X[2]; + Current_X[1] = Next_X[1]; + Current_X[0] = Next_X[0]; + + qei1_old = qei1_new; + qei2_old = qei2_new; + qei3_old = qei3_new; + // odometry end // + + // control law beginning // + //#define r 0.05 + //#define L 0.125 + + X1_b[0] = X1[0]-f1[0]; + X1_b[1] = X1[1]-f1[1]; + X1_b[2] = X1[2]-f1[2]; + u[0] = -c0*(X1_b[0]-XL[0]); + u[1] = -c0*(X1_b[1]-XL[1]); + u[2] = -c0*(X1_b[2]-XL[2]); + // + omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta); + omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta); + omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta); + // control law end // + + //theta = theta + pi/3; // shift: origin to new + + // omega to PWM // + pwm_w1 = 0.5 + omg1/2; + pwm_w2 = 0.5 + omg2/2; + pwm_w3 = 0.5 + omg3/2; + + pwm_1.write(pwm_w1); + pwm_2.write(pwm_w2); + pwm_3.write(pwm_w3); + // END // + + // define error // not abs() yet + err[0] = Current_X[0]-XL[0]; + err[1] = Current_X[1]-XL[1]; + err[2] = Current_X[2]-XL[2]; + // END // + if ( t.read_ms() > 200) { // print + + t.reset(); + t.start(); + if (s0 == true) printf ("=================Homing====================\n"); + else printf ("===========================================\n"); + printf ("Current Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", Current_X[0]*100, Current_X[1]*100, Current_X[2]*180/pi-60); + printf ("Error : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", (err[0])*100, (err[1])*100, (err[2])*180/pi); + /* + printf ("\nWheel[1] counts: %d | revolutions: % .2f", qei1_new, (float)qei1_new/(4096*6)); + printf ("\nWheel[2] counts: %d | revolutions: % .2f", qei2_new, (float)qei2_new/(4096*6)); + printf ("\nWheel[3] counts: %d | revolutions: % .2f", qei3_new, (float)qei3_new/(4096*6)); + */ + //printf("Wheel Vel. [%3.2f %3.2f %3.2f]\n",pwm_w1,pwm_w2,pwm_w3); + printf("OMEGA: [%.2f %.2f %.2f]\n",omg1,omg2,omg3); + printf("PWM: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100); + //printf("X: [%.3f] Y:[%.3f]\n",px,py); + //printf("px+py=%.2f\n", px+py); + + } + if ( abs(err[0])<=(ex/100) && abs(err[1])<=(ey/100) && abs(err[2])<=(etheta*pi/180) ) { + printf("The robot Arrived!\n"); + pwm_1.write(0.50); + pwm_2.write(0.50); + pwm_3.write(0.50); + //s0 = true; + c = 3; + goto aa; + /* + printf("Press USER_BUTTON to go HOME\n"); + while (mybutton == 1) { + wait(0.25); + myled = !myled; + + } + + while(c>0) { + wait(1); + printf("%d\n",c--); + myled = !myled; + } + XL[0]=0; + XL[1]=0; + XL[2]=0+pi/3; + */ + } + + wait_ms(10);// 會有什麼影響? + } +}