中継機能つけた受け取りオムニ
Dependencies: mbed MultiSerial
Diff: main.cpp
- Revision:
- 29:7a8efb8a9af8
- Parent:
- 28:4da223ae4392
- Child:
- 30:00277f63ffeb
diff -r 4da223ae4392 -r 7a8efb8a9af8 main.cpp --- a/main.cpp Tue Oct 07 09:51:52 2014 +0000 +++ b/main.cpp Tue Oct 07 10:07:56 2014 +0000 @@ -30,7 +30,7 @@ #define L_R 0x10 #define L_L 0x40 -#define L_U 0x20 +#define L_U 0x20 #define L_D 0x80 #define TIME 0 @@ -56,30 +56,31 @@ xbee_packet packet; uint8_t get_data[DATA_NUM]= {0}; -uint8_t check_data[2]={0}; +uint8_t check_data[2]= {0}; /*start display LED*/ -void display_LED(int kind){ +void display_LED(int kind) +{ - switch(kind){ - + switch(kind) { + case 0: //err check check = 0xA; wait(0.5); check != 0xF; wait(0.5); - + break; - + case 1: - // + // check = 0xF; wait(0.5); check = 0; break; - } } +} /*Safety Mode*/ @@ -87,19 +88,19 @@ { xbee.stop_read(); armMbed.stop_write(); - + for(;;) { - + motors = 0; display_LED(0); - + if(xbee.readable_check()==1) { - + NVIC_SystemReset(); - + //xbee.start_read(); //arm_mbed.start_write(); - return; + return; } } } @@ -118,17 +119,17 @@ //connect_check(); int radio_check=0; - + xbee.start_read(); xbee.read_data(get_data,XBEE_KEY); - + armMbed.start_write(); armMbed.write_data(pt_packet->arm,ARM_KEY); - + display_LED(1); - + int counter=0; - + for(;;) { wait_ms(0.5); @@ -139,42 +140,42 @@ //check = get_data[1]; counter++; - check = counter/100; - if(counter>=15*100){counter=0;} - - - if(xbee.readable_check() == 0) { - + check = counter/1000; + if(counter>=15*100) { + counter=0; + } + + + if(xbee.readable_check() == 0) { + radio_check++; - - if(radio_check>=500){ + + if(radio_check>=500) { safety_mode(); } - } else { - + } else { + radio_check = 0; - } - + } + /* Stop */ - if(packet.leg==0x0) { - - - motors = 0; - - pwm[0] = 0; - pwm[1] = 0; - pwm[2] = 0; - pwm[3] = 0; - + if(packet.leg==0x0) { + + motors = 0; - }else{ - - pwm[0] = PWM; - pwm[1] = PWM; - pwm[2] = PWM; - pwm[3] = PWM; - } + pwm[0] = 0; + pwm[1] = 0; + pwm[2] = 0; + pwm[3] = 0; + + } else { + + pwm[0] = PWM; + pwm[1] = PWM; + pwm[2] = PWM; + pwm[3] = PWM; + } /* Direction of movement */ if(packet.leg&R_D) motors = 0x55; //01010101 R 接続に気をつけること @@ -185,7 +186,7 @@ /* Turn circling */ if(packet.leg&L_L) motors = 0x66; //01100110 if(packet.leg&L_R) motors = 0x99; //10011001 - //if(packet.leg&L_U) + //if(packet.leg&L_U) //if(packet.leg&L_D) } } \ No newline at end of file