匡尚 青柳 / Mbed 2 deprecated LineTrace_frdm

ライントレース

Dependencies:   TextLCD mbed

main.cpp@0:19606d41f8c8, 2016-09-29 (annotated)

Committer:
ojityan
Date:
Thu Sep 29 07:37:00 2016 +0000
Revision:
0:19606d41f8c8
??????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojityan 0:19606d41f8c8 1 #include "mbed.h"
ojityan 0:19606d41f8c8 2 #include "TextLCD.h"
ojityan 0:19606d41f8c8 3
ojityan 0:19606d41f8c8 4 #define Center_anL 0.01f
ojityan 0:19606d41f8c8 5 #define Center_anR 0.01f
ojityan 0:19606d41f8c8 6 #define Center_anL_lift 0.01f
ojityan 0:19606d41f8c8 7 #define Center_anR_lift 0.30f
ojityan 0:19606d41f8c8 8
ojityan 0:19606d41f8c8 9 #define Center_an 0.25f
ojityan 0:19606d41f8c8 10
ojityan 0:19606d41f8c8 11 #define spin_num1 0.005
ojityan 0:19606d41f8c8 12 #define spin_num2 0.008
ojityan 0:19606d41f8c8 13 /*
ojityan 0:19606d41f8c8 14 #define lift_CW() mortor1 = 1; mortor2 = 0
ojityan 0:19606d41f8c8 15 #define lift_CCW() mortor1 = 0; mortor2 = 1
ojityan 0:19606d41f8c8 16 #define lift_STOP() mortor1 = 0; mortor2 = 0
ojityan 0:19606d41f8c8 17 */
ojityan 0:19606d41f8c8 18 BusOut LEDArray(PTB8, PTB9, PTB10, PTB11, PTE2, PTE3, PTE4, PTE5);
ojityan 0:19606d41f8c8 19 TextLCD lcd(PTC12, PTC13, PTC16, PTC17, PTA16, PTA17);
ojityan 0:19606d41f8c8 20
ojityan 0:19606d41f8c8 21 DigitalOut led_l(PTB9);
ojityan 0:19606d41f8c8 22 DigitalOut led_r(PTB11);
ojityan 0:19606d41f8c8 23 DigitalOut led_c(PTB10);
ojityan 0:19606d41f8c8 24
ojityan 0:19606d41f8c8 25 //USBSerial vcom;
ojityan 0:19606d41f8c8 26 Timer t;
ojityan 0:19606d41f8c8 27
ojityan 0:19606d41f8c8 28 DigitalOut myled(PTB8);
ojityan 0:19606d41f8c8 29
ojityan 0:19606d41f8c8 30 AnalogIn SENSOR2_(PTB0);//R
ojityan 0:19606d41f8c8 31 AnalogIn SENSOR1_(PTC2);//L
ojityan 0:19606d41f8c8 32 AnalogIn SENSOR3_(PTB2);//C
ojityan 0:19606d41f8c8 33
ojityan 0:19606d41f8c8 34 DigitalIn SENSOR3(PTE21);
ojityan 0:19606d41f8c8 35
ojityan 0:19606d41f8c8 36 AnalogIn lift_sensor_R_(PTB3);
ojityan 0:19606d41f8c8 37 AnalogIn lift_sensor_L_(PTB1);
ojityan 0:19606d41f8c8 38
ojityan 0:19606d41f8c8 39 DigitalOut lift_led_R(PTE2);//R
ojityan 0:19606d41f8c8 40 DigitalOut lift_led_L(PTB8);//L
ojityan 0:19606d41f8c8 41
ojityan 0:19606d41f8c8 42 DigitalOut spin_led(PTE5);
ojityan 0:19606d41f8c8 43 DigitalOut time_led(PTE4);
ojityan 0:19606d41f8c8 44
ojityan 0:19606d41f8c8 45 BusOut iL(PTA1,PTA2);
ojityan 0:19606d41f8c8 46 PwmOut pL(PTD4);
ojityan 0:19606d41f8c8 47
ojityan 0:19606d41f8c8 48 BusOut iR(PTC0,PTC7);
ojityan 0:19606d41f8c8 49 PwmOut pR(PTA12);
ojityan 0:19606d41f8c8 50
ojityan 0:19606d41f8c8 51 BusOut i_lift(PTC6,PTC5);
ojityan 0:19606d41f8c8 52 PwmOut p_lift(PTA4);
ojityan 0:19606d41f8c8 53
ojityan 0:19606d41f8c8 54 union {
ojityan 0:19606d41f8c8 55 unsigned char ALL;
ojityan 0:19606d41f8c8 56 struct {
ojityan 0:19606d41f8c8 57 unsigned R : 1;
ojityan 0:19606d41f8c8 58 unsigned L : 1;
ojityan 0:19606d41f8c8 59 unsigned lift : 1;
ojityan 0:19606d41f8c8 60 } bits;
ojityan 0:19606d41f8c8 61 } flags;
ojityan 0:19606d41f8c8 62
ojityan 0:19606d41f8c8 63 void liftmove(void);
ojityan 0:19606d41f8c8 64 void lift_stop(void);
ojityan 0:19606d41f8c8 65 double vector(double num);
ojityan 0:19606d41f8c8 66 void drive_set(int num_L, int num_R, char mode_L, char mode_R);
ojityan 0:19606d41f8c8 67 void liftmove(void);
ojityan 0:19606d41f8c8 68 void spin(void);
ojityan 0:19606d41f8c8 69
ojityan 0:19606d41f8c8 70 void drive_set(int num_L, int num_R, char mode_L, char mode_R);
ojityan 0:19606d41f8c8 71 void MOTOR_A_STOP(void);//左
ojityan 0:19606d41f8c8 72 void MOTOR_A_GO(void);
ojityan 0:19606d41f8c8 73 void MOTOR_A_BACK(void);
ojityan 0:19606d41f8c8 74 void MOTOR_A_BRAKE(void);
ojityan 0:19606d41f8c8 75 void MOTOR_B_STOP(void);//右
ojityan 0:19606d41f8c8 76 void MOTOR_B_GO(void);
ojityan 0:19606d41f8c8 77 void MOTOR_B_BACK(void);
ojityan 0:19606d41f8c8 78 void MOTOR_B_BRAKE(void);
ojityan 0:19606d41f8c8 79 void lift_STOP(void);
ojityan 0:19606d41f8c8 80 void lift_CW(void);
ojityan 0:19606d41f8c8 81 void lift_CCW(void);
ojityan 0:19606d41f8c8 82
ojityan 0:19606d41f8c8 83 void led_flash(char SENSOR1, char SENSOR2, char SENSOR3, char lift_sensor_L, char lift_sensor_R);
ojityan 0:19606d41f8c8 84 void past_data(int past);
ojityan 0:19606d41f8c8 85 void lift_f(char lift_sensor_R, char lift_sensor_L);
ojityan 0:19606d41f8c8 86
ojityan 0:19606d41f8c8 87 int main(void)
ojityan 0:19606d41f8c8 88 {
ojityan 0:19606d41f8c8 89 myled = 0;
ojityan 0:19606d41f8c8 90 int past = 7;
ojityan 0:19606d41f8c8 91 int mode = 0;
ojityan 0:19606d41f8c8 92 char SENSOR1;
ojityan 0:19606d41f8c8 93 char SENSOR2;
ojityan 0:19606d41f8c8 94 char SENSOR3;
ojityan 0:19606d41f8c8 95 char lift_sensor_R;
ojityan 0:19606d41f8c8 96 char lift_sensor_L;
ojityan 0:19606d41f8c8 97
ojityan 0:19606d41f8c8 98 while(1) {
ojityan 0:19606d41f8c8 99 //lcd.printf("%3f,%3f,%3f\n%3f,%3f\n",SENSOR1_.read(),SENSOR2_.read(),SENSOR3_.read(),lift_sensor_R_.read(),lift_sensor_L_.read());
ojityan 0:19606d41f8c8 100 if(SENSOR1_.read() < Center_an) {
ojityan 0:19606d41f8c8 101 SENSOR1 = 0;
ojityan 0:19606d41f8c8 102 } else {
ojityan 0:19606d41f8c8 103 SENSOR1 = 1;
ojityan 0:19606d41f8c8 104 }
ojityan 0:19606d41f8c8 105
ojityan 0:19606d41f8c8 106 if(SENSOR2_.read() < Center_an) {
ojityan 0:19606d41f8c8 107 SENSOR2 = 0;
ojityan 0:19606d41f8c8 108 } else {
ojityan 0:19606d41f8c8 109 SENSOR2 = 1;
ojityan 0:19606d41f8c8 110 }
ojityan 0:19606d41f8c8 111
ojityan 0:19606d41f8c8 112
ojityan 0:19606d41f8c8 113 if(SENSOR3_.read() < Center_an) {
ojityan 0:19606d41f8c8 114 SENSOR3 = 0;
ojityan 0:19606d41f8c8 115 } else {
ojityan 0:19606d41f8c8 116 SENSOR3 = 1;
ojityan 0:19606d41f8c8 117 }
ojityan 0:19606d41f8c8 118
ojityan 0:19606d41f8c8 119 if(lift_sensor_R_.read() < Center_an) {
ojityan 0:19606d41f8c8 120 lift_sensor_R = 0;
ojityan 0:19606d41f8c8 121 } else {
ojityan 0:19606d41f8c8 122 lift_sensor_R = 1;
ojityan 0:19606d41f8c8 123 }
ojityan 0:19606d41f8c8 124
ojityan 0:19606d41f8c8 125 if(lift_sensor_L_.read() < Center_an) {
ojityan 0:19606d41f8c8 126 lift_sensor_L = 0;
ojityan 0:19606d41f8c8 127 } else {
ojityan 0:19606d41f8c8 128 lift_sensor_L = 1;
ojityan 0:19606d41f8c8 129 }
ojityan 0:19606d41f8c8 130
ojityan 0:19606d41f8c8 131 led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
ojityan 0:19606d41f8c8 132
ojityan 0:19606d41f8c8 133
ojityan 0:19606d41f8c8 134
ojityan 0:19606d41f8c8 135 switch(mode) {
ojityan 0:19606d41f8c8 136 case 0:
ojityan 0:19606d41f8c8 137 case 3: {
ojityan 0:19606d41f8c8 138 if(lift_sensor_R == 0 && !lift_sensor_L) {
ojityan 0:19606d41f8c8 139 liftmove();
ojityan 0:19606d41f8c8 140 mode = 1;
ojityan 0:19606d41f8c8 141 }
ojityan 0:19606d41f8c8 142 break;
ojityan 0:19606d41f8c8 143 }
ojityan 0:19606d41f8c8 144 case 1: {
ojityan 0:19606d41f8c8 145 if(!lift_sensor_R) {
ojityan 0:19606d41f8c8 146 t.start();
ojityan 0:19606d41f8c8 147 time_led = 1;
ojityan 0:19606d41f8c8 148 mode = 2;
ojityan 0:19606d41f8c8 149 }
ojityan 0:19606d41f8c8 150 break;
ojityan 0:19606d41f8c8 151 }
ojityan 0:19606d41f8c8 152 case 2: {
ojityan 0:19606d41f8c8 153 if(t.read() > 2.0) {
ojityan 0:19606d41f8c8 154 t.stop();
ojityan 0:19606d41f8c8 155 time_led = 0;
ojityan 0:19606d41f8c8 156 mode = 3;
ojityan 0:19606d41f8c8 157 }
ojityan 0:19606d41f8c8 158 break;
ojityan 0:19606d41f8c8 159 }
ojityan 0:19606d41f8c8 160 }
ojityan 0:19606d41f8c8 161
ojityan 0:19606d41f8c8 162 // lift_f();
ojityan 0:19606d41f8c8 163
ojityan 0:19606d41f8c8 164 //すべてのセンサが黒の時, モータAが正転, モータBが正転
ojityan 0:19606d41f8c8 165 if(SENSOR1 == 0 && SENSOR2 == 0 && SENSOR3 == 0) {
ojityan 0:19606d41f8c8 166 MOTOR_A_GO() ;
ojityan 0:19606d41f8c8 167 MOTOR_B_GO() ;
ojityan 0:19606d41f8c8 168 // past = 7;
ojityan 0:19606d41f8c8 169 led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
ojityan 0:19606d41f8c8 170 }
ojityan 0:19606d41f8c8 171 //センサ1と2が黒でセンサ3が白の時, モータAとBが両方とも正転.
ojityan 0:19606d41f8c8 172 else if(SENSOR1 == 0 && SENSOR2 == 0 &&SENSOR3 == 1) {
ojityan 0:19606d41f8c8 173 MOTOR_A_GO();
ojityan 0:19606d41f8c8 174 MOTOR_B_GO();
ojityan 0:19606d41f8c8 175 // past = 5;
ojityan 0:19606d41f8c8 176 led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
ojityan 0:19606d41f8c8 177 }
ojityan 0:19606d41f8c8 178 //センサ1と3が黒でセンサ2が白の時, モータAが逆転, モータBが正転.
ojityan 0:19606d41f8c8 179 else if(SENSOR1 == 0 && SENSOR2 == 1 && SENSOR3 == 0) {
ojityan 0:19606d41f8c8 180 MOTOR_A_BACK() ;
ojityan 0:19606d41f8c8 181 MOTOR_B_GO();
ojityan 0:19606d41f8c8 182 past = 6;
ojityan 0:19606d41f8c8 183 led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
ojityan 0:19606d41f8c8 184 }
ojityan 0:19606d41f8c8 185 //センサ1が黒でセンサ2と3が白の時, モータAが停止, モータBが正転.
ojityan 0:19606d41f8c8 186 else if(SENSOR1 == 0 && SENSOR2 == 1 && SENSOR3 == 1) {
ojityan 0:19606d41f8c8 187 MOTOR_A_STOP() ;
ojityan 0:19606d41f8c8 188 MOTOR_B_GO();
ojityan 0:19606d41f8c8 189 past = 4;
ojityan 0:19606d41f8c8 190 led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
ojityan 0:19606d41f8c8 191 }
ojityan 0:19606d41f8c8 192 //センサ1が白でセンサ2と3が黒の時, モータAが正転, モータBが逆転.
ojityan 0:19606d41f8c8 193 else if(SENSOR1 == 1 && SENSOR2 == 0 && SENSOR3 == 0) {
ojityan 0:19606d41f8c8 194 MOTOR_A_GO() ;
ojityan 0:19606d41f8c8 195 MOTOR_B_BACK() ;
ojityan 0:19606d41f8c8 196 past = 3;
ojityan 0:19606d41f8c8 197 led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
ojityan 0:19606d41f8c8 198 }
ojityan 0:19606d41f8c8 199 //センサ1と3が白でセンサ2が黒の時, モータAが正転, モータBが停止.
ojityan 0:19606d41f8c8 200 else if(SENSOR1 == 1 && SENSOR2 == 0 && SENSOR3 == 1) {
ojityan 0:19606d41f8c8 201 MOTOR_A_GO() ;
ojityan 0:19606d41f8c8 202 MOTOR_B_STOP() ;
ojityan 0:19606d41f8c8 203 past = 1;
ojityan 0:19606d41f8c8 204 led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
ojityan 0:19606d41f8c8 205 }
ojityan 0:19606d41f8c8 206 //センサ1と2が白でセンサ３が黒の時, モータAが正転, モータBが正転.
ojityan 0:19606d41f8c8 207 else if(SENSOR1 == 1 && SENSOR2 == 1 && SENSOR3 == 0) {
ojityan 0:19606d41f8c8 208 MOTOR_A_GO() ;
ojityan 0:19606d41f8c8 209 MOTOR_B_GO() ;
ojityan 0:19606d41f8c8 210 // past = 2;
ojityan 0:19606d41f8c8 211 led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
ojityan 0:19606d41f8c8 212 }
ojityan 0:19606d41f8c8 213 //すべてのセンサが白の時，過去のデータを使う
ojityan 0:19606d41f8c8 214 else if(SENSOR1 == 1 && SENSOR2 == 1 && SENSOR3 == 1) {
ojityan 0:19606d41f8c8 215 past_data(past);
ojityan 0:19606d41f8c8 216 }
ojityan 0:19606d41f8c8 217 // wait(0.05);
ojityan 0:19606d41f8c8 218 }
ojityan 0:19606d41f8c8 219 }
ojityan 0:19606d41f8c8 220
ojityan 0:19606d41f8c8 221 void past_data(int past)//過去のデータを使ってライントレース
ojityan 0:19606d41f8c8 222 {
ojityan 0:19606d41f8c8 223 flags.bits.lift = 1;
ojityan 0:19606d41f8c8 224 switch(past) {
ojityan 0:19606d41f8c8 225 case 1: {
ojityan 0:19606d41f8c8 226 MOTOR_A_GO();
ojityan 0:19606d41f8c8 227 MOTOR_B_STOP();
ojityan 0:19606d41f8c8 228 break;
ojityan 0:19606d41f8c8 229 }
ojityan 0:19606d41f8c8 230 case 7:
ojityan 0:19606d41f8c8 231 case 5:
ojityan 0:19606d41f8c8 232 case 2: {
ojityan 0:19606d41f8c8 233 MOTOR_A_GO();
ojityan 0:19606d41f8c8 234 MOTOR_B_GO();
ojityan 0:19606d41f8c8 235 break;
ojityan 0:19606d41f8c8 236 }
ojityan 0:19606d41f8c8 237 case 3: {
ojityan 0:19606d41f8c8 238 MOTOR_A_GO();
ojityan 0:19606d41f8c8 239 MOTOR_B_BACK();
ojityan 0:19606d41f8c8 240 break;
ojityan 0:19606d41f8c8 241 }
ojityan 0:19606d41f8c8 242 case 4: {
ojityan 0:19606d41f8c8 243 MOTOR_A_STOP();
ojityan 0:19606d41f8c8 244 MOTOR_B_GO();
ojityan 0:19606d41f8c8 245 break;
ojityan 0:19606d41f8c8 246 }
ojityan 0:19606d41f8c8 247 case 6: {
ojityan 0:19606d41f8c8 248 MOTOR_A_BACK();
ojityan 0:19606d41f8c8 249 MOTOR_B_GO();
ojityan 0:19606d41f8c8 250 break;
ojityan 0:19606d41f8c8 251 }
ojityan 0:19606d41f8c8 252 }
ojityan 0:19606d41f8c8 253 }
ojityan 0:19606d41f8c8 254
ojityan 0:19606d41f8c8 255 void MOTOR_A_STOP(void)
ojityan 0:19606d41f8c8 256 {
ojityan 0:19606d41f8c8 257 iL = 0;
ojityan 0:19606d41f8c8 258 pL = 0.0f;
ojityan 0:19606d41f8c8 259 }
ojityan 0:19606d41f8c8 260 void MOTOR_A_GO(void)
ojityan 0:19606d41f8c8 261 {
ojityan 0:19606d41f8c8 262 iL = 1;
ojityan 0:19606d41f8c8 263 pL = 0.007f;
ojityan 0:19606d41f8c8 264 }
ojityan 0:19606d41f8c8 265 void MOTOR_A_BACK(void)
ojityan 0:19606d41f8c8 266 {
ojityan 0:19606d41f8c8 267 iL = 2;
ojityan 0:19606d41f8c8 268 pL = 0.01f;
ojityan 0:19606d41f8c8 269 }
ojityan 0:19606d41f8c8 270
ojityan 0:19606d41f8c8 271 void MOTOR_B_STOP(void)
ojityan 0:19606d41f8c8 272 {
ojityan 0:19606d41f8c8 273 iR = 0;
ojityan 0:19606d41f8c8 274 pR = 0.0f;
ojityan 0:19606d41f8c8 275 }
ojityan 0:19606d41f8c8 276 void MOTOR_B_GO(void)
ojityan 0:19606d41f8c8 277 {
ojityan 0:19606d41f8c8 278 iR = 1;
ojityan 0:19606d41f8c8 279 pR = 0.007f;
ojityan 0:19606d41f8c8 280 }
ojityan 0:19606d41f8c8 281 void MOTOR_B_BACK(void)
ojityan 0:19606d41f8c8 282 {
ojityan 0:19606d41f8c8 283 iR = 2;
ojityan 0:19606d41f8c8 284 pR = 0.01f;
ojityan 0:19606d41f8c8 285 }
ojityan 0:19606d41f8c8 286
ojityan 0:19606d41f8c8 287 void lift_STOP(void)
ojityan 0:19606d41f8c8 288 {
ojityan 0:19606d41f8c8 289 i_lift = 0;
ojityan 0:19606d41f8c8 290 p_lift = 0.0f;
ojityan 0:19606d41f8c8 291 }
ojityan 0:19606d41f8c8 292 void lift_CW(void)
ojityan 0:19606d41f8c8 293 {
ojityan 0:19606d41f8c8 294 i_lift = 2;
ojityan 0:19606d41f8c8 295 p_lift = 0.7f;
ojityan 0:19606d41f8c8 296 }
ojityan 0:19606d41f8c8 297 void lift_CCW(void)
ojityan 0:19606d41f8c8 298 {
ojityan 0:19606d41f8c8 299 i_lift = 1;
ojityan 0:19606d41f8c8 300 p_lift = 0.7f;
ojityan 0:19606d41f8c8 301 }
ojityan 0:19606d41f8c8 302
ojityan 0:19606d41f8c8 303 void led_flash(char SENSOR1, char SENSOR2, char SENSOR3, char lift_sensor_L, char lift_sensor_R)//LEDを光らせる
ojityan 0:19606d41f8c8 304 {
ojityan 0:19606d41f8c8 305 led_r = SENSOR1;
ojityan 0:19606d41f8c8 306 led_l = SENSOR2;
ojityan 0:19606d41f8c8 307 led_c = SENSOR3;
ojityan 0:19606d41f8c8 308 lift_led_L = lift_sensor_L;
ojityan 0:19606d41f8c8 309 lift_led_R = lift_sensor_R;
ojityan 0:19606d41f8c8 310 }
ojityan 0:19606d41f8c8 311
ojityan 0:19606d41f8c8 312
ojityan 0:19606d41f8c8 313
ojityan 0:19606d41f8c8 314 void liftmove(void)//リフトの動作
ojityan 0:19606d41f8c8 315 {
ojityan 0:19606d41f8c8 316 static char mode = 0;
ojityan 0:19606d41f8c8 317 if(!mode) {
ojityan 0:19606d41f8c8 318 wait(0.3);
ojityan 0:19606d41f8c8 319 MOTOR_A_STOP();
ojityan 0:19606d41f8c8 320 MOTOR_B_STOP();
ojityan 0:19606d41f8c8 321 // spin();
ojityan 0:19606d41f8c8 322 wait(1.0);
ojityan 0:19606d41f8c8 323 lift_CW();
ojityan 0:19606d41f8c8 324 wait(1.5);
ojityan 0:19606d41f8c8 325 lift_STOP();
ojityan 0:19606d41f8c8 326 mode = 1;
ojityan 0:19606d41f8c8 327 MOTOR_A_GO();
ojityan 0:19606d41f8c8 328 MOTOR_B_GO();
ojityan 0:19606d41f8c8 329 wait(0.5);
ojityan 0:19606d41f8c8 330 myled = 1;
ojityan 0:19606d41f8c8 331
ojityan 0:19606d41f8c8 332 } else {
ojityan 0:19606d41f8c8 333 MOTOR_A_STOP();
ojityan 0:19606d41f8c8 334 MOTOR_B_STOP();
ojityan 0:19606d41f8c8 335 wait(1.0);
ojityan 0:19606d41f8c8 336 lift_CCW();
ojityan 0:19606d41f8c8 337 wait(0.8);
ojityan 0:19606d41f8c8 338 lift_STOP();
ojityan 0:19606d41f8c8 339 MOTOR_A_BACK();
ojityan 0:19606d41f8c8 340 MOTOR_B_BACK();
ojityan 0:19606d41f8c8 341 wait(1.0);
ojityan 0:19606d41f8c8 342 MOTOR_A_STOP();
ojityan 0:19606d41f8c8 343 MOTOR_B_STOP();
ojityan 0:19606d41f8c8 344 myled = 0;
ojityan 0:19606d41f8c8 345 while(1) {
ojityan 0:19606d41f8c8 346 spin_led = !spin_led;
ojityan 0:19606d41f8c8 347 wait(0.1);
ojityan 0:19606d41f8c8 348 }
ojityan 0:19606d41f8c8 349 }
ojityan 0:19606d41f8c8 350 }
ojityan 0:19606d41f8c8 351 void spin(void)//確認用で入るとそこでストップし対応するLEDが点滅する
ojityan 0:19606d41f8c8 352 {
ojityan 0:19606d41f8c8 353 while(1) {
ojityan 0:19606d41f8c8 354 MOTOR_A_STOP();
ojityan 0:19606d41f8c8 355 MOTOR_B_STOP();
ojityan 0:19606d41f8c8 356 spin_led = !spin_led;
ojityan 0:19606d41f8c8 357 wait(0.1);
ojityan 0:19606d41f8c8 358 }
ojityan 0:19606d41f8c8 359 }