中継機能つけた受け取りオムニ
Dependencies: mbed MultiSerial
Diff: main.cpp
- Revision:
- 15:1c1ae4077900
- Parent:
- 14:731c52eb333b
- Child:
- 16:4b502070bea8
--- a/main.cpp Mon Sep 08 07:22:30 2014 +0000 +++ b/main.cpp Tue Sep 23 06:21:30 2014 +0000 @@ -7,7 +7,7 @@ * motor(p17,p18):motor(p19,p20) * * Serial Pins - * packet.arm[0]:arm + * packet.arm[0]:arm * packet.leg[1]:omni * reset 0 */ @@ -19,17 +19,27 @@ #define XBEE_KEY 0xAA //keycode #define ARM_KEY 0xAA -#define PWM 0.5 +#define PWM 1.0 + +#define PWM_L 1.0 +#define PWM_R 1.0 +#define PWM_U 1.0 +#define PWM_D 1.0 -#define PWM_L 0.5 -#define PWM_R 0.5 -#define PWM_U 0.5 -#define PWM_D 0.5 +#define R 0x4 +#define L 0x1 +#define U 0x2 +#define D 0x8 + +#define R_t 0x10 +#define L_t 0x40 + +#define TIME 0 BusOut check(LED1,LED2,LED3,LED4); BusOut motors(p13,p14,p15,p16,p17,p18,p19,p20); -PwmOut pwm[4]={p21,p22,p23,p24}; +PwmOut pwm[4]= {p21,p22,p23,p24}; Serial pc(USBTX,USBRX); @@ -37,32 +47,53 @@ MultiSerial armMbed(p9,p10,write); /* data structure */ -typedef struct{ +typedef struct { uint8_t arm[1]; uint8_t leg; -}xbee_packet; +} xbee_packet; xbee_packet packet; -uint8_t get_data[DATA_NUM]={0}; +uint8_t get_data[DATA_NUM]= {0}; /* Out put PC stdout function */ + void pc_print(){ static int count=0; - if(count==10e3){ + if(count==10e3) { pc.printf(" arm = %d" ,packet.arm[0]); pc.printf(" leg = %d" ,packet.leg); count=0; - } + } count++; } +void stop_slowly(int count) +{ + + for(float i=1.0; i>=0; i-=0.1) { + + /*for(int j=0; j>=4; j++) { + + pwm[j] = i; + + }*/ + pwm[0] = 0.1; + pwm[1] = 0.1; + pwm[2] = 0.1; + pwm[3] = 0.1; + + wait(count*10e-3); //count value * 0.01 sec + } + check = 0; +} + int main() -{ +{ /* PWM init */ pwm[0] = PWM; @@ -74,50 +105,67 @@ xbee.read_data(get_data,XBEE_KEY); - for(;;){ + int counter=0; - wait_ms(0.1); + for(;;) { + + wait_ms(1.0); //memcpy(&packet, get_data, DATA_NUM); packet.arm[0] = get_data[1]; - packet.leg = get_data[0]; + packet.leg = get_data[0]; - check = get_data[1]; + //check = get_data[1]; - armMbed.write_data(pt_packet->arm,ARM_KEY); + armMbed.write_data(pt_packet->arm,ARM_KEY); /* Stop */ - if(packet.leg==0x0) motors = 0; + if(packet.leg==0x0) { + + counter /= 10e2; //mirisec -->> sec + + if(counter>=TIME) { //wait 2 sec + + stop_slowly(counter); + } - /* PWM */ - if((packet.leg>>4)==0){ - + motors = 0; + counter = 0; //cunter init + + } else { + + counter++; + check = 0xFF; + } + + /* PWM */ + if((packet.leg>>4)==0) { + pwm[0] = PWM_L; pwm[1] = PWM_R; pwm[2] = PWM_U; - pwm[3] = PWM_D; + pwm[3] = PWM_D; - }else{ + } else { pwm[0] = PWM; pwm[1] = PWM; pwm[2] = PWM; pwm[3] = PWM; - + } /* Direction of movement */ - if(packet.leg&0x1) motors = 0x55; //01010101 L 接続に気をつけること - if(packet.leg&0x4) motors = 0xAA; //10101010 U 多分モータードライバの信号線をそれぞれ逆にすればOKかな - if(packet.leg&0x2) motors = 0x5A; //01011010 R - if(packet.leg&0x8) motors = 0xA5; //10100101 D + if(packet.leg&R) motors = 0x55; //01010101 R 接続に気をつけること + if(packet.leg&L) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな + if(packet.leg&U) motors = 0x5A; //01011010 U + if(packet.leg&D) motors = 0xA5; //10100101 D /* Turn circling */ - if(packet.leg&0x10) motors = 0x66; //01100110 - if(packet.leg&0x40) motors = 0x99; //10011001 + if(packet.leg&R_t) motors = 0x66; //01100110 + if(packet.leg&L_t) motors = 0x99; //10011001 pc_print(); - } } \ No newline at end of file