2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   PID QEI mbed

Fork of 2015robot_main by Naoto Deguchi

communicate.h@59:9d66edf3e734, 2015-10-01 (annotated)

Committer:
DeguNaoto
Date:
Thu Oct 01 09:52:36 2015 +0000
Revision:
59:9d66edf3e734
Parent:
57:3fbd487e055e
Child:
67:658a54be9212
Child:
68:2b2b88ecdcce
RS485 ????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:bd4719e15f7e 1 /***RS485 Communication.***/
DeguNaoto 18:526124eef0d1 2 #define RS485_TX p13
DeguNaoto 18:526124eef0d1 3 #define RS485_RX p14
DeguNaoto 0:bd4719e15f7e 4 Serial RS485(RS485_TX, RS485_RX);
DeguNaoto 26:8e6c736b6791 5 #define EnablePin p7
DeguNaoto 26:8e6c736b6791 6 DigitalOut Enable(EnablePin);
DeguNaoto 59:9d66edf3e734 7 inline void initializeRS485(){
DeguNaoto 59:9d66edf3e734 8 RS485.format(8,Serial::Even);
DeguNaoto 59:9d66edf3e734 9 }
DeguNaoto 4:51d87d2b698c 10 inline void sendData(int address, int data5) {
DeguNaoto 4:51d87d2b698c 11 unsigned int data;
DeguNaoto 4:51d87d2b698c 12 if (data5 <= 31) {
DeguNaoto 4:51d87d2b698c 13 data = ((address << 5) | data5);
DeguNaoto 4:51d87d2b698c 14 RS485.putc(data);
DeguNaoto 4:51d87d2b698c 15 }
DeguNaoto 4:51d87d2b698c 16 }
DeguNaoto 0:bd4719e15f7e 17
DeguNaoto 57:3fbd487e055e 18 #ifdef IM920
DeguNaoto 44:315d5960f18c 19 /***IM920 Communication.***/
DeguNaoto 51:cb430192b28b 20 char rcvBUFF_IM920[40], rcvBUFF_SBDBT[40];
DeguNaoto 44:315d5960f18c 21 int rcvCOUNT_IM920 = 0, rcvCOUNT_SBDBT = 0;
DeguNaoto 44:315d5960f18c 22 int rcvFLAG = 0;
DeguNaoto 44:315d5960f18c 23 int sendFLAG = 0;
DeguNaoto 44:315d5960f18c 24 int deadCOUNT = 0;
DeguNaoto 51:cb430192b28b 25 int deadflag = 0;
DeguNaoto 44:315d5960f18c 26 int oldDATA;
DeguNaoto 44:315d5960f18c 27 unsigned int cm, a2, b, X, Y;
DeguNaoto 51:cb430192b28b 28 unsigned int sendDATA = 0;
DeguNaoto 44:315d5960f18c 29 int LED = 0;
DeguNaoto 51:cb430192b28b 30 unsigned long outDATA[4];
DeguNaoto 44:315d5960f18c 31
DeguNaoto 44:315d5960f18c 32 Serial IM920_USART(p9, p10);
DeguNaoto 51:cb430192b28b 33 Serial SBDBT_USART(p28, p27);
DeguNaoto 44:315d5960f18c 34 DigitalIn BUSY(p8);
DeguNaoto 44:315d5960f18c 35
DeguNaoto 44:315d5960f18c 36 Ticker DTimer;
DeguNaoto 44:315d5960f18c 37
DeguNaoto 44:315d5960f18c 38 void DEVICE_TX(int DATA, int DEVICE = 0) {
DeguNaoto 44:315d5960f18c 39 char sDATA[2];
DeguNaoto 44:315d5960f18c 40 if (DATA <= 15)
DeguNaoto 44:315d5960f18c 41 sprintf(sDATA, "0%X", DATA);
DeguNaoto 44:315d5960f18c 42 else
DeguNaoto 44:315d5960f18c 43 sprintf(sDATA, "%X", DATA);
DeguNaoto 44:315d5960f18c 44
DeguNaoto 44:315d5960f18c 45 if (DEVICE == 1) {
DeguNaoto 44:315d5960f18c 46 IM920_USART.printf("TXDA %s\r\n", sDATA);
DeguNaoto 44:315d5960f18c 47 }
DeguNaoto 44:315d5960f18c 48 else if (DEVICE == 2) {
DeguNaoto 44:315d5960f18c 49 SBDBT_USART.printf("00000000000%s\r\n", sDATA);
DeguNaoto 44:315d5960f18c 50 }
DeguNaoto 44:315d5960f18c 51 else {
DeguNaoto 44:315d5960f18c 52 IM920_USART.printf("TXDA %s\r\n", sDATA);
DeguNaoto 44:315d5960f18c 53 SBDBT_USART.printf("00000000000%s\r\n", sDATA);
DeguNaoto 44:315d5960f18c 54 }
DeguNaoto 44:315d5960f18c 55 }
DeguNaoto 44:315d5960f18c 56
DeguNaoto 51:cb430192b28b 57 int cvsendDATA(int Buzzer, int LED) {
DeguNaoto 44:315d5960f18c 58 if (Buzzer > 1)
DeguNaoto 44:315d5960f18c 59 Buzzer = 1;
DeguNaoto 44:315d5960f18c 60 else if (Buzzer < 0)
DeguNaoto 44:315d5960f18c 61 Buzzer = 0;
DeguNaoto 44:315d5960f18c 62 if (LED < 0)
DeguNaoto 44:315d5960f18c 63 LED = 0;
DeguNaoto 44:315d5960f18c 64 else if (LED >= 16)
DeguNaoto 44:315d5960f18c 65 LED = 15;
DeguNaoto 44:315d5960f18c 66 return (Buzzer << 6) | (LED << 2);
DeguNaoto 44:315d5960f18c 67 }
DeguNaoto 44:315d5960f18c 68
DeguNaoto 44:315d5960f18c 69 void cvrecvDATA(char *buffDATA, unsigned long *outputDATA) {
DeguNaoto 44:315d5960f18c 70 char s1[2], s2[2], s3[2], s4[2];
DeguNaoto 44:315d5960f18c 71 strncpy(s1, buffDATA+11, 2);
DeguNaoto 44:315d5960f18c 72 strncpy(s2, buffDATA+14, 2);
DeguNaoto 44:315d5960f18c 73 strncpy(s3, buffDATA+17, 2);
DeguNaoto 44:315d5960f18c 74 strncpy(s4, buffDATA+20, 2);
DeguNaoto 44:315d5960f18c 75 outputDATA[0] = strtoul(s1, (char **) NULL, 16);
DeguNaoto 44:315d5960f18c 76 outputDATA[1] = strtoul(s2, (char **) NULL, 16);
DeguNaoto 44:315d5960f18c 77 outputDATA[2] = strtoul(s3, (char **) NULL, 16);
DeguNaoto 44:315d5960f18c 78 outputDATA[3] = strtoul(s4, (char **) NULL, 16);
DeguNaoto 44:315d5960f18c 79 }
DeguNaoto 44:315d5960f18c 80
DeguNaoto 44:315d5960f18c 81 void cvrecvDATAsbdbt(char *buffDATA, unsigned long *outputDATA) {
DeguNaoto 44:315d5960f18c 82 char s1[2], s2[2];
DeguNaoto 44:315d5960f18c 83 strncpy(s1, buffDATA+5, 2);
DeguNaoto 44:315d5960f18c 84 strncpy(s2, buffDATA+7, 2);
DeguNaoto 44:315d5960f18c 85 outputDATA[0] = strtoul(s1, (char **) NULL, 16);
DeguNaoto 44:315d5960f18c 86 outputDATA[1] = strtoul(s2, (char **) NULL, 16);
DeguNaoto 44:315d5960f18c 87 }
DeguNaoto 44:315d5960f18c 88
DeguNaoto 44:315d5960f18c 89 void readDATA(unsigned long* outDATA) {
DeguNaoto 44:315d5960f18c 90 cm = (outDATA[0] & 128) >> 7;
DeguNaoto 44:315d5960f18c 91 a2 = (outDATA[0] & 64) >> 6;
DeguNaoto 44:315d5960f18c 92 X = (outDATA[0] & 56) >> 3;
DeguNaoto 44:315d5960f18c 93 Y = outDATA[0] & 7;
DeguNaoto 44:315d5960f18c 94 b = outDATA[1];
DeguNaoto 44:315d5960f18c 95 }
DeguNaoto 44:315d5960f18c 96
DeguNaoto 44:315d5960f18c 97 void SBDBT_RX() {
DeguNaoto 44:315d5960f18c 98 char rcvDATA;
DeguNaoto 44:315d5960f18c 99 rcvDATA = SBDBT_USART.getc();
DeguNaoto 44:315d5960f18c 100 rcvBUFF_SBDBT[rcvCOUNT_SBDBT] = rcvDATA;
DeguNaoto 44:315d5960f18c 101 rcvCOUNT_SBDBT++;
DeguNaoto 44:315d5960f18c 102 if (rcvDATA == 0x0A) {
DeguNaoto 44:315d5960f18c 103 rcvCOUNT_SBDBT = 0;
DeguNaoto 44:315d5960f18c 104 rcvFLAG = 2;
DeguNaoto 44:315d5960f18c 105 }
DeguNaoto 44:315d5960f18c 106 else if (rcvCOUNT_SBDBT >= 40) {
DeguNaoto 44:315d5960f18c 107 rcvCOUNT_SBDBT = 0;
DeguNaoto 44:315d5960f18c 108 memset(rcvBUFF_SBDBT, '\0', 40);
DeguNaoto 44:315d5960f18c 109 }
DeguNaoto 44:315d5960f18c 110 }
DeguNaoto 44:315d5960f18c 111
DeguNaoto 44:315d5960f18c 112 void IM920_RX() {
DeguNaoto 44:315d5960f18c 113 char rcvDATA;
DeguNaoto 44:315d5960f18c 114 rcvDATA = IM920_USART.getc();
DeguNaoto 44:315d5960f18c 115 rcvBUFF_IM920[rcvCOUNT_IM920] = rcvDATA;
DeguNaoto 44:315d5960f18c 116 rcvCOUNT_IM920++;
DeguNaoto 44:315d5960f18c 117 if (rcvDATA == 0x0A) {
DeguNaoto 44:315d5960f18c 118 rcvCOUNT_IM920 = 0;
DeguNaoto 44:315d5960f18c 119 rcvFLAG = 1;
DeguNaoto 44:315d5960f18c 120 }
DeguNaoto 44:315d5960f18c 121 else if (rcvCOUNT_IM920 >= 40) {
DeguNaoto 44:315d5960f18c 122 rcvCOUNT_IM920 = 0;
DeguNaoto 44:315d5960f18c 123 memset(rcvBUFF_IM920, '\0', 40);
DeguNaoto 44:315d5960f18c 124 }
DeguNaoto 44:315d5960f18c 125 }
DeguNaoto 44:315d5960f18c 126
DeguNaoto 44:315d5960f18c 127 void deadCheck() {
DeguNaoto 44:315d5960f18c 128 if (rcvFLAG == 0 && deadCOUNT >= 5) {
DeguNaoto 51:cb430192b28b 129 deadflag = 1;
DeguNaoto 44:315d5960f18c 130 deadCOUNT = 0;
DeguNaoto 44:315d5960f18c 131 }
DeguNaoto 44:315d5960f18c 132 else if (rcvFLAG == 0 && deadCOUNT >= 2) {
DeguNaoto 44:315d5960f18c 133 deadCOUNT++;
DeguNaoto 51:cb430192b28b 134 DEVICE_TX(sendDATA);
DeguNaoto 44:315d5960f18c 135 }
DeguNaoto 44:315d5960f18c 136 else if (rcvFLAG == 0) {
DeguNaoto 44:315d5960f18c 137 deadCOUNT++;
DeguNaoto 44:315d5960f18c 138 }
DeguNaoto 51:cb430192b28b 139 sendDATA = cvsendDATA(0, LED);
DeguNaoto 44:315d5960f18c 140 }
DeguNaoto 44:315d5960f18c 141
DeguNaoto 44:315d5960f18c 142 inline void initializeIM920(){
DeguNaoto 44:315d5960f18c 143 IM920_USART.baud(9600);
DeguNaoto 44:315d5960f18c 144 IM920_USART.format(8, Serial::None, 1);
DeguNaoto 44:315d5960f18c 145 IM920_USART.attach(IM920_RX, Serial::RxIrq);
DeguNaoto 44:315d5960f18c 146 SBDBT_USART.baud(9600);
DeguNaoto 44:315d5960f18c 147 SBDBT_USART.format(8, Serial::None, 1);
DeguNaoto 44:315d5960f18c 148 SBDBT_USART.attach(SBDBT_RX, Serial::RxIrq);
DeguNaoto 44:315d5960f18c 149 DTimer.attach(deadCheck, 0.3);
DeguNaoto 51:cb430192b28b 150 sendDATA = cvsendDATA(0, 0);
DeguNaoto 44:315d5960f18c 151 }
DeguNaoto 44:315d5960f18c 152
DeguNaoto 44:315d5960f18c 153 inline void readIM920(){
DeguNaoto 44:315d5960f18c 154 if (rcvFLAG == 1) {
DeguNaoto 44:315d5960f18c 155 if (strlen(rcvBUFF_IM920) > 8) {
DeguNaoto 44:315d5960f18c 156 cvrecvDATA(rcvBUFF_IM920, outDATA);
DeguNaoto 44:315d5960f18c 157 readDATA(outDATA);
DeguNaoto 44:315d5960f18c 158 deadCOUNT = 0;
DeguNaoto 51:cb430192b28b 159 deadflag = 0;
DeguNaoto 51:cb430192b28b 160 DEVICE_TX(sendDATA, 1);
DeguNaoto 44:315d5960f18c 161 }
DeguNaoto 44:315d5960f18c 162 memset(rcvBUFF_IM920, '\0', 40);
DeguNaoto 44:315d5960f18c 163 rcvFLAG = 0;
DeguNaoto 44:315d5960f18c 164 }
DeguNaoto 44:315d5960f18c 165 else if (rcvFLAG == 2) {
DeguNaoto 44:315d5960f18c 166 if (strlen(rcvBUFF_SBDBT) > 4) {
DeguNaoto 44:315d5960f18c 167 cvrecvDATAsbdbt(rcvBUFF_SBDBT, outDATA);
DeguNaoto 44:315d5960f18c 168 readDATA(outDATA);
DeguNaoto 44:315d5960f18c 169 deadCOUNT = 0;
DeguNaoto 51:cb430192b28b 170 deadflag = 0;
DeguNaoto 51:cb430192b28b 171 DEVICE_TX(sendDATA, 2);
DeguNaoto 44:315d5960f18c 172 }
DeguNaoto 44:315d5960f18c 173 memset(rcvBUFF_SBDBT, '\0', 40);
DeguNaoto 44:315d5960f18c 174 rcvFLAG = 0;
DeguNaoto 44:315d5960f18c 175 }
DeguNaoto 51:cb430192b28b 176 }
DeguNaoto 57:3fbd487e055e 177 #else
DeguNaoto 0:bd4719e15f7e 178 /***Get state ps3con.***/
DeguNaoto 57:3fbd487e055e 179 #define SBDBT_TX p28
DeguNaoto 18:526124eef0d1 180 #define SBDBT_RX p27
DeguNaoto 0:bd4719e15f7e 181 Serial SBDBT(SBDBT_TX, SBDBT_RX);
DeguNaoto 0:bd4719e15f7e 182 #define ps3_start 0x80
DeguNaoto 18:526124eef0d1 183 short toggle = 0;
DeguNaoto 18:526124eef0d1 184 short edge_circle = 0;
DeguNaoto 18:526124eef0d1 185 short edge_triangle = 0;
DeguNaoto 21:79b94cb922f0 186 short edge_square = 0;
DeguNaoto 18:526124eef0d1 187 short edge_r1 = 0;
DeguNaoto 18:526124eef0d1 188 short edge_l1 = 0;
DeguNaoto 57:3fbd487e055e 189 short edge_r2 = 0;
DeguNaoto 57:3fbd487e055e 190 short edge_l2 = 0;
DeguNaoto 18:526124eef0d1 191 short edge_l_up = 0;
DeguNaoto 18:526124eef0d1 192 short edge_l_down = 0;
DeguNaoto 18:526124eef0d1 193 short edge_r_up = 0;
DeguNaoto 18:526124eef0d1 194 short edge_r_down = 0;
DeguNaoto 18:526124eef0d1 195 short edge_right = 0;
DeguNaoto 18:526124eef0d1 196 short edge_left = 0;
DeguNaoto 18:526124eef0d1 197 short edge_up = 0;
DeguNaoto 18:526124eef0d1 198 short square = 0;
DeguNaoto 18:526124eef0d1 199 short triangle = 0;
DeguNaoto 18:526124eef0d1 200 short circle = 0;
DeguNaoto 18:526124eef0d1 201 short cross = 0;
DeguNaoto 18:526124eef0d1 202 short up = 0;
DeguNaoto 18:526124eef0d1 203 short down = 0;
DeguNaoto 18:526124eef0d1 204 short right = 0;
DeguNaoto 18:526124eef0d1 205 short left = 0;
DeguNaoto 18:526124eef0d1 206 short l1 = 0;
DeguNaoto 18:526124eef0d1 207 short l2 = 0;
DeguNaoto 18:526124eef0d1 208 short r1 = 0;
DeguNaoto 18:526124eef0d1 209 short r2 = 0;
DeguNaoto 18:526124eef0d1 210 signed int analog_lx = 0;
DeguNaoto 18:526124eef0d1 211 signed int analog_ly = 0;
DeguNaoto 18:526124eef0d1 212 signed int analog_rx = 0;
DeguNaoto 18:526124eef0d1 213 signed int analog_ry = 0;
DeguNaoto 18:526124eef0d1 214 int count = 0;
DeguNaoto 18:526124eef0d1 215 int sample = 0;
DeguNaoto 0:bd4719e15f7e 216 int ps3_data[7];
DeguNaoto 18:526124eef0d1 217 inline void data_clear();
DeguNaoto 18:526124eef0d1 218 inline void data_check();
DeguNaoto 18:526124eef0d1 219 inline void SBDBT_interrupt();
DeguNaoto 18:526124eef0d1 220 inline void initializeSBDBT();
DeguNaoto 18:526124eef0d1 221
DeguNaoto 2:cf8ca6742db9 222 inline void data_clear()
DeguNaoto 2:cf8ca6742db9 223 {
DeguNaoto 2:cf8ca6742db9 224 for(int i=0; i<7; i++) ps3_data[i]=0;
DeguNaoto 0:bd4719e15f7e 225 }
DeguNaoto 2:cf8ca6742db9 226 inline void data_check()
DeguNaoto 2:cf8ca6742db9 227 {
DeguNaoto 0:bd4719e15f7e 228 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:bd4719e15f7e 229 analog_lx=0,analog_ly=0,analog_rx=0,analog_ry=0;
DeguNaoto 0:bd4719e15f7e 230 if((ps3_data[0]==0x00)&&(ps3_data[1]==0x01)) up=1;
DeguNaoto 0:bd4719e15f7e 231 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x02)) down=1;
DeguNaoto 0:bd4719e15f7e 232 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x04)) right=1;
DeguNaoto 0:bd4719e15f7e 233 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x08)) left=1;
DeguNaoto 0:bd4719e15f7e 234 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x10)) triangle=1;
DeguNaoto 0:bd4719e15f7e 235 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x20)) cross=1;
DeguNaoto 0:bd4719e15f7e 236 else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x40)) circle=1;
DeguNaoto 0:bd4719e15f7e 237 else if((ps3_data[0]==0x01)&&(ps3_data[1]==0x00)) square=1;
DeguNaoto 0:bd4719e15f7e 238 else if((ps3_data[0]==0x02)&&(ps3_data[1]==0x00)) l1=1;
DeguNaoto 0:bd4719e15f7e 239 else if((ps3_data[0]==0x04)&&(ps3_data[1]==0x00)) l2=1;
DeguNaoto 0:bd4719e15f7e 240 else if((ps3_data[0]==0x08)&&(ps3_data[1]==0x00)) r1=1;
DeguNaoto 0:bd4719e15f7e 241 else if((ps3_data[0]==0x10)&&(ps3_data[1]==0x00)) r2=1;
DeguNaoto 8:1638e8c8ca93 242 analog_lx=64-(signed int)ps3_data[2];
DeguNaoto 8:1638e8c8ca93 243 analog_ly=64-(signed int)ps3_data[3];
DeguNaoto 8:1638e8c8ca93 244 analog_rx=64-(signed int)ps3_data[4];
DeguNaoto 8:1638e8c8ca93 245 analog_ry=64-(signed int)ps3_data[5];
DeguNaoto 4:51d87d2b698c 246 if(!circle) edge_circle=1;
DeguNaoto 4:51d87d2b698c 247 if(!triangle) edge_triangle=1;
DeguNaoto 4:51d87d2b698c 248 if(!r1) edge_r1=1;
DeguNaoto 4:51d87d2b698c 249 if(!l1) edge_l1=1;
DeguNaoto 57:3fbd487e055e 250 if(!r2) edge_r2=1;
DeguNaoto 57:3fbd487e055e 251 if(!l2) edge_l2=1;
DeguNaoto 25:e591b9fa5796 252 if(!right) edge_right=1;
DeguNaoto 25:e591b9fa5796 253 if(!left) edge_left=1;
DeguNaoto 25:e591b9fa5796 254 if(!up) edge_up=1;
DeguNaoto 25:e591b9fa5796 255 if(!square) edge_square=1;
DeguNaoto 0:bd4719e15f7e 256 }
DeguNaoto 0:bd4719e15f7e 257 ///interrupt SBDBT RX.
DeguNaoto 2:cf8ca6742db9 258 inline void SBDBT_interrupt()
DeguNaoto 2:cf8ca6742db9 259 {
DeguNaoto 0:bd4719e15f7e 260 sample=SBDBT.getc();
DeguNaoto 0:bd4719e15f7e 261 if(count==7) data_clear();
DeguNaoto 0:bd4719e15f7e 262 if(sample==ps3_start) count=0;
DeguNaoto 2:cf8ca6742db9 263 else {
DeguNaoto 0:bd4719e15f7e 264 ps3_data[count]=sample;
DeguNaoto 0:bd4719e15f7e 265 count++;
DeguNaoto 0:bd4719e15f7e 266 }
DeguNaoto 0:bd4719e15f7e 267 if(count==6) data_check();
DeguNaoto 0:bd4719e15f7e 268 }
DeguNaoto 2:cf8ca6742db9 269 inline void initializeSBDBT()
DeguNaoto 2:cf8ca6742db9 270 {
DeguNaoto 22:3996c3f41922 271 wait(0.1);
DeguNaoto 22:3996c3f41922 272 while(analog_ly==-64)
DeguNaoto 2:cf8ca6742db9 273 for(int i=0; i<7; i++) ps3_data[i]=0;
DeguNaoto 0:bd4719e15f7e 274 SBDBT.baud(2400);
DeguNaoto 0:bd4719e15f7e 275 SBDBT.format(8, Serial::None, 1);
DeguNaoto 0:bd4719e15f7e 276 SBDBT.attach(SBDBT_interrupt, Serial::RxIrq);
DeguNaoto 57:3fbd487e055e 277 }
DeguNaoto 57:3fbd487e055e 278 #endif