yi li
/
3W_wifi_odo_p2p_0722
20160725
main.cpp@1:a78e5f6154d3, 2016-07-25 (annotated)
- Committer:
- liyi
- Date:
- Mon Jul 25 06:38:27 2016 +0000
- Revision:
- 1:a78e5f6154d3
- Parent:
- 0:926cd923a480
- Child:
- 2:388988e5733a
20160725
Who changed what in which revision?
User | Revision | Line number | New 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; |
liyi | 1:a78e5f6154d3 | 69 | char c_for_get; |
liyi | 1:a78e5f6154d3 | 70 | char c_for_send; |
liyi | 1:a78e5f6154d3 | 71 | SPI spi(D11, D12, D13); // mosi, miso, sclk |
liyi | 1:a78e5f6154d3 | 72 | DigitalOut cs(D10); |
xx123456987 | 0:926cd923a480 | 73 | |
liyi | 1:a78e5f6154d3 | 74 | Serial pc(SERIAL_TX, SERIAL_RX); |
xx123456987 | 0:926cd923a480 | 75 | |
liyi | 1:a78e5f6154d3 | 76 | char *delim = " "; |
liyi | 1:a78e5f6154d3 | 77 | char *pch; |
liyi | 1:a78e5f6154d3 | 78 | float cha_L[3]; |
liyi | 1:a78e5f6154d3 | 79 | |
xx123456987 | 0:926cd923a480 | 80 | int main() |
xx123456987 | 0:926cd923a480 | 81 | { |
xx123456987 | 0:926cd923a480 | 82 | |
xx123456987 | 0:926cd923a480 | 83 | t.start(); |
xx123456987 | 0:926cd923a480 | 84 | |
xx123456987 | 0:926cd923a480 | 85 | pwm_1.period_us(pwm_period_us); |
xx123456987 | 0:926cd923a480 | 86 | pwm_2.period_us(pwm_period_us); |
xx123456987 | 0:926cd923a480 | 87 | pwm_3.period_us(pwm_period_us); |
xx123456987 | 0:926cd923a480 | 88 | |
xx123456987 | 0:926cd923a480 | 89 | pwm_1.write(0.50); |
xx123456987 | 0:926cd923a480 | 90 | pwm_2.write(0.50); |
xx123456987 | 0:926cd923a480 | 91 | pwm_3.write(0.50); |
xx123456987 | 0:926cd923a480 | 92 | |
xx123456987 | 0:926cd923a480 | 93 | qei1_old = 0; |
xx123456987 | 0:926cd923a480 | 94 | qei2_old = 0; |
xx123456987 | 0:926cd923a480 | 95 | qei3_old = 0; |
xx123456987 | 0:926cd923a480 | 96 | printf("===================Reset===================\n"); |
xx123456987 | 0:926cd923a480 | 97 | aa: { |
xx123456987 | 0:926cd923a480 | 98 | |
xx123456987 | 0:926cd923a480 | 99 | printf("Which direction to go?\n"); |
xx123456987 | 0:926cd923a480 | 100 | printf(" 7 8 9 \n"); |
xx123456987 | 0:926cd923a480 | 101 | printf(" | \n"); |
xx123456987 | 0:926cd923a480 | 102 | printf(" 4- 5 -6 \n"); |
xx123456987 | 0:926cd923a480 | 103 | printf(" | \n"); |
xx123456987 | 0:926cd923a480 | 104 | printf(" 1 2 3 \n"); |
xx123456987 | 0:926cd923a480 | 105 | myled = 1; |
xx123456987 | 0:926cd923a480 | 106 | } |
xx123456987 | 0:926cd923a480 | 107 | |
xx123456987 | 0:926cd923a480 | 108 | scanf("%c", &dire); |
xx123456987 | 0:926cd923a480 | 109 | switch (dire) { |
xx123456987 | 0:926cd923a480 | 110 | case '1': |
xx123456987 | 0:926cd923a480 | 111 | XL[0]=-0.4; |
xx123456987 | 0:926cd923a480 | 112 | XL[1]=-0.4; |
xx123456987 | 0:926cd923a480 | 113 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 114 | break; |
xx123456987 | 0:926cd923a480 | 115 | case '2': |
xx123456987 | 0:926cd923a480 | 116 | XL[0]=0; |
xx123456987 | 0:926cd923a480 | 117 | XL[1]=-0.4; |
xx123456987 | 0:926cd923a480 | 118 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 119 | break; |
xx123456987 | 0:926cd923a480 | 120 | case '3': |
xx123456987 | 0:926cd923a480 | 121 | XL[0]=0.4; |
xx123456987 | 0:926cd923a480 | 122 | XL[1]=-0.4; |
xx123456987 | 0:926cd923a480 | 123 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 124 | break; |
xx123456987 | 0:926cd923a480 | 125 | case '4': |
xx123456987 | 0:926cd923a480 | 126 | XL[0]=-0.4; |
xx123456987 | 0:926cd923a480 | 127 | XL[1]=0; |
xx123456987 | 0:926cd923a480 | 128 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 129 | break; |
xx123456987 | 0:926cd923a480 | 130 | case '5': |
xx123456987 | 0:926cd923a480 | 131 | XL[0]=0; |
xx123456987 | 0:926cd923a480 | 132 | XL[1]=0; |
xx123456987 | 0:926cd923a480 | 133 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 134 | break; |
xx123456987 | 0:926cd923a480 | 135 | case '6': |
xx123456987 | 0:926cd923a480 | 136 | XL[0]=0.4; |
xx123456987 | 0:926cd923a480 | 137 | XL[1]=0; |
xx123456987 | 0:926cd923a480 | 138 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 139 | break; |
xx123456987 | 0:926cd923a480 | 140 | case '7': |
xx123456987 | 0:926cd923a480 | 141 | XL[0]=-0.4; |
xx123456987 | 0:926cd923a480 | 142 | XL[1]=0.4; |
xx123456987 | 0:926cd923a480 | 143 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 144 | break; |
xx123456987 | 0:926cd923a480 | 145 | case '8': |
xx123456987 | 0:926cd923a480 | 146 | XL[0]=0; |
xx123456987 | 0:926cd923a480 | 147 | XL[1]=0.4; |
xx123456987 | 0:926cd923a480 | 148 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 149 | break; |
xx123456987 | 0:926cd923a480 | 150 | case '9': |
xx123456987 | 0:926cd923a480 | 151 | XL[0]=0.4; |
xx123456987 | 0:926cd923a480 | 152 | XL[1]=0.4; |
xx123456987 | 0:926cd923a480 | 153 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 154 | break; |
xx123456987 | 0:926cd923a480 | 155 | } |
liyi | 1:a78e5f6154d3 | 156 | |
xx123456987 | 0:926cd923a480 | 157 | printf("PWM Reset: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100); |
xx123456987 | 0:926cd923a480 | 158 | 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 | 159 | printf("Desired Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60); |
xx123456987 | 0:926cd923a480 | 160 | //printf("Press USER_BUTTON to START (Go to Direction '%c')\n", dire); |
xx123456987 | 0:926cd923a480 | 161 | printf("Go to Direction '%c')\n", dire); |
xx123456987 | 0:926cd923a480 | 162 | //while (mybutton == 1) {} |
xx123456987 | 0:926cd923a480 | 163 | |
xx123456987 | 0:926cd923a480 | 164 | while(c>0) { |
xx123456987 | 0:926cd923a480 | 165 | wait(1); |
xx123456987 | 0:926cd923a480 | 166 | printf("%d\n",c--); |
xx123456987 | 0:926cd923a480 | 167 | myled = !myled; |
xx123456987 | 0:926cd923a480 | 168 | } |
xx123456987 | 0:926cd923a480 | 169 | |
xx123456987 | 0:926cd923a480 | 170 | while(1) { |
liyi | 1:a78e5f6154d3 | 171 | |
liyi | 1:a78e5f6154d3 | 172 | cs = 0; |
liyi | 1:a78e5f6154d3 | 173 | for (const char *p = "32\n" ; c_for_get = *p; p++) |
liyi | 1:a78e5f6154d3 | 174 | { |
liyi | 1:a78e5f6154d3 | 175 | spi.write (c_for_get); |
liyi | 1:a78e5f6154d3 | 176 | } |
liyi | 1:a78e5f6154d3 | 177 | wait(0.1); |
liyi | 1:a78e5f6154d3 | 178 | float bb = spi.write (0x00); |
liyi | 1:a78e5f6154d3 | 179 | char ddest[3]; |
liyi | 1:a78e5f6154d3 | 180 | sprintf(ddest,"%.1f \n", bb); |
liyi | 1:a78e5f6154d3 | 181 | pc.printf(ddest); |
liyi | 1:a78e5f6154d3 | 182 | cs = 1;//須將ddest矩陣拆開成三個不同的數 |
liyi | 1:a78e5f6154d3 | 183 | |
liyi | 1:a78e5f6154d3 | 184 | int strtok_n = 0; |
liyi | 1:a78e5f6154d3 | 185 | pch = strtok(ddest,delim); |
liyi | 1:a78e5f6154d3 | 186 | while(pch != NULL) |
liyi | 1:a78e5f6154d3 | 187 | { |
liyi | 1:a78e5f6154d3 | 188 | cha_L[strtok_n] = atof(pch); |
liyi | 1:a78e5f6154d3 | 189 | //printf("%s\n",pch); |
liyi | 1:a78e5f6154d3 | 190 | pch = strtok(NULL, delim); |
liyi | 1:a78e5f6154d3 | 191 | strtok_n = strtok_n+1; |
liyi | 1:a78e5f6154d3 | 192 | } |
liyi | 1:a78e5f6154d3 | 193 | XL[0] = cha_L[0]; |
liyi | 1:a78e5f6154d3 | 194 | XL[1] = cha_L[1]; |
liyi | 1:a78e5f6154d3 | 195 | XL[2] = cha_L[2]; |
liyi | 1:a78e5f6154d3 | 196 | |
xx123456987 | 0:926cd923a480 | 197 | // |
xx123456987 | 0:926cd923a480 | 198 | myled = 0; |
xx123456987 | 0:926cd923a480 | 199 | X1[0] = Current_X[0]; |
xx123456987 | 0:926cd923a480 | 200 | X1[1] = Current_X[1]; |
xx123456987 | 0:926cd923a480 | 201 | X1[2] = Current_X[2]; |
xx123456987 | 0:926cd923a480 | 202 | |
xx123456987 | 0:926cd923a480 | 203 | // odometry beginning // |
xx123456987 | 0:926cd923a480 | 204 | qei1_new = w1.getPulses(); |
xx123456987 | 0:926cd923a480 | 205 | qei2_new = w2.getPulses(); |
xx123456987 | 0:926cd923a480 | 206 | qei3_new = w3.getPulses(); |
xx123456987 | 0:926cd923a480 | 207 | |
xx123456987 | 0:926cd923a480 | 208 | double qei1 = qei1_new - qei1_old; |
xx123456987 | 0:926cd923a480 | 209 | double qei2 = qei2_new - qei2_old; |
xx123456987 | 0:926cd923a480 | 210 | double qei3 = qei3_new - qei3_old; |
xx123456987 | 0:926cd923a480 | 211 | |
xx123456987 | 0:926cd923a480 | 212 | //wait_ms(50); |
xx123456987 | 0:926cd923a480 | 213 | |
xx123456987 | 0:926cd923a480 | 214 | d_theta1 = qei1*360*pi/(4096*6*180); //DEG to RAD |
xx123456987 | 0:926cd923a480 | 215 | d_theta2 = qei2*360*pi/(4096*6*180); |
xx123456987 | 0:926cd923a480 | 216 | d_theta3 = qei3*360*pi/(4096*6*180); |
xx123456987 | 0:926cd923a480 | 217 | d_theta = c1*( d_theta1 + d_theta2 + d_theta3 ); |
xx123456987 | 0:926cd923a480 | 218 | |
xx123456987 | 0:926cd923a480 | 219 | // compute theta |
xx123456987 | 0:926cd923a480 | 220 | Next_X[2] = Current_X[2] + d_theta; |
xx123456987 | 0:926cd923a480 | 221 | double theta = Current_X[2]; |
xx123456987 | 0:926cd923a480 | 222 | float c3 = Current_X[2] + d_theta/2; |
xx123456987 | 0:926cd923a480 | 223 | // compute y |
xx123456987 | 0:926cd923a480 | 224 | 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 | 225 | // compute x |
xx123456987 | 0:926cd923a480 | 226 | 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 | 227 | |
xx123456987 | 0:926cd923a480 | 228 | // transition |
xx123456987 | 0:926cd923a480 | 229 | Current_X[2] = Next_X[2]; |
xx123456987 | 0:926cd923a480 | 230 | Current_X[1] = Next_X[1]; |
xx123456987 | 0:926cd923a480 | 231 | Current_X[0] = Next_X[0]; |
xx123456987 | 0:926cd923a480 | 232 | |
xx123456987 | 0:926cd923a480 | 233 | qei1_old = qei1_new; |
xx123456987 | 0:926cd923a480 | 234 | qei2_old = qei2_new; |
xx123456987 | 0:926cd923a480 | 235 | qei3_old = qei3_new; |
xx123456987 | 0:926cd923a480 | 236 | // odometry end // |
xx123456987 | 0:926cd923a480 | 237 | |
xx123456987 | 0:926cd923a480 | 238 | // control law beginning // |
xx123456987 | 0:926cd923a480 | 239 | //#define r 0.05 |
xx123456987 | 0:926cd923a480 | 240 | //#define L 0.125 |
xx123456987 | 0:926cd923a480 | 241 | |
xx123456987 | 0:926cd923a480 | 242 | X1_b[0] = X1[0]-f1[0]; |
xx123456987 | 0:926cd923a480 | 243 | X1_b[1] = X1[1]-f1[1]; |
xx123456987 | 0:926cd923a480 | 244 | X1_b[2] = X1[2]-f1[2]; |
xx123456987 | 0:926cd923a480 | 245 | u[0] = -c0*(X1_b[0]-XL[0]); |
xx123456987 | 0:926cd923a480 | 246 | u[1] = -c0*(X1_b[1]-XL[1]); |
xx123456987 | 0:926cd923a480 | 247 | u[2] = -c0*(X1_b[2]-XL[2]); |
xx123456987 | 0:926cd923a480 | 248 | // |
xx123456987 | 0:926cd923a480 | 249 | omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta); |
xx123456987 | 0:926cd923a480 | 250 | omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta); |
xx123456987 | 0:926cd923a480 | 251 | omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta); |
xx123456987 | 0:926cd923a480 | 252 | // control law end // |
xx123456987 | 0:926cd923a480 | 253 | |
xx123456987 | 0:926cd923a480 | 254 | //theta = theta + pi/3; // shift: origin to new |
xx123456987 | 0:926cd923a480 | 255 | |
xx123456987 | 0:926cd923a480 | 256 | // omega to PWM // |
xx123456987 | 0:926cd923a480 | 257 | pwm_w1 = 0.5 + omg1/2; |
xx123456987 | 0:926cd923a480 | 258 | pwm_w2 = 0.5 + omg2/2; |
xx123456987 | 0:926cd923a480 | 259 | pwm_w3 = 0.5 + omg3/2; |
xx123456987 | 0:926cd923a480 | 260 | |
xx123456987 | 0:926cd923a480 | 261 | pwm_1.write(pwm_w1); |
xx123456987 | 0:926cd923a480 | 262 | pwm_2.write(pwm_w2); |
xx123456987 | 0:926cd923a480 | 263 | pwm_3.write(pwm_w3); |
xx123456987 | 0:926cd923a480 | 264 | // END // |
xx123456987 | 0:926cd923a480 | 265 | |
xx123456987 | 0:926cd923a480 | 266 | // define error // not abs() yet |
xx123456987 | 0:926cd923a480 | 267 | err[0] = Current_X[0]-XL[0]; |
xx123456987 | 0:926cd923a480 | 268 | err[1] = Current_X[1]-XL[1]; |
xx123456987 | 0:926cd923a480 | 269 | err[2] = Current_X[2]-XL[2]; |
xx123456987 | 0:926cd923a480 | 270 | // END // |
liyi | 1:a78e5f6154d3 | 271 | char send_array[6]; |
liyi | 1:a78e5f6154d3 | 272 | sprintf(send_array, "%.1f %.1f %.1f\n", Current_X[0], Current_X[1], Current_X[2]); |
liyi | 1:a78e5f6154d3 | 273 | |
liyi | 1:a78e5f6154d3 | 274 | cs = 0; |
liyi | 1:a78e5f6154d3 | 275 | for (const char *p = send_array ; c = *p; p++) //尚未實測 |
liyi | 1:a78e5f6154d3 | 276 | { |
liyi | 1:a78e5f6154d3 | 277 | spi.write (c); |
liyi | 1:a78e5f6154d3 | 278 | } |
liyi | 1:a78e5f6154d3 | 279 | wait(0.1); |
liyi | 1:a78e5f6154d3 | 280 | cs = 1; |
liyi | 1:a78e5f6154d3 | 281 | |
xx123456987 | 0:926cd923a480 | 282 | if ( t.read_ms() > 200) { // print |
xx123456987 | 0:926cd923a480 | 283 | |
xx123456987 | 0:926cd923a480 | 284 | t.reset(); |
xx123456987 | 0:926cd923a480 | 285 | t.start(); |
xx123456987 | 0:926cd923a480 | 286 | if (s0 == true) printf ("=================Homing====================\n"); |
xx123456987 | 0:926cd923a480 | 287 | else printf ("===========================================\n"); |
xx123456987 | 0:926cd923a480 | 288 | 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 | 289 | printf ("Error : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", (err[0])*100, (err[1])*100, (err[2])*180/pi); |
xx123456987 | 0:926cd923a480 | 290 | /* |
xx123456987 | 0:926cd923a480 | 291 | printf ("\nWheel[1] counts: %d | revolutions: % .2f", qei1_new, (float)qei1_new/(4096*6)); |
xx123456987 | 0:926cd923a480 | 292 | printf ("\nWheel[2] counts: %d | revolutions: % .2f", qei2_new, (float)qei2_new/(4096*6)); |
xx123456987 | 0:926cd923a480 | 293 | printf ("\nWheel[3] counts: %d | revolutions: % .2f", qei3_new, (float)qei3_new/(4096*6)); |
xx123456987 | 0:926cd923a480 | 294 | */ |
xx123456987 | 0:926cd923a480 | 295 | //printf("Wheel Vel. [%3.2f %3.2f %3.2f]\n",pwm_w1,pwm_w2,pwm_w3); |
xx123456987 | 0:926cd923a480 | 296 | printf("OMEGA: [%.2f %.2f %.2f]\n",omg1,omg2,omg3); |
xx123456987 | 0:926cd923a480 | 297 | printf("PWM: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100); |
xx123456987 | 0:926cd923a480 | 298 | //printf("X: [%.3f] Y:[%.3f]\n",px,py); |
xx123456987 | 0:926cd923a480 | 299 | //printf("px+py=%.2f\n", px+py); |
xx123456987 | 0:926cd923a480 | 300 | |
xx123456987 | 0:926cd923a480 | 301 | } |
xx123456987 | 0:926cd923a480 | 302 | if ( abs(err[0])<=(ex/100) && abs(err[1])<=(ey/100) && abs(err[2])<=(etheta*pi/180) ) { |
xx123456987 | 0:926cd923a480 | 303 | printf("The robot Arrived!\n"); |
xx123456987 | 0:926cd923a480 | 304 | pwm_1.write(0.50); |
xx123456987 | 0:926cd923a480 | 305 | pwm_2.write(0.50); |
xx123456987 | 0:926cd923a480 | 306 | pwm_3.write(0.50); |
xx123456987 | 0:926cd923a480 | 307 | //s0 = true; |
xx123456987 | 0:926cd923a480 | 308 | c = 3; |
xx123456987 | 0:926cd923a480 | 309 | goto aa; |
xx123456987 | 0:926cd923a480 | 310 | /* |
xx123456987 | 0:926cd923a480 | 311 | printf("Press USER_BUTTON to go HOME\n"); |
xx123456987 | 0:926cd923a480 | 312 | while (mybutton == 1) { |
xx123456987 | 0:926cd923a480 | 313 | wait(0.25); |
xx123456987 | 0:926cd923a480 | 314 | myled = !myled; |
xx123456987 | 0:926cd923a480 | 315 | |
xx123456987 | 0:926cd923a480 | 316 | } |
xx123456987 | 0:926cd923a480 | 317 | |
xx123456987 | 0:926cd923a480 | 318 | while(c>0) { |
xx123456987 | 0:926cd923a480 | 319 | wait(1); |
xx123456987 | 0:926cd923a480 | 320 | printf("%d\n",c--); |
xx123456987 | 0:926cd923a480 | 321 | myled = !myled; |
xx123456987 | 0:926cd923a480 | 322 | } |
xx123456987 | 0:926cd923a480 | 323 | XL[0]=0; |
xx123456987 | 0:926cd923a480 | 324 | XL[1]=0; |
xx123456987 | 0:926cd923a480 | 325 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 326 | */ |
xx123456987 | 0:926cd923a480 | 327 | } |
xx123456987 | 0:926cd923a480 | 328 | |
liyi | 1:a78e5f6154d3 | 329 | wait_ms(50);// 會有什麼影響? |
xx123456987 | 0:926cd923a480 | 330 | } |
xx123456987 | 0:926cd923a480 | 331 | } |