2015_robocon_bteam
/
mesure_swing_speed_20150904
回転機構の回転速度計測用プログラム
Diff: communicate.h
- Revision:
- 0:569847dea423
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/communicate.h Sat Sep 05 04:40:56 2015 +0000 @@ -0,0 +1,66 @@ +/***RS485 Communication.***/ + +Serial RS485(RS485_TX, RS485_RX); + +/***Get state ps3con.***/ +Serial SBDBT(SBDBT_TX, SBDBT_RX); +#define ps3_start 0x80 +short square=0,triangle=0,circle=0,cross=0,up=0,down=0,right=0,left=0,l1=0,l2=0,r1=0,r2=0; +int analog_lx=0,analog_ly=0,analog_rx=0,analog_ry=0; +int ps3_data[7]; +int count=0; +int sample=0; +inline void data_clear() +{ + for(int i=0; i<7; i++) ps3_data[i]=0; +} +inline void data_check() +{ + square=0,triangle=0,circle=0,cross=0,up=0,down=0,right=0,left=0,l1=0,l2=0,r1=0,r2=0; + analog_lx=0,analog_ly=0,analog_rx=0,analog_ry=0; + if((ps3_data[0]==0x00)&&(ps3_data[1]==0x01)) up=1; + else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x02)) down=1; + else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x04)) right=1; + else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x08)) left=1; + else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x10)) triangle=1; + else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x20)) cross=1; + else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x40)) circle=1; + else if((ps3_data[0]==0x01)&&(ps3_data[1]==0x00)) square=1; + else if((ps3_data[0]==0x02)&&(ps3_data[1]==0x00)) l1=1; + else if((ps3_data[0]==0x04)&&(ps3_data[1]==0x00)) l2=1; + else if((ps3_data[0]==0x08)&&(ps3_data[1]==0x00)) r1=1; + else if((ps3_data[0]==0x10)&&(ps3_data[1]==0x00)) r2=1; + analog_lx=ps3_data[2]; + analog_ly=ps3_data[3]; + analog_rx=ps3_data[4]; + analog_ry=ps3_data[5]; + if(!circle) edge_circle=1; + if(!triangle) edge_triangle=1; + if(!r1) edge_r1=1; + if(!l1) edge_l1=1; + if(!(analog_ry<40)) edge_r_up=1; + if(!(analog_ry>90)) edge_r_down=1; + if(!(analog_ly<40)) edge_l_up=1; + if(!(analog_ly>90)) edge_l_down=1; + if(!edge_right) edge_right=1; + if(!edge_left) edge_left=1; +} +///interrupt SBDBT RX. +inline void SBDBT_interrupt() +{ + sample=SBDBT.getc(); + if(count==7) data_clear(); + if(sample==ps3_start) count=0; + else { + ps3_data[count]=sample; + count++; + } + if(count==6) data_check(); +} +inline void initializeSBDBT() +{ + for(int i=0; i<7; i++) ps3_data[i]=0; + SBDBT.baud(2400); + SBDBT.format(8, Serial::None, 1); + SBDBT.attach(SBDBT_interrupt, Serial::RxIrq); +} \ No newline at end of file