AECL 601 / Mbed 2 deprecated 3W_1_fmt_squ_0717

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   QEI mbed

main.cpp@1:1b3714ce6cf9, 2016-09-09 (annotated)

Committer:
xx123456987
Date:
Fri Sep 09 04:42:43 2016 +0000
Revision:
1:1b3714ce6cf9
Parent:
0:f0f040a29912
Child:
2:16af99e96778
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xx123456987 0:f0f040a29912 1 /* 版本說明: 正方形軌跡追蹤(從邊緣出發)邊長3m
xx123456987 0:f0f040a29912 2 實驗說明:
xx123456987 0:f0f040a29912 3 正式實驗:leader為正方形軌跡
xx123456987 0:f0f040a29912 4 followers維持等腰三角形隊形
xx123456987 0:f0f040a29912 5
xx123456987 0:f0f040a29912 6 實驗規格:
xx123456987 0:f0f040a29912 7
xx123456987 0:f0f040a29912 8 07/17測試紀錄:
xx123456987 0:f0f040a29912 9
xx123456987 0:f0f040a29912 10 */
xx123456987 0:f0f040a29912 11 #include "mbed.h"
xx123456987 0:f0f040a29912 12 #include "math.h"
xx123456987 0:f0f040a29912 13 #include "QEI.h"
xx123456987 0:f0f040a29912 14 // pre-define
xx123456987 0:f0f040a29912 15 #define pi 3.1416
xx123456987 0:f0f040a29912 16 #define pwm_period_us 500
xx123456987 0:f0f040a29912 17 #define r 0.05 // wheel radius [m]
xx123456987 0:f0f040a29912 18 #define L 0.125 // distance between wheel center & geometric center [m]
xx123456987 0:f0f040a29912 19 #define pulsesPerRev 1024
xx123456987 0:f0f040a29912 20 #define kp 0.7// p gain
xx123456987 0:f0f040a29912 21 #define kd 0.03// d gain
xx123456987 0:f0f040a29912 22 // leader initial pose
xx123456987 0:f0f040a29912 23 #define x0 0 //[m]
xx123456987 0:f0f040a29912 24 #define y0 0 //[m]
xx123456987 0:f0f040a29912 25 #define theta0 0//[deg.]
xx123456987 0:f0f040a29912 26 // desied pose
xx123456987 0:f0f040a29912 27 #define xd 0 //[cm]
xx123456987 0:f0f040a29912 28 #define yd 0 //[cm]
xx123456987 0:f0f040a29912 29 #define thetad 0//[deg.]
xx123456987 0:f0f040a29912 30 // error threshold
xx123456987 0:f0f040a29912 31 #define ex 0.02 //[m]
xx123456987 0:f0f040a29912 32 #define ey 0.02 //[m]
xx123456987 0:f0f040a29912 33 #define etheta 3//[deg.]
xx123456987 1:1b3714ce6cf9 34 /*
xx123456987 0:f0f040a29912 35 PwmOut pwm_1(D9);
xx123456987 0:f0f040a29912 36 PwmOut pwm_2(D10);
xx123456987 0:f0f040a29912 37 PwmOut pwm_3(D11);
xx123456987 1:1b3714ce6cf9 38 */
xx123456987 1:1b3714ce6cf9 39 PwmOut pwm_1(D8);
xx123456987 1:1b3714ce6cf9 40 PwmOut pwm_2(D9);
xx123456987 1:1b3714ce6cf9 41 PwmOut pwm_3(D7);
xx123456987 0:f0f040a29912 42
xx123456987 0:f0f040a29912 43 Timer clk;
xx123456987 0:f0f040a29912 44
xx123456987 0:f0f040a29912 45 // define
xx123456987 0:f0f040a29912 46 QEI w1 (D2, D3, NC, pulsesPerRev, QEI::X4_ENCODING);
xx123456987 0:f0f040a29912 47 QEI w2 (D4, D5, NC, pulsesPerRev, QEI::X4_ENCODING);
xx123456987 0:f0f040a29912 48 QEI w3 (D14, D15, NC, pulsesPerRev, QEI::X4_ENCODING);
xx123456987 0:f0f040a29912 49 // formation para.
xx123456987 0:f0f040a29912 50 //float x=14.43;
xx123456987 0:f0f040a29912 51 float f1[3] = {0,1.5,0}; // formation vector
xx123456987 0:f0f040a29912 52 // odomerty para.
xx123456987 0:f0f040a29912 53 float c1 = r/(3*L), c2 = 2*r/3;
xx123456987 0:f0f040a29912 54 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
xx123456987 0:f0f040a29912 55 float Next_X[3] = {0,0,0};
xx123456987 0:f0f040a29912 56 double d_theta1,d_theta2,d_theta3,d_theta;
xx123456987 0:f0f040a29912 57 int qei1_new, qei2_new, qei3_new, qei1_old, qei2_old, qei3_old;
xx123456987 0:f0f040a29912 58 // control law para.
xx123456987 0:f0f040a29912 59
xx123456987 0:f0f040a29912 60 float X1[3],V1[3],Vc[3],X1_b[3];// X1_b(X_BAR) is defined as X1-f1
xx123456987 0:f0f040a29912 61 float XL[3];// = {xd,yd,(thetad*pi/180)+pi/3};// pose of virtual leader [m m rad.]
xx123456987 0:f0f040a29912 62 float u[3] = {0};// control law
xx123456987 0:f0f040a29912 63 float omg1,omg2,omg3;// velocity of wheels
xx123456987 0:f0f040a29912 64 // PWM
xx123456987 0:f0f040a29912 65 float pwm_w1,pwm_w2,pwm_w3;
xx123456987 0:f0f040a29912 66 // Differential
xx123456987 0:f0f040a29912 67 float dy,dx,dtheta;
xx123456987 0:f0f040a29912 68 float dt = 0.05;
xx123456987 0:f0f040a29912 69 // V command
xx123456987 0:f0f040a29912 70 float t1 = 7.5;//2*3.75
xx123456987 0:f0f040a29912 71 float T = 22.5;//6*3.75
xx123456987 0:f0f040a29912 72 float S = 3.0;//0.8*3.75
xx123456987 0:f0f040a29912 73 // Others
xx123456987 0:f0f040a29912 74 DigitalIn mybutton(USER_BUTTON);
xx123456987 0:f0f040a29912 75 DigitalOut myled(LED1);
xx123456987 0:f0f040a29912 76 int c = 3;
xx123456987 0:f0f040a29912 77 int xdir,ydir; // x y direction
xx123456987 0:f0f040a29912 78 float err[3]= {0};
xx123456987 0:f0f040a29912 79 //bool s0 = false;
xx123456987 0:f0f040a29912 80 bool s0 = true;
xx123456987 0:f0f040a29912 81 bool stop = false;
xx123456987 0:f0f040a29912 82 char gosw = true;
xx123456987 0:f0f040a29912 83 int sw = 13;
xx123456987 0:f0f040a29912 84 float xL0,yL0;// initial position of leader
xx123456987 0:f0f040a29912 85 char kb='p';
xx123456987 0:f0f040a29912 86
xx123456987 0:f0f040a29912 87 int main()
xx123456987 0:f0f040a29912 88 {
xx123456987 0:f0f040a29912 89 pwm_1.period_us(pwm_period_us);
xx123456987 0:f0f040a29912 90 pwm_2.period_us(pwm_period_us);
xx123456987 0:f0f040a29912 91 pwm_3.period_us(pwm_period_us);
xx123456987 0:f0f040a29912 92
xx123456987 0:f0f040a29912 93 pwm_1.write(0.50);
xx123456987 0:f0f040a29912 94 pwm_2.write(0.50);
xx123456987 0:f0f040a29912 95 pwm_3.write(0.50);
xx123456987 0:f0f040a29912 96
xx123456987 0:f0f040a29912 97 qei1_old = 0;
xx123456987 0:f0f040a29912 98 qei2_old = 0;
xx123456987 0:f0f040a29912 99 qei3_old = 0;
xx123456987 0:f0f040a29912 100 printf("3W_1_fmt_squ_0717\n");
xx123456987 0:f0f040a29912 101 printf("Press any key to START\n");
xx123456987 0:f0f040a29912 102
xx123456987 0:f0f040a29912 103 while (kb=='p') {
xx123456987 0:f0f040a29912 104 scanf("%c",&kb);
xx123456987 0:f0f040a29912 105 }
xx123456987 0:f0f040a29912 106
xx123456987 0:f0f040a29912 107 //if (s0 == ture)
xx123456987 0:f0f040a29912 108 s0 = true;
xx123456987 0:f0f040a29912 109 /*
xx123456987 0:f0f040a29912 110 while(c>0) {
xx123456987 0:f0f040a29912 111 wait(1);
xx123456987 0:f0f040a29912 112 c--;
xx123456987 0:f0f040a29912 113 myled = !myled;
xx123456987 0:f0f040a29912 114 }
xx123456987 0:f0f040a29912 115 */
xx123456987 0:f0f040a29912 116 xL0=0.0;// dire. 9
xx123456987 0:f0f040a29912 117 yL0=0.0;
xx123456987 0:f0f040a29912 118 //clk.reset();
xx123456987 0:f0f040a29912 119 clk.start();
xx123456987 0:f0f040a29912 120 while(1) {
xx123456987 0:f0f040a29912 121 //
xx123456987 0:f0f040a29912 122 myled = 0;
xx123456987 0:f0f040a29912 123 X1[0] = Current_X[0];
xx123456987 0:f0f040a29912 124 X1[1] = Current_X[1];
xx123456987 0:f0f040a29912 125 X1[2] = Current_X[2];
xx123456987 0:f0f040a29912 126
xx123456987 0:f0f040a29912 127 // P&V command 1→3→9→7→1
xx123456987 0:f0f040a29912 128 if(s0==true) {
xx123456987 0:f0f040a29912 129 if(gosw==true) {
xx123456987 0:f0f040a29912 130 switch (sw) {
xx123456987 0:f0f040a29912 131 case 97:
xx123456987 0:f0f040a29912 132 xdir=-1;
xx123456987 0:f0f040a29912 133 ydir=0;
xx123456987 0:f0f040a29912 134 sw=71;
xx123456987 0:f0f040a29912 135 break;
xx123456987 0:f0f040a29912 136 case 71:
xx123456987 0:f0f040a29912 137 xdir=0;
xx123456987 0:f0f040a29912 138 ydir=-1;
xx123456987 0:f0f040a29912 139 sw=13;
xx123456987 0:f0f040a29912 140 break;
xx123456987 0:f0f040a29912 141 case 13:
xx123456987 0:f0f040a29912 142 xdir=1;
xx123456987 0:f0f040a29912 143 ydir=0;
xx123456987 0:f0f040a29912 144 sw=39;
xx123456987 0:f0f040a29912 145 break;
xx123456987 0:f0f040a29912 146 case 39:
xx123456987 0:f0f040a29912 147 xdir=0;
xx123456987 0:f0f040a29912 148 ydir=1;
xx123456987 0:f0f040a29912 149 sw=97;
xx123456987 0:f0f040a29912 150 stop=true;
xx123456987 0:f0f040a29912 151 break;
xx123456987 0:f0f040a29912 152 default:
xx123456987 0:f0f040a29912 153 printf("err.");
xx123456987 0:f0f040a29912 154 }
xx123456987 0:f0f040a29912 155 }
xx123456987 0:f0f040a29912 156 gosw=false;
xx123456987 0:f0f040a29912 157 clk.start();
xx123456987 0:f0f040a29912 158 if(clk.read()<t1) {//1
xx123456987 0:f0f040a29912 159 Vc[0]=(2*clk.read()/75)*xdir;
xx123456987 0:f0f040a29912 160 Vc[1]=(2*clk.read()/75)*ydir;
xx123456987 0:f0f040a29912 161 Vc[2]=0;
xx123456987 0:f0f040a29912 162 //
xx123456987 0:f0f040a29912 163 XL[0]=xL0 + (clk.read()*clk.read()/75)*xdir;// go to target
xx123456987 0:f0f040a29912 164 XL[1]=yL0 + (clk.read()*clk.read()/75)*ydir;
xx123456987 0:f0f040a29912 165 XL[2]=0+pi/3;
xx123456987 0:f0f040a29912 166 } else if(clk.read()>t1 && clk.read()<(T-t1) ) {//2
xx123456987 0:f0f040a29912 167 Vc[0]=0.2*xdir;
xx123456987 0:f0f040a29912 168 Vc[1]=0.2*ydir;
xx123456987 0:f0f040a29912 169 Vc[2]=0;
xx123456987 0:f0f040a29912 170 //
xx123456987 0:f0f040a29912 171 XL[0]=xL0 + (clk.read()/5 - 0.75)*xdir;
xx123456987 0:f0f040a29912 172 XL[1]=yL0 + (clk.read()/5 - 0.75)*ydir;
xx123456987 0:f0f040a29912 173 XL[2]=0+pi/3;
xx123456987 0:f0f040a29912 174 } else if (clk.read()>T) {//4
xx123456987 0:f0f040a29912 175 Vc[0]=0;
xx123456987 0:f0f040a29912 176 Vc[1]=0;
xx123456987 0:f0f040a29912 177 Vc[2]=0;
xx123456987 0:f0f040a29912 178 //
xx123456987 0:f0f040a29912 179 XL[0]=xL0+S*xdir;
xx123456987 0:f0f040a29912 180 XL[1]=yL0+S*ydir;
xx123456987 0:f0f040a29912 181 XL[2]=0+pi/3;
xx123456987 0:f0f040a29912 182 clk.reset();
xx123456987 0:f0f040a29912 183 xL0=XL[0];
xx123456987 0:f0f040a29912 184 yL0=XL[1];
xx123456987 0:f0f040a29912 185 gosw=true;
xx123456987 0:f0f040a29912 186 } else {//3
xx123456987 0:f0f040a29912 187 Vc[0]=(0.6-2*clk.read()/75)*xdir;
xx123456987 0:f0f040a29912 188 Vc[1]=(0.6-2*clk.read()/75)*ydir;
xx123456987 0:f0f040a29912 189 Vc[2]=0;
xx123456987 0:f0f040a29912 190 //
xx123456987 0:f0f040a29912 191 XL[0]=xL0 + (-((2*clk.read()/75 - 0.8)*(clk.read() - 15))/2 + 2.25)*xdir;
xx123456987 0:f0f040a29912 192 XL[1]=yL0 + (-((2*clk.read()/75 - 0.8)*(clk.read() - 15))/2 + 2.25)*ydir;
xx123456987 0:f0f040a29912 193 XL[2]=0+pi/3;
xx123456987 0:f0f040a29912 194 }
xx123456987 0:f0f040a29912 195 }
xx123456987 0:f0f040a29912 196 //printf("% .2f|% .2f|%f|X V cmd(xdire)\n", XL[0]*100, Vc[0]*100, clk.read());
xx123456987 0:f0f040a29912 197
xx123456987 0:f0f040a29912 198 // odometry beginning //
xx123456987 0:f0f040a29912 199 qei1_new = w1.getPulses();
xx123456987 0:f0f040a29912 200 qei2_new = w2.getPulses();
xx123456987 0:f0f040a29912 201 qei3_new = w3.getPulses();
xx123456987 0:f0f040a29912 202
xx123456987 0:f0f040a29912 203 double qei1 = qei1_new - qei1_old;
xx123456987 0:f0f040a29912 204 double qei2 = qei2_new - qei2_old;
xx123456987 0:f0f040a29912 205 double qei3 = qei3_new - qei3_old;
xx123456987 0:f0f040a29912 206
xx123456987 0:f0f040a29912 207 //wait_ms(50);
xx123456987 0:f0f040a29912 208
xx123456987 0:f0f040a29912 209 d_theta1 = qei1*360*pi/(4096*6*180); //DEG to RAD
xx123456987 0:f0f040a29912 210 d_theta2 = qei2*360*pi/(4096*6*180);
xx123456987 0:f0f040a29912 211 d_theta3 = qei3*360*pi/(4096*6*180);
xx123456987 0:f0f040a29912 212 d_theta = c1*( d_theta1 + d_theta2 + d_theta3 );
xx123456987 0:f0f040a29912 213
xx123456987 0:f0f040a29912 214 // compute theta
xx123456987 0:f0f040a29912 215 Next_X[2] = Current_X[2] + d_theta;
xx123456987 0:f0f040a29912 216 double theta = Current_X[2];
xx123456987 0:f0f040a29912 217 float c3 = Current_X[2] + d_theta/2;
xx123456987 0:f0f040a29912 218 // compute y
xx123456987 0:f0f040a29912 219 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:f0f040a29912 220 // compute x
xx123456987 0:f0f040a29912 221 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:f0f040a29912 222
xx123456987 0:f0f040a29912 223 // compute velocity
xx123456987 0:f0f040a29912 224 dx =Next_X[0]-Current_X[0];
xx123456987 0:f0f040a29912 225 dy =Next_X[1]-Current_X[1];
xx123456987 0:f0f040a29912 226 dtheta =Next_X[2]-Current_X[2];
xx123456987 0:f0f040a29912 227 V1[0]=dx/dt;
xx123456987 0:f0f040a29912 228 V1[1]=dy/dt;
xx123456987 0:f0f040a29912 229 V1[2]=dtheta/dt;
xx123456987 0:f0f040a29912 230 // transition
xx123456987 0:f0f040a29912 231 Current_X[2] = Next_X[2];
xx123456987 0:f0f040a29912 232 Current_X[1] = Next_X[1];
xx123456987 0:f0f040a29912 233 Current_X[0] = Next_X[0];
xx123456987 0:f0f040a29912 234
xx123456987 0:f0f040a29912 235
xx123456987 0:f0f040a29912 236 qei1_old = qei1_new;
xx123456987 0:f0f040a29912 237 qei2_old = qei2_new;
xx123456987 0:f0f040a29912 238 qei3_old = qei3_new;
xx123456987 0:f0f040a29912 239 // odometry end //
xx123456987 0:f0f040a29912 240
xx123456987 0:f0f040a29912 241
xx123456987 0:f0f040a29912 242 //printf ("% .1f|% .1f|% .1f|%f|Vc\n", Vc[0]*100, Vc[1]*100, Vc[2]*180/pi,clk.read());
xx123456987 0:f0f040a29912 243 // control law beginning //
xx123456987 0:f0f040a29912 244 //#define r 0.05
xx123456987 0:f0f040a29912 245 //#define L 0.125
xx123456987 0:f0f040a29912 246 X1_b[0] = X1[0]-f1[0];
xx123456987 0:f0f040a29912 247 X1_b[1] = X1[1]-f1[1];
xx123456987 0:f0f040a29912 248 X1_b[2] = X1[2]-f1[2];
xx123456987 0:f0f040a29912 249
xx123456987 0:f0f040a29912 250 u[0] = -kp*(X1_b[0]-XL[0])-kd*(V1[0]-Vc[0]);
xx123456987 0:f0f040a29912 251 u[1] = -kp*(X1_b[1]-XL[1])-kd*(V1[1]-Vc[1]);
xx123456987 0:f0f040a29912 252 u[2] = -kp*(X1_b[2]-XL[2])-kd*(V1[2]-Vc[2]);
xx123456987 0:f0f040a29912 253 //
xx123456987 0:f0f040a29912 254 omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta);
xx123456987 0:f0f040a29912 255 omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta);
xx123456987 0:f0f040a29912 256 omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta);
xx123456987 0:f0f040a29912 257 // control law end //
xx123456987 0:f0f040a29912 258
xx123456987 0:f0f040a29912 259 // omega to PWM //
xx123456987 0:f0f040a29912 260 pwm_w1 = 0.5 + omg1/2;
xx123456987 0:f0f040a29912 261 pwm_w2 = 0.5 + omg2/2;
xx123456987 0:f0f040a29912 262 pwm_w3 = 0.5 + omg3/2;
xx123456987 0:f0f040a29912 263
xx123456987 0:f0f040a29912 264 pwm_1.write(pwm_w1);
xx123456987 0:f0f040a29912 265 pwm_2.write(pwm_w2);
xx123456987 0:f0f040a29912 266 pwm_3.write(pwm_w3);
xx123456987 0:f0f040a29912 267 // END //
xx123456987 0:f0f040a29912 268
xx123456987 0:f0f040a29912 269 // define error // not abs() yet
xx123456987 0:f0f040a29912 270 err[0] = (Current_X[0]-f1[0])-(0.0);
xx123456987 0:f0f040a29912 271 err[1] = (Current_X[1]-f1[1])-(0.0);
xx123456987 0:f0f040a29912 272 err[2] = (Current_X[2]-f1[2])-XL[2];
xx123456987 0:f0f040a29912 273
xx123456987 0:f0f040a29912 274 if ( abs(err[0])<ex && abs(err[1])<ey && abs(err[2])<(etheta*pi/180) && stop==true ) {
xx123456987 0:f0f040a29912 275 printf("Arrived!\n");
xx123456987 0:f0f040a29912 276 pwm_1.write(0.50);
xx123456987 0:f0f040a29912 277 pwm_2.write(0.50);
xx123456987 0:f0f040a29912 278 pwm_3.write(0.50);
xx123456987 0:f0f040a29912 279 while (mybutton == 1) {}
xx123456987 0:f0f040a29912 280 }
xx123456987 0:f0f040a29912 281
xx123456987 0:f0f040a29912 282 //print
xx123456987 0:f0f040a29912 283 //printf ("\n");
xx123456987 0:f0f040a29912 284 //printf("% .2f|% .2f|% .1f|Xnow|%f\n", Current_X[0]*100, Current_X[1]*100,Current_X[2]*180/pi-60,clk.read());
xx123456987 0:f0f040a29912 285 printf("% .2f|% .2f|% .1f|Xnow|% .2f|% .2f|% .1f|XL|%f\n",
xx123456987 0:f0f040a29912 286 Current_X[0]*100, Current_X[1]*100,Current_X[2]*180/pi-60,
xx123456987 0:f0f040a29912 287 XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60,clk.read());
xx123456987 0:f0f040a29912 288 //printf("% .1f|% .1f|% .1f|Xc|%f\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60,clk.read());
xx123456987 0:f0f040a29912 289 ///printf ("% .2f|% .2f|% .1f|Vnow\n", V1[0]*100, V1[1]*100, V1[2]*180/pi);
xx123456987 0:f0f040a29912 290 //printf("%f\n",clk.read());
xx123456987 0:f0f040a29912 291 //printf("% .1f|% .1f|% .1f|Xc \n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60);
xx123456987 0:f0f040a29912 292 //printf("% .2f|% .2f|% .1f|Vc \n", Vc[0]*100, Vc[1]*100, Vc[2]*180/pi);
xx123456987 0:f0f040a29912 293 //printf ("% .2f|% .2f|%f|now\n", Current_X[0]*100, V1[0]*100,clk.read());
xx123456987 0:f0f040a29912 294 //printf("% .2f|% .2f|%f|X V cmd(xdire)\n", XL[0]*100, Vc[0]*100, clk.read());//測命令用
xx123456987 0:f0f040a29912 295 //printf("% .2f|% .2f|%f|X V cmd(ydire)\n", XL[1]*100, Vc[1]*100, clk.read());
xx123456987 0:f0f040a29912 296 //printf ("Error : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", (err[0])*100, (err[1])*100, (err[2])*180/pi);
xx123456987 0:f0f040a29912 297 /*
xx123456987 0:f0f040a29912 298 printf ("\nWheel[1] counts: %d | revolutions: % .2f", qei1_new, (float)qei1_new/(4096*6));
xx123456987 0:f0f040a29912 299 printf ("\nWheel[2] counts: %d | revolutions: % .2f", qei2_new, (float)qei2_new/(4096*6));
xx123456987 0:f0f040a29912 300 printf ("\nWheel[3] counts: %d | revolutions: % .2f", qei3_new, (float)qei3_new/(4096*6));
xx123456987 0:f0f040a29912 301 */
xx123456987 0:f0f040a29912 302 //printf("Wheel Vel. [%3.2f %3.2f %3.2f]\n",pwm_w1,pwm_w2,pwm_w3);
xx123456987 0:f0f040a29912 303 //printf("OMEGA: [%.2f %.2f %.2f]\n",omg1,omg2,omg3);
xx123456987 0:f0f040a29912 304 //printf("PWM: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100);
xx123456987 0:f0f040a29912 305 //printf("%f\n",clk.read());
xx123456987 0:f0f040a29912 306
xx123456987 0:f0f040a29912 307 //}
xx123456987 0:f0f040a29912 308
xx123456987 0:f0f040a29912 309 // arrive?
xx123456987 0:f0f040a29912 310 wait_ms(50);//
xx123456987 0:f0f040a29912 311 }
xx123456987 0:f0f040a29912 312 }
xx123456987 0:f0f040a29912 313
xx123456987 0:f0f040a29912 314 /*
xx123456987 0:f0f040a29912 315 switch (sw) {
xx123456987 0:f0f040a29912 316 case '1':
xx123456987 0:f0f040a29912 317 tempchar = sw;
xx123456987 0:f0f040a29912 318 xdir=-1;
xx123456987 0:f0f040a29912 319 ydir=-1;
xx123456987 0:f0f040a29912 320 sw = '2';
xx123456987 0:f0f040a29912 321 break;
xx123456987 0:f0f040a29912 322 case '2':
xx123456987 0:f0f040a29912 323 tempchar = sw;
xx123456987 0:f0f040a29912 324 xdir=0;
xx123456987 0:f0f040a29912 325 ydir=-1;
xx123456987 0:f0f040a29912 326 sw = '3';
xx123456987 0:f0f040a29912 327 break;
xx123456987 0:f0f040a29912 328 case '3':
xx123456987 0:f0f040a29912 329 tempchar = sw;
xx123456987 0:f0f040a29912 330 xdir=1;
xx123456987 0:f0f040a29912 331 ydir=-1;
xx123456987 0:f0f040a29912 332 sw = '6';
xx123456987 0:f0f040a29912 333 break;
xx123456987 0:f0f040a29912 334 case '4':
xx123456987 0:f0f040a29912 335 tempchar = sw;
xx123456987 0:f0f040a29912 336 xdir=-1;
xx123456987 0:f0f040a29912 337 ydir=0;
xx123456987 0:f0f040a29912 338 sw = '1';
xx123456987 0:f0f040a29912 339 break;
xx123456987 0:f0f040a29912 340 case '5':
xx123456987 0:f0f040a29912 341 tempchar = sw;
xx123456987 0:f0f040a29912 342
xx123456987 0:f0f040a29912 343 xdir=0;
xx123456987 0:f0f040a29912 344 ydir=0;
xx123456987 0:f0f040a29912 345 XL[2]=0+pi/3;
xx123456987 0:f0f040a29912 346 break;
xx123456987 0:f0f040a29912 347 case '6':
xx123456987 0:f0f040a29912 348 tempchar = sw;
xx123456987 0:f0f040a29912 349 xdir=1;
xx123456987 0:f0f040a29912 350 ydir=0;
xx123456987 0:f0f040a29912 351 sw = '9';
xx123456987 0:f0f040a29912 352 break;
xx123456987 0:f0f040a29912 353 case '7':
xx123456987 0:f0f040a29912 354 tempchar = sw;
xx123456987 0:f0f040a29912 355 xdir=-1;
xx123456987 0:f0f040a29912 356 ydir=1;
xx123456987 0:f0f040a29912 357 sw = '4';
xx123456987 0:f0f040a29912 358 break;
xx123456987 0:f0f040a29912 359 case '8':
xx123456987 0:f0f040a29912 360 tempchar = sw;
xx123456987 0:f0f040a29912 361 xdir=0;
xx123456987 0:f0f040a29912 362 ydir=1;
xx123456987 0:f0f040a29912 363 sw = '7';
xx123456987 0:f0f040a29912 364 break;
xx123456987 0:f0f040a29912 365 case '9':
xx123456987 0:f0f040a29912 366 tempchar = sw;
xx123456987 0:f0f040a29912 367 xdir=1;
xx123456987 0:f0f040a29912 368 ydir=1;
xx123456987 0:f0f040a29912 369 sw = '8';
xx123456987 0:f0f040a29912 370 break;
xx123456987 0:f0f040a29912 371 default:
xx123456987 0:f0f040a29912 372 xdir=0;
xx123456987 0:f0f040a29912 373 ydir=0;
xx123456987 0:f0f040a29912 374 XL[2]=0+pi/3;
xx123456987 0:f0f040a29912 375 break;
xx123456987 0:f0f040a29912 376 }*/
xx123456987 0:f0f040a29912 377
xx123456987 0:f0f040a29912 378 // P&V command compensation
xx123456987 0:f0f040a29912 379 /*if(s0==false) {
xx123456987 0:f0f040a29912 380 if(clk.read()<1) {
xx123456987 0:f0f040a29912 381 Vc[0]=0.15;
xx123456987 0:f0f040a29912 382 Vc[1]=0.05+0.1*clk.read();
xx123456987 0:f0f040a29912 383 Vc[2]=0;
xx123456987 0:f0f040a29912 384 //
xx123456987 0:f0f040a29912 385 XL[0]=0.15*clk.read();// go to target
xx123456987 0:f0f040a29912 386 XL[1]=0.05*clk.read()+0.05*clk.read()*clk.read();
xx123456987 0:f0f040a29912 387 XL[2]=0+pi/3;
xx123456987 0:f0f040a29912 388 } else if(clk.read()>1 && clk.read()<2) {
xx123456987 0:f0f040a29912 389 Vc[0]=0.15;
xx123456987 0:f0f040a29912 390 Vc[1]=0.15;
xx123456987 0:f0f040a29912 391 Vc[2]=0;
xx123456987 0:f0f040a29912 392 //
xx123456987 0:f0f040a29912 393 XL[0]=0.15*clk.read();// go to target
xx123456987 0:f0f040a29912 394 XL[1]=0.1+0.15*(clk.read()-1);
xx123456987 0:f0f040a29912 395 XL[2]=0+pi/3;
xx123456987 0:f0f040a29912 396 } else if(clk.read()>2 && clk.read()<3) {
xx123456987 0:f0f040a29912 397 Vc[0]=0.15-0.1*(clk.read()-2);
xx123456987 0:f0f040a29912 398 Vc[1]=0.15;
xx123456987 0:f0f040a29912 399 Vc[2]=0;
xx123456987 0:f0f040a29912 400 //
xx123456987 0:f0f040a29912 401 XL[0]=0.3+(0.3-0.1*(clk.read()-2))*(clk.read()-2)/2;// go to target
xx123456987 0:f0f040a29912 402 XL[1]=0.25+0.15*(clk.read()-2);
xx123456987 0:f0f040a29912 403 XL[2]=0+pi/3;
xx123456987 0:f0f040a29912 404 } else {
xx123456987 0:f0f040a29912 405 Vc[0]=0;
xx123456987 0:f0f040a29912 406 Vc[1]=0;
xx123456987 0:f0f040a29912 407 Vc[2]=0;
xx123456987 0:f0f040a29912 408 //
xx123456987 0:f0f040a29912 409 XL[0]=0.4;// go to target
xx123456987 0:f0f040a29912 410 XL[1]=0.4;
xx123456987 0:f0f040a29912 411 XL[2]=0+pi/3;
xx123456987 0:f0f040a29912 412 s0=true;
xx123456987 0:f0f040a29912 413 clk.reset();
xx123456987 0:f0f040a29912 414 }
xx123456987 0:f0f040a29912 415 }*/