中継機能つけた受け取りオムニ
Dependencies: mbed MultiSerial
Diff: main.cpp
- Revision:
- 13:8a9d3855ab96
- Parent:
- 12:6e2ee1c83ac1
- Child:
- 14:731c52eb333b
diff -r 6e2ee1c83ac1 -r 8a9d3855ab96 main.cpp --- a/main.cpp Thu Sep 04 08:50:40 2014 +0000 +++ b/main.cpp Mon Sep 08 04:13:50 2014 +0000 @@ -1,13 +1,14 @@ /* * 4WD_OMNI_simple by Tomoki Hokida -* Simple Type + * Simple Type * * motor pins piar - * motor(p21,p22):motor(p23,p24) - * motor(p25,p26):motor(p27,p28) - * 通信中継用 - * data[0]:arm - * data[1]:omni + * motor(p13,p14):motor(p15,p16) + * motor(p17,p18):motor(p19,p20) + * + * Serial Pins + * packet.arm[0]:arm + * packet.leg[1]:omni * reset 0 */ @@ -15,23 +16,27 @@ #include "MultiSerial.h" #define DATA_NUM 8 //DATA_NUM[byte]通信 この値はmemcpy使ってるんでxbee_packet構造体のメモリ数といい感じに合わせるようにしてください -#define XBEE_KEY 0xAA//keycode +#define XBEE_KEY 0xAA //keycode #define ARM_KEY 0xAA #define PWM 1.0 +#define PWM_L 0.5 +#define PWM_R 0.5 +#define PWM_U 0.5 +#define PWM_D 0.5 + BusOut check(LED1,LED2,LED3,LED4); -//BusIn sw(p21,p22,p23,p24); -//BusOut motors(p21,p22,p23,p24,p25,p26,p27,p28); -BusOut motors(p5,p6,p7,p8); +BusOut motors(p13,p14,p15,p16,p17,p18,p19,p20); PwmOut pwm[4]={p21,p22,p23,p24}; Serial pc(USBTX,USBRX); MultiSerial xbee(p28,p27,read); -MultiSerial armMbed(p13,p14,write); +MultiSerial armMbed(p9,p10,write); +/* data structure */ typedef struct{ uint8_t arm[1]; @@ -42,29 +47,44 @@ xbee_packet packet; uint8_t get_data[DATA_NUM]={0}; +uint8_t value[4]; + +/* Out put PC stdout function */ +void pc_print(){ + + static int count=0; + + if(count==10e3){ + pc.printf(" arm = %d" ,packet.arm[0]); + pc.printf(" leg = %d" ,packet.leg); + count=0; + } + count++; +} int main() -{ - //sw.mode(PullUp); - - //PWM init +{ + + /* PWM init */ pwm[0] = PWM; pwm[1] = PWM; pwm[2] = PWM; pwm[3] = PWM; + value[0] = PWM_L; + value[1] = PWM_R; + value[2] = PWM_U; + value[3] = PWM_D; + xbee_packet *pt_packet=&packet; - xbee.read_data(get_data,XBEE_KEY); - int i=0; + xbee.read_data(get_data,XBEE_KEY); + for(;;){ - wait_ms(0.1); + wait_ms(0.1); - // get_data[0]=0xFF; - // get_data[1]=0x01; - - //memcpy(&packet, get_data, DATA_NUM); //ちょい危険 + //memcpy(&packet, get_data, DATA_NUM); packet.arm[0] = get_data[1]; packet.leg = get_data[0]; @@ -73,17 +93,28 @@ armMbed.write_data(pt_packet->arm,ARM_KEY); + /* Stop */ if(packet.leg==0x0) motors = 0; - if(packet.leg&0x1) motors = 0x05; //p21,p23 - if(packet.leg&0x2) motors = 0x0A; //p22,p24 - if(packet.leg&0x4) motors = 0x50; //p25,p27 - if(packet.leg&0x8) motors = 0xA0; //p26,p28 + + /* PWM */ + if((packet.leg>>4)==0){ + + for(int i=0;i==4;i++){ + pwm[i] = value[i]; + } + } - if(i==10e3){ - pc.printf(" arm = %d" ,packet.arm[0]); - pc.printf(" leg = %d" ,packet.leg); - i=0; - } - i++; + /* Direction of movement */ + if(packet.leg&0x1) motors = 0x55; //01010101 + if(packet.leg&0x8) motors = 0xAA; //10101010 + if(packet.leg&0x2) motors = 0x5A; //01011010 + if(packet.leg&0x4) motors = 0xA5; //10100101 + + /* Turn circling */ + if(packet.leg&0x10) motors = 0x66; //01010101 + if(packet.leg&0x80) motors = 0x99; //10101010 + + pc_print(); + } } \ No newline at end of file