yi li
/
3W_wifi_odo_p2p_0722
20160725
main.cpp@0:926cd923a480, 2016-07-22 (annotated)
- Committer:
- xx123456987
- Date:
- Fri Jul 22 07:57:56 2016 +0000
- Revision:
- 0:926cd923a480
- Child:
- 1:a78e5f6154d3
test
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; |
xx123456987 | 0:926cd923a480 | 69 | |
xx123456987 | 0:926cd923a480 | 70 | |
xx123456987 | 0:926cd923a480 | 71 | int main() |
xx123456987 | 0:926cd923a480 | 72 | { |
xx123456987 | 0:926cd923a480 | 73 | |
xx123456987 | 0:926cd923a480 | 74 | t.start(); |
xx123456987 | 0:926cd923a480 | 75 | |
xx123456987 | 0:926cd923a480 | 76 | pwm_1.period_us(pwm_period_us); |
xx123456987 | 0:926cd923a480 | 77 | pwm_2.period_us(pwm_period_us); |
xx123456987 | 0:926cd923a480 | 78 | pwm_3.period_us(pwm_period_us); |
xx123456987 | 0:926cd923a480 | 79 | |
xx123456987 | 0:926cd923a480 | 80 | pwm_1.write(0.50); |
xx123456987 | 0:926cd923a480 | 81 | pwm_2.write(0.50); |
xx123456987 | 0:926cd923a480 | 82 | pwm_3.write(0.50); |
xx123456987 | 0:926cd923a480 | 83 | |
xx123456987 | 0:926cd923a480 | 84 | qei1_old = 0; |
xx123456987 | 0:926cd923a480 | 85 | qei2_old = 0; |
xx123456987 | 0:926cd923a480 | 86 | qei3_old = 0; |
xx123456987 | 0:926cd923a480 | 87 | printf("===================Reset===================\n"); |
xx123456987 | 0:926cd923a480 | 88 | aa: { |
xx123456987 | 0:926cd923a480 | 89 | |
xx123456987 | 0:926cd923a480 | 90 | printf("Which direction to go?\n"); |
xx123456987 | 0:926cd923a480 | 91 | printf(" 7 8 9 \n"); |
xx123456987 | 0:926cd923a480 | 92 | printf(" | \n"); |
xx123456987 | 0:926cd923a480 | 93 | printf(" 4- 5 -6 \n"); |
xx123456987 | 0:926cd923a480 | 94 | printf(" | \n"); |
xx123456987 | 0:926cd923a480 | 95 | printf(" 1 2 3 \n"); |
xx123456987 | 0:926cd923a480 | 96 | myled = 1; |
xx123456987 | 0:926cd923a480 | 97 | } |
xx123456987 | 0:926cd923a480 | 98 | |
xx123456987 | 0:926cd923a480 | 99 | scanf("%c", &dire); |
xx123456987 | 0:926cd923a480 | 100 | switch (dire) { |
xx123456987 | 0:926cd923a480 | 101 | case '1': |
xx123456987 | 0:926cd923a480 | 102 | XL[0]=-0.4; |
xx123456987 | 0:926cd923a480 | 103 | XL[1]=-0.4; |
xx123456987 | 0:926cd923a480 | 104 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 105 | break; |
xx123456987 | 0:926cd923a480 | 106 | case '2': |
xx123456987 | 0:926cd923a480 | 107 | XL[0]=0; |
xx123456987 | 0:926cd923a480 | 108 | XL[1]=-0.4; |
xx123456987 | 0:926cd923a480 | 109 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 110 | break; |
xx123456987 | 0:926cd923a480 | 111 | case '3': |
xx123456987 | 0:926cd923a480 | 112 | XL[0]=0.4; |
xx123456987 | 0:926cd923a480 | 113 | XL[1]=-0.4; |
xx123456987 | 0:926cd923a480 | 114 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 115 | break; |
xx123456987 | 0:926cd923a480 | 116 | case '4': |
xx123456987 | 0:926cd923a480 | 117 | XL[0]=-0.4; |
xx123456987 | 0:926cd923a480 | 118 | XL[1]=0; |
xx123456987 | 0:926cd923a480 | 119 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 120 | break; |
xx123456987 | 0:926cd923a480 | 121 | case '5': |
xx123456987 | 0:926cd923a480 | 122 | XL[0]=0; |
xx123456987 | 0:926cd923a480 | 123 | XL[1]=0; |
xx123456987 | 0:926cd923a480 | 124 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 125 | break; |
xx123456987 | 0:926cd923a480 | 126 | case '6': |
xx123456987 | 0:926cd923a480 | 127 | XL[0]=0.4; |
xx123456987 | 0:926cd923a480 | 128 | XL[1]=0; |
xx123456987 | 0:926cd923a480 | 129 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 130 | break; |
xx123456987 | 0:926cd923a480 | 131 | case '7': |
xx123456987 | 0:926cd923a480 | 132 | XL[0]=-0.4; |
xx123456987 | 0:926cd923a480 | 133 | XL[1]=0.4; |
xx123456987 | 0:926cd923a480 | 134 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 135 | break; |
xx123456987 | 0:926cd923a480 | 136 | case '8': |
xx123456987 | 0:926cd923a480 | 137 | XL[0]=0; |
xx123456987 | 0:926cd923a480 | 138 | XL[1]=0.4; |
xx123456987 | 0:926cd923a480 | 139 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 140 | break; |
xx123456987 | 0:926cd923a480 | 141 | case '9': |
xx123456987 | 0:926cd923a480 | 142 | XL[0]=0.4; |
xx123456987 | 0:926cd923a480 | 143 | XL[1]=0.4; |
xx123456987 | 0:926cd923a480 | 144 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 145 | break; |
xx123456987 | 0:926cd923a480 | 146 | } |
xx123456987 | 0:926cd923a480 | 147 | |
xx123456987 | 0:926cd923a480 | 148 | printf("PWM Reset: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100); |
xx123456987 | 0:926cd923a480 | 149 | 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 | 150 | printf("Desired Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60); |
xx123456987 | 0:926cd923a480 | 151 | //printf("Press USER_BUTTON to START (Go to Direction '%c')\n", dire); |
xx123456987 | 0:926cd923a480 | 152 | printf("Go to Direction '%c')\n", dire); |
xx123456987 | 0:926cd923a480 | 153 | //while (mybutton == 1) {} |
xx123456987 | 0:926cd923a480 | 154 | |
xx123456987 | 0:926cd923a480 | 155 | while(c>0) { |
xx123456987 | 0:926cd923a480 | 156 | wait(1); |
xx123456987 | 0:926cd923a480 | 157 | printf("%d\n",c--); |
xx123456987 | 0:926cd923a480 | 158 | myled = !myled; |
xx123456987 | 0:926cd923a480 | 159 | } |
xx123456987 | 0:926cd923a480 | 160 | |
xx123456987 | 0:926cd923a480 | 161 | while(1) { |
xx123456987 | 0:926cd923a480 | 162 | // |
xx123456987 | 0:926cd923a480 | 163 | myled = 0; |
xx123456987 | 0:926cd923a480 | 164 | X1[0] = Current_X[0]; |
xx123456987 | 0:926cd923a480 | 165 | X1[1] = Current_X[1]; |
xx123456987 | 0:926cd923a480 | 166 | X1[2] = Current_X[2]; |
xx123456987 | 0:926cd923a480 | 167 | |
xx123456987 | 0:926cd923a480 | 168 | // odometry beginning // |
xx123456987 | 0:926cd923a480 | 169 | qei1_new = w1.getPulses(); |
xx123456987 | 0:926cd923a480 | 170 | qei2_new = w2.getPulses(); |
xx123456987 | 0:926cd923a480 | 171 | qei3_new = w3.getPulses(); |
xx123456987 | 0:926cd923a480 | 172 | |
xx123456987 | 0:926cd923a480 | 173 | double qei1 = qei1_new - qei1_old; |
xx123456987 | 0:926cd923a480 | 174 | double qei2 = qei2_new - qei2_old; |
xx123456987 | 0:926cd923a480 | 175 | double qei3 = qei3_new - qei3_old; |
xx123456987 | 0:926cd923a480 | 176 | |
xx123456987 | 0:926cd923a480 | 177 | //wait_ms(50); |
xx123456987 | 0:926cd923a480 | 178 | |
xx123456987 | 0:926cd923a480 | 179 | d_theta1 = qei1*360*pi/(4096*6*180); //DEG to RAD |
xx123456987 | 0:926cd923a480 | 180 | d_theta2 = qei2*360*pi/(4096*6*180); |
xx123456987 | 0:926cd923a480 | 181 | d_theta3 = qei3*360*pi/(4096*6*180); |
xx123456987 | 0:926cd923a480 | 182 | d_theta = c1*( d_theta1 + d_theta2 + d_theta3 ); |
xx123456987 | 0:926cd923a480 | 183 | |
xx123456987 | 0:926cd923a480 | 184 | // compute theta |
xx123456987 | 0:926cd923a480 | 185 | Next_X[2] = Current_X[2] + d_theta; |
xx123456987 | 0:926cd923a480 | 186 | double theta = Current_X[2]; |
xx123456987 | 0:926cd923a480 | 187 | float c3 = Current_X[2] + d_theta/2; |
xx123456987 | 0:926cd923a480 | 188 | // compute y |
xx123456987 | 0:926cd923a480 | 189 | 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 | 190 | // compute x |
xx123456987 | 0:926cd923a480 | 191 | 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 | 192 | |
xx123456987 | 0:926cd923a480 | 193 | // transition |
xx123456987 | 0:926cd923a480 | 194 | Current_X[2] = Next_X[2]; |
xx123456987 | 0:926cd923a480 | 195 | Current_X[1] = Next_X[1]; |
xx123456987 | 0:926cd923a480 | 196 | Current_X[0] = Next_X[0]; |
xx123456987 | 0:926cd923a480 | 197 | |
xx123456987 | 0:926cd923a480 | 198 | qei1_old = qei1_new; |
xx123456987 | 0:926cd923a480 | 199 | qei2_old = qei2_new; |
xx123456987 | 0:926cd923a480 | 200 | qei3_old = qei3_new; |
xx123456987 | 0:926cd923a480 | 201 | // odometry end // |
xx123456987 | 0:926cd923a480 | 202 | |
xx123456987 | 0:926cd923a480 | 203 | // control law beginning // |
xx123456987 | 0:926cd923a480 | 204 | //#define r 0.05 |
xx123456987 | 0:926cd923a480 | 205 | //#define L 0.125 |
xx123456987 | 0:926cd923a480 | 206 | |
xx123456987 | 0:926cd923a480 | 207 | X1_b[0] = X1[0]-f1[0]; |
xx123456987 | 0:926cd923a480 | 208 | X1_b[1] = X1[1]-f1[1]; |
xx123456987 | 0:926cd923a480 | 209 | X1_b[2] = X1[2]-f1[2]; |
xx123456987 | 0:926cd923a480 | 210 | u[0] = -c0*(X1_b[0]-XL[0]); |
xx123456987 | 0:926cd923a480 | 211 | u[1] = -c0*(X1_b[1]-XL[1]); |
xx123456987 | 0:926cd923a480 | 212 | u[2] = -c0*(X1_b[2]-XL[2]); |
xx123456987 | 0:926cd923a480 | 213 | // |
xx123456987 | 0:926cd923a480 | 214 | omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta); |
xx123456987 | 0:926cd923a480 | 215 | omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta); |
xx123456987 | 0:926cd923a480 | 216 | omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta); |
xx123456987 | 0:926cd923a480 | 217 | // control law end // |
xx123456987 | 0:926cd923a480 | 218 | |
xx123456987 | 0:926cd923a480 | 219 | //theta = theta + pi/3; // shift: origin to new |
xx123456987 | 0:926cd923a480 | 220 | |
xx123456987 | 0:926cd923a480 | 221 | // omega to PWM // |
xx123456987 | 0:926cd923a480 | 222 | pwm_w1 = 0.5 + omg1/2; |
xx123456987 | 0:926cd923a480 | 223 | pwm_w2 = 0.5 + omg2/2; |
xx123456987 | 0:926cd923a480 | 224 | pwm_w3 = 0.5 + omg3/2; |
xx123456987 | 0:926cd923a480 | 225 | |
xx123456987 | 0:926cd923a480 | 226 | pwm_1.write(pwm_w1); |
xx123456987 | 0:926cd923a480 | 227 | pwm_2.write(pwm_w2); |
xx123456987 | 0:926cd923a480 | 228 | pwm_3.write(pwm_w3); |
xx123456987 | 0:926cd923a480 | 229 | // END // |
xx123456987 | 0:926cd923a480 | 230 | |
xx123456987 | 0:926cd923a480 | 231 | // define error // not abs() yet |
xx123456987 | 0:926cd923a480 | 232 | err[0] = Current_X[0]-XL[0]; |
xx123456987 | 0:926cd923a480 | 233 | err[1] = Current_X[1]-XL[1]; |
xx123456987 | 0:926cd923a480 | 234 | err[2] = Current_X[2]-XL[2]; |
xx123456987 | 0:926cd923a480 | 235 | // END // |
xx123456987 | 0:926cd923a480 | 236 | if ( t.read_ms() > 200) { // print |
xx123456987 | 0:926cd923a480 | 237 | |
xx123456987 | 0:926cd923a480 | 238 | t.reset(); |
xx123456987 | 0:926cd923a480 | 239 | t.start(); |
xx123456987 | 0:926cd923a480 | 240 | if (s0 == true) printf ("=================Homing====================\n"); |
xx123456987 | 0:926cd923a480 | 241 | else printf ("===========================================\n"); |
xx123456987 | 0:926cd923a480 | 242 | 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 | 243 | printf ("Error : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", (err[0])*100, (err[1])*100, (err[2])*180/pi); |
xx123456987 | 0:926cd923a480 | 244 | /* |
xx123456987 | 0:926cd923a480 | 245 | printf ("\nWheel[1] counts: %d | revolutions: % .2f", qei1_new, (float)qei1_new/(4096*6)); |
xx123456987 | 0:926cd923a480 | 246 | printf ("\nWheel[2] counts: %d | revolutions: % .2f", qei2_new, (float)qei2_new/(4096*6)); |
xx123456987 | 0:926cd923a480 | 247 | printf ("\nWheel[3] counts: %d | revolutions: % .2f", qei3_new, (float)qei3_new/(4096*6)); |
xx123456987 | 0:926cd923a480 | 248 | */ |
xx123456987 | 0:926cd923a480 | 249 | //printf("Wheel Vel. [%3.2f %3.2f %3.2f]\n",pwm_w1,pwm_w2,pwm_w3); |
xx123456987 | 0:926cd923a480 | 250 | printf("OMEGA: [%.2f %.2f %.2f]\n",omg1,omg2,omg3); |
xx123456987 | 0:926cd923a480 | 251 | printf("PWM: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100); |
xx123456987 | 0:926cd923a480 | 252 | //printf("X: [%.3f] Y:[%.3f]\n",px,py); |
xx123456987 | 0:926cd923a480 | 253 | //printf("px+py=%.2f\n", px+py); |
xx123456987 | 0:926cd923a480 | 254 | |
xx123456987 | 0:926cd923a480 | 255 | } |
xx123456987 | 0:926cd923a480 | 256 | if ( abs(err[0])<=(ex/100) && abs(err[1])<=(ey/100) && abs(err[2])<=(etheta*pi/180) ) { |
xx123456987 | 0:926cd923a480 | 257 | printf("The robot Arrived!\n"); |
xx123456987 | 0:926cd923a480 | 258 | pwm_1.write(0.50); |
xx123456987 | 0:926cd923a480 | 259 | pwm_2.write(0.50); |
xx123456987 | 0:926cd923a480 | 260 | pwm_3.write(0.50); |
xx123456987 | 0:926cd923a480 | 261 | //s0 = true; |
xx123456987 | 0:926cd923a480 | 262 | c = 3; |
xx123456987 | 0:926cd923a480 | 263 | goto aa; |
xx123456987 | 0:926cd923a480 | 264 | /* |
xx123456987 | 0:926cd923a480 | 265 | printf("Press USER_BUTTON to go HOME\n"); |
xx123456987 | 0:926cd923a480 | 266 | while (mybutton == 1) { |
xx123456987 | 0:926cd923a480 | 267 | wait(0.25); |
xx123456987 | 0:926cd923a480 | 268 | myled = !myled; |
xx123456987 | 0:926cd923a480 | 269 | |
xx123456987 | 0:926cd923a480 | 270 | } |
xx123456987 | 0:926cd923a480 | 271 | |
xx123456987 | 0:926cd923a480 | 272 | while(c>0) { |
xx123456987 | 0:926cd923a480 | 273 | wait(1); |
xx123456987 | 0:926cd923a480 | 274 | printf("%d\n",c--); |
xx123456987 | 0:926cd923a480 | 275 | myled = !myled; |
xx123456987 | 0:926cd923a480 | 276 | } |
xx123456987 | 0:926cd923a480 | 277 | XL[0]=0; |
xx123456987 | 0:926cd923a480 | 278 | XL[1]=0; |
xx123456987 | 0:926cd923a480 | 279 | XL[2]=0+pi/3; |
xx123456987 | 0:926cd923a480 | 280 | */ |
xx123456987 | 0:926cd923a480 | 281 | } |
xx123456987 | 0:926cd923a480 | 282 | |
xx123456987 | 0:926cd923a480 | 283 | wait_ms(10);// 會有什麼影響? |
xx123456987 | 0:926cd923a480 | 284 | } |
xx123456987 | 0:926cd923a480 | 285 | } |