2015_robocon_bteam / Mbed 2 deprecated mesure_swing_speed_20150904

回転機構の回転速度計測用プログラム

Dependencies:   PID QEI mbed

communicate.h@0:569847dea423, 2015-09-05 (annotated)

Committer:
DeguNaoto
Date:
Sat Sep 05 04:40:56 2015 +0000
Revision:
0:569847dea423
localfile

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:569847dea423 1 /***RS485 Communication.***/
DeguNaoto 0:569847dea423 2
DeguNaoto 0:569847dea423 3 Serial RS485(RS485_TX, RS485_RX);
DeguNaoto 0:569847dea423 4
DeguNaoto 0:569847dea423 5 /***Get state ps3con.***/
DeguNaoto 0:569847dea423 6 Serial SBDBT(SBDBT_TX, SBDBT_RX);
DeguNaoto 0:569847dea423 7 #define ps3_start 0x80
DeguNaoto 0:569847dea423 8 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;
DeguNaoto 0:569847dea423 9 int analog_lx=0,analog_ly=0,analog_rx=0,analog_ry=0;
DeguNaoto 0:569847dea423 10 int ps3_data[7];
DeguNaoto 0:569847dea423 11 int count=0;
DeguNaoto 0:569847dea423 12 int sample=0;
DeguNaoto 0:569847dea423 13 inline void data_clear()
DeguNaoto 0:569847dea423 14 {
DeguNaoto 0:569847dea423 15 for(int i=0; i<7; i++) ps3_data[i]=0;
DeguNaoto 0:569847dea423 16 }
DeguNaoto 0:569847dea423 17 inline void data_check()
DeguNaoto 0:569847dea423 18 {
DeguNaoto 0:569847dea423 19 square=0,triangle=0,circle=0,cross=0,up=0,down=0,right=0,left=0,l1=0,l2=0,r1=0,r2=0;
DeguNaoto 0:569847dea423 20 analog_lx=0,analog_ly=0,analog_rx=0,analog_ry=0;
DeguNaoto 0:569847dea423 21 if((ps3_data[0]==0x00)&&(ps3_data[1]==0x01)) up=1;
DeguNaoto 0:569847dea423 22 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x02)) down=1;
DeguNaoto 0:569847dea423 23 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x04)) right=1;
DeguNaoto 0:569847dea423 24 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x08)) left=1;
DeguNaoto 0:569847dea423 25 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x10)) triangle=1;
DeguNaoto 0:569847dea423 26 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x20)) cross=1;
DeguNaoto 0:569847dea423 27 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x40)) circle=1;
DeguNaoto 0:569847dea423 28 else if((ps3_data[0]==0x01)&&(ps3_data[1]==0x00)) square=1;
DeguNaoto 0:569847dea423 29 else if((ps3_data[0]==0x02)&&(ps3_data[1]==0x00)) l1=1;
DeguNaoto 0:569847dea423 30 else if((ps3_data[0]==0x04)&&(ps3_data[1]==0x00)) l2=1;
DeguNaoto 0:569847dea423 31 else if((ps3_data[0]==0x08)&&(ps3_data[1]==0x00)) r1=1;
DeguNaoto 0:569847dea423 32 else if((ps3_data[0]==0x10)&&(ps3_data[1]==0x00)) r2=1;
DeguNaoto 0:569847dea423 33 analog_lx=ps3_data[2];
DeguNaoto 0:569847dea423 34 analog_ly=ps3_data[3];
DeguNaoto 0:569847dea423 35 analog_rx=ps3_data[4];
DeguNaoto 0:569847dea423 36 analog_ry=ps3_data[5];
DeguNaoto 0:569847dea423 37 if(!circle) edge_circle=1;
DeguNaoto 0:569847dea423 38 if(!triangle) edge_triangle=1;
DeguNaoto 0:569847dea423 39 if(!r1) edge_r1=1;
DeguNaoto 0:569847dea423 40 if(!l1) edge_l1=1;
DeguNaoto 0:569847dea423 41 if(!(analog_ry<40)) edge_r_up=1;
DeguNaoto 0:569847dea423 42 if(!(analog_ry>90)) edge_r_down=1;
DeguNaoto 0:569847dea423 43 if(!(analog_ly<40)) edge_l_up=1;
DeguNaoto 0:569847dea423 44 if(!(analog_ly>90)) edge_l_down=1;
DeguNaoto 0:569847dea423 45 if(!edge_right) edge_right=1;
DeguNaoto 0:569847dea423 46 if(!edge_left) edge_left=1;
DeguNaoto 0:569847dea423 47 }
DeguNaoto 0:569847dea423 48 ///interrupt SBDBT RX.
DeguNaoto 0:569847dea423 49 inline void SBDBT_interrupt()
DeguNaoto 0:569847dea423 50 {
DeguNaoto 0:569847dea423 51 sample=SBDBT.getc();
DeguNaoto 0:569847dea423 52 if(count==7) data_clear();
DeguNaoto 0:569847dea423 53 if(sample==ps3_start) count=0;
DeguNaoto 0:569847dea423 54 else {
DeguNaoto 0:569847dea423 55 ps3_data[count]=sample;
DeguNaoto 0:569847dea423 56 count++;
DeguNaoto 0:569847dea423 57 }
DeguNaoto 0:569847dea423 58 if(count==6) data_check();
DeguNaoto 0:569847dea423 59 }
DeguNaoto 0:569847dea423 60 inline void initializeSBDBT()
DeguNaoto 0:569847dea423 61 {
DeguNaoto 0:569847dea423 62 for(int i=0; i<7; i++) ps3_data[i]=0;
DeguNaoto 0:569847dea423 63 SBDBT.baud(2400);
DeguNaoto 0:569847dea423 64 SBDBT.format(8, Serial::None, 1);
DeguNaoto 0:569847dea423 65 SBDBT.attach(SBDBT_interrupt, Serial::RxIrq);
DeguNaoto 0:569847dea423 66 }