yi li
20160725
Diff: main.cpp
- Revision:
- 0:926cd923a480
- Child:
- 1:a78e5f6154d3
--- /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);// 會有什麼影響?
+ }
+}