yi li / Mbed 2 deprecated 3W_wifi_odo_p2p_0722

20160725

Dependencies:   QEI mbed

main.cpp@2:388988e5733a, 2016-07-25 (annotated)

Committer:
liyi
Date:
Mon Jul 25 06:58:49 2016 +0000
Revision:
2:388988e5733a
Parent:
1:a78e5f6154d3 
C change to c_to_send;

Who changed what in which revision?

UserRevisionLine numberNew 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 2:388988e5733a 173 for (const char *p_get = "32\n" ; c_for_get = *p_get; p_get++)
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 2:388988e5733a 275 for (const char *p_send = send_array ; c_for_send = *p_send; p_send++) //尚未實測
liyi 1:a78e5f6154d3 276 {
liyi 2:388988e5733a 277 spi.write (c_for_send);
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 }