AECL 601
Robot 1
Diff: main.cpp
- Revision:
- 0:f0f040a29912
- Child:
- 1:1b3714ce6cf9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Aug 03 06:45:31 2016 +0000
@@ -0,0 +1,411 @@
+/* 版本說明: 正方形軌跡追蹤(從邊緣出發)邊長3m
+實驗說明:
+ 正式實驗:leader為正方形軌跡
+ followers維持等腰三角形隊形
+
+實驗規格:
+
+07/17測試紀錄:
+
+*/
+#include "mbed.h"
+#include "math.h"
+#include "QEI.h"
+// pre-define
+#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 kp 0.7// p gain
+#define kd 0.03// d gain
+// leader initial pose
+#define x0 0 //[m]
+#define y0 0 //[m]
+#define theta0 0//[deg.]
+// desied pose
+#define xd 0 //[cm]
+#define yd 0 //[cm]
+#define thetad 0//[deg.]
+// error threshold
+#define ex 0.02 //[m]
+#define ey 0.02 //[m]
+#define etheta 3//[deg.]
+
+PwmOut pwm_1(D9);
+PwmOut pwm_2(D10);
+PwmOut pwm_3(D11);
+
+Timer clk;
+
+// define
+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);
+// formation para.
+//float x=14.43;
+float f1[3] = {0,1.5,0}; // formation vector
+// odomerty para.
+float c1 = r/(3*L), c2 = 2*r/3;
+float Current_X[3] = {x0+f1[0],y0+f1[1],((theta0+f1[2])*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, qei1_old, qei2_old, qei3_old;
+// control law para.
+
+float X1[3],V1[3],Vc[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;
+// Differential
+float dy,dx,dtheta;
+float dt = 0.05;
+// V command
+float t1 = 7.5;//2*3.75
+float T = 22.5;//6*3.75
+float S = 3.0;//0.8*3.75
+// Others
+DigitalIn mybutton(USER_BUTTON);
+DigitalOut myled(LED1);
+int c = 3;
+int xdir,ydir; // x y direction
+float err[3]= {0};
+//bool s0 = false;
+bool s0 = true;
+bool stop = false;
+char gosw = true;
+int sw = 13;
+float xL0,yL0;// initial position of leader
+char kb='p';
+
+int main()
+{
+ 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("3W_1_fmt_squ_0717\n");
+ printf("Press any key to START\n");
+
+ while (kb=='p') {
+ scanf("%c",&kb);
+ }
+
+ //if (s0 == ture)
+ s0 = true;
+ /*
+ while(c>0) {
+ wait(1);
+ c--;
+ myled = !myled;
+ }
+ */
+ xL0=0.0;// dire. 9
+ yL0=0.0;
+ //clk.reset();
+ clk.start();
+ while(1) {
+ //
+ myled = 0;
+ X1[0] = Current_X[0];
+ X1[1] = Current_X[1];
+ X1[2] = Current_X[2];
+
+ // P&V command 1→3→9→7→1
+ if(s0==true) {
+ if(gosw==true) {
+ switch (sw) {
+ case 97:
+ xdir=-1;
+ ydir=0;
+ sw=71;
+ break;
+ case 71:
+ xdir=0;
+ ydir=-1;
+ sw=13;
+ break;
+ case 13:
+ xdir=1;
+ ydir=0;
+ sw=39;
+ break;
+ case 39:
+ xdir=0;
+ ydir=1;
+ sw=97;
+ stop=true;
+ break;
+ default:
+ printf("err.");
+ }
+ }
+ gosw=false;
+ clk.start();
+ if(clk.read()<t1) {//1
+ Vc[0]=(2*clk.read()/75)*xdir;
+ Vc[1]=(2*clk.read()/75)*ydir;
+ Vc[2]=0;
+ //
+ XL[0]=xL0 + (clk.read()*clk.read()/75)*xdir;// go to target
+ XL[1]=yL0 + (clk.read()*clk.read()/75)*ydir;
+ XL[2]=0+pi/3;
+ } else if(clk.read()>t1 && clk.read()<(T-t1) ) {//2
+ Vc[0]=0.2*xdir;
+ Vc[1]=0.2*ydir;
+ Vc[2]=0;
+ //
+ XL[0]=xL0 + (clk.read()/5 - 0.75)*xdir;
+ XL[1]=yL0 + (clk.read()/5 - 0.75)*ydir;
+ XL[2]=0+pi/3;
+ } else if (clk.read()>T) {//4
+ Vc[0]=0;
+ Vc[1]=0;
+ Vc[2]=0;
+ //
+ XL[0]=xL0+S*xdir;
+ XL[1]=yL0+S*ydir;
+ XL[2]=0+pi/3;
+ clk.reset();
+ xL0=XL[0];
+ yL0=XL[1];
+ gosw=true;
+ } else {//3
+ Vc[0]=(0.6-2*clk.read()/75)*xdir;
+ Vc[1]=(0.6-2*clk.read()/75)*ydir;
+ Vc[2]=0;
+ //
+ XL[0]=xL0 + (-((2*clk.read()/75 - 0.8)*(clk.read() - 15))/2 + 2.25)*xdir;
+ XL[1]=yL0 + (-((2*clk.read()/75 - 0.8)*(clk.read() - 15))/2 + 2.25)*ydir;
+ XL[2]=0+pi/3;
+ }
+ }
+ //printf("% .2f|% .2f|%f|X V cmd(xdire)\n", XL[0]*100, Vc[0]*100, clk.read());
+
+ // 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));
+
+ // compute velocity
+ dx =Next_X[0]-Current_X[0];
+ dy =Next_X[1]-Current_X[1];
+ dtheta =Next_X[2]-Current_X[2];
+ V1[0]=dx/dt;
+ V1[1]=dy/dt;
+ V1[2]=dtheta/dt;
+ // 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 //
+
+
+ //printf ("% .1f|% .1f|% .1f|%f|Vc\n", Vc[0]*100, Vc[1]*100, Vc[2]*180/pi,clk.read());
+ // 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] = -kp*(X1_b[0]-XL[0])-kd*(V1[0]-Vc[0]);
+ u[1] = -kp*(X1_b[1]-XL[1])-kd*(V1[1]-Vc[1]);
+ u[2] = -kp*(X1_b[2]-XL[2])-kd*(V1[2]-Vc[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 //
+
+ // 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]-f1[0])-(0.0);
+ err[1] = (Current_X[1]-f1[1])-(0.0);
+ err[2] = (Current_X[2]-f1[2])-XL[2];
+
+ if ( abs(err[0])<ex && abs(err[1])<ey && abs(err[2])<(etheta*pi/180) && stop==true ) {
+ printf("Arrived!\n");
+ pwm_1.write(0.50);
+ pwm_2.write(0.50);
+ pwm_3.write(0.50);
+ while (mybutton == 1) {}
+ }
+
+ //print
+ //printf ("\n");
+ //printf("% .2f|% .2f|% .1f|Xnow|%f\n", Current_X[0]*100, Current_X[1]*100,Current_X[2]*180/pi-60,clk.read());
+ printf("% .2f|% .2f|% .1f|Xnow|% .2f|% .2f|% .1f|XL|%f\n",
+ Current_X[0]*100, Current_X[1]*100,Current_X[2]*180/pi-60,
+ XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60,clk.read());
+ //printf("% .1f|% .1f|% .1f|Xc|%f\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60,clk.read());
+ ///printf ("% .2f|% .2f|% .1f|Vnow\n", V1[0]*100, V1[1]*100, V1[2]*180/pi);
+ //printf("%f\n",clk.read());
+ //printf("% .1f|% .1f|% .1f|Xc \n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60);
+ //printf("% .2f|% .2f|% .1f|Vc \n", Vc[0]*100, Vc[1]*100, Vc[2]*180/pi);
+ //printf ("% .2f|% .2f|%f|now\n", Current_X[0]*100, V1[0]*100,clk.read());
+ //printf("% .2f|% .2f|%f|X V cmd(xdire)\n", XL[0]*100, Vc[0]*100, clk.read());//測命令用
+ //printf("% .2f|% .2f|%f|X V cmd(ydire)\n", XL[1]*100, Vc[1]*100, clk.read());
+ //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("%f\n",clk.read());
+
+ //}
+
+ // arrive?
+ wait_ms(50);//
+ }
+}
+
+/*
+ switch (sw) {
+ case '1':
+ tempchar = sw;
+ xdir=-1;
+ ydir=-1;
+ sw = '2';
+ break;
+ case '2':
+ tempchar = sw;
+ xdir=0;
+ ydir=-1;
+ sw = '3';
+ break;
+ case '3':
+ tempchar = sw;
+ xdir=1;
+ ydir=-1;
+ sw = '6';
+ break;
+ case '4':
+ tempchar = sw;
+ xdir=-1;
+ ydir=0;
+ sw = '1';
+ break;
+ case '5':
+ tempchar = sw;
+
+ xdir=0;
+ ydir=0;
+ XL[2]=0+pi/3;
+ break;
+ case '6':
+ tempchar = sw;
+ xdir=1;
+ ydir=0;
+ sw = '9';
+ break;
+ case '7':
+ tempchar = sw;
+ xdir=-1;
+ ydir=1;
+ sw = '4';
+ break;
+ case '8':
+ tempchar = sw;
+ xdir=0;
+ ydir=1;
+ sw = '7';
+ break;
+ case '9':
+ tempchar = sw;
+ xdir=1;
+ ydir=1;
+ sw = '8';
+ break;
+ default:
+ xdir=0;
+ ydir=0;
+ XL[2]=0+pi/3;
+ break;
+ }*/
+
+// P&V command compensation
+/*if(s0==false) {
+ if(clk.read()<1) {
+ Vc[0]=0.15;
+ Vc[1]=0.05+0.1*clk.read();
+ Vc[2]=0;
+ //
+ XL[0]=0.15*clk.read();// go to target
+ XL[1]=0.05*clk.read()+0.05*clk.read()*clk.read();
+ XL[2]=0+pi/3;
+ } else if(clk.read()>1 && clk.read()<2) {
+ Vc[0]=0.15;
+ Vc[1]=0.15;
+ Vc[2]=0;
+ //
+ XL[0]=0.15*clk.read();// go to target
+ XL[1]=0.1+0.15*(clk.read()-1);
+ XL[2]=0+pi/3;
+ } else if(clk.read()>2 && clk.read()<3) {
+ Vc[0]=0.15-0.1*(clk.read()-2);
+ Vc[1]=0.15;
+ Vc[2]=0;
+ //
+ XL[0]=0.3+(0.3-0.1*(clk.read()-2))*(clk.read()-2)/2;// go to target
+ XL[1]=0.25+0.15*(clk.read()-2);
+ XL[2]=0+pi/3;
+ } else {
+ Vc[0]=0;
+ Vc[1]=0;
+ Vc[2]=0;
+ //
+ XL[0]=0.4;// go to target
+ XL[1]=0.4;
+ XL[2]=0+pi/3;
+ s0=true;
+ clk.reset();
+ }
+}*/
\ No newline at end of file