Communicate with IAI Robotnet linear actuators
Dependents: IAILinearActuators
linactbuf.cpp@0:de88dc2515d3, 2014-02-24 (annotated)
- Committer:
- henryeherman
- Date:
- Mon Feb 24 05:54:34 2014 +0000
- Revision:
- 0:de88dc2515d3
Initial commit of IAI code to mbed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
henryeherman | 0:de88dc2515d3 | 1 | #include <linactbuf.h> |
henryeherman | 0:de88dc2515d3 | 2 | |
henryeherman | 0:de88dc2515d3 | 3 | |
henryeherman | 0:de88dc2515d3 | 4 | namespace IAI { |
henryeherman | 0:de88dc2515d3 | 5 | |
henryeherman | 0:de88dc2515d3 | 6 | LinActBuf::LinActBuf() { |
henryeherman | 0:de88dc2515d3 | 7 | reset(); |
henryeherman | 0:de88dc2515d3 | 8 | } |
henryeherman | 0:de88dc2515d3 | 9 | |
henryeherman | 0:de88dc2515d3 | 10 | void LinActBuf::push(unsigned char value) { |
henryeherman | 0:de88dc2515d3 | 11 | len++; |
henryeherman | 0:de88dc2515d3 | 12 | *ptr = value; |
henryeherman | 0:de88dc2515d3 | 13 | ptr++; |
henryeherman | 0:de88dc2515d3 | 14 | } |
henryeherman | 0:de88dc2515d3 | 15 | |
henryeherman | 0:de88dc2515d3 | 16 | void LinActBuf::pushRx(unsigned char value) { |
henryeherman | 0:de88dc2515d3 | 17 | rxlen++; |
henryeherman | 0:de88dc2515d3 | 18 | *rxptr = value; |
henryeherman | 0:de88dc2515d3 | 19 | rxptr++; |
henryeherman | 0:de88dc2515d3 | 20 | } |
henryeherman | 0:de88dc2515d3 | 21 | |
henryeherman | 0:de88dc2515d3 | 22 | void LinActBuf::reset() { |
henryeherman | 0:de88dc2515d3 | 23 | len = 0; |
henryeherman | 0:de88dc2515d3 | 24 | ptr = buf; |
henryeherman | 0:de88dc2515d3 | 25 | buf[0] = GWADDR; |
henryeherman | 0:de88dc2515d3 | 26 | resetRx(); |
henryeherman | 0:de88dc2515d3 | 27 | } |
henryeherman | 0:de88dc2515d3 | 28 | |
henryeherman | 0:de88dc2515d3 | 29 | |
henryeherman | 0:de88dc2515d3 | 30 | void LinActBuf::resetRx() { |
henryeherman | 0:de88dc2515d3 | 31 | rxlen = 0; |
henryeherman | 0:de88dc2515d3 | 32 | exprxlen = 0; |
henryeherman | 0:de88dc2515d3 | 33 | rxptr = rxbuf; |
henryeherman | 0:de88dc2515d3 | 34 | rxbuf[0] = 0; |
henryeherman | 0:de88dc2515d3 | 35 | rxmsgptr = 0; |
henryeherman | 0:de88dc2515d3 | 36 | datalen = 0; |
henryeherman | 0:de88dc2515d3 | 37 | } |
henryeherman | 0:de88dc2515d3 | 38 | |
henryeherman | 0:de88dc2515d3 | 39 | void LinActBuf::copy(LinActBuf *other) { |
henryeherman | 0:de88dc2515d3 | 40 | for(int i=0;i<other->len;i++) { |
henryeherman | 0:de88dc2515d3 | 41 | buf[i] = other->buf[i]; |
henryeherman | 0:de88dc2515d3 | 42 | } |
henryeherman | 0:de88dc2515d3 | 43 | } |
henryeherman | 0:de88dc2515d3 | 44 | |
henryeherman | 0:de88dc2515d3 | 45 | unsigned int LinActBuf::crc_update(unsigned int crc, unsigned char a) { |
henryeherman | 0:de88dc2515d3 | 46 | crc ^= a; |
henryeherman | 0:de88dc2515d3 | 47 | //printf("CRC^=a:0x%x, a=%x\r\n", crc, a); |
henryeherman | 0:de88dc2515d3 | 48 | for(int i=0;i<8;i++) { |
henryeherman | 0:de88dc2515d3 | 49 | if(crc & 1) { |
henryeherman | 0:de88dc2515d3 | 50 | crc = (crc >> 1) ^ 0xA001; |
henryeherman | 0:de88dc2515d3 | 51 | } else { |
henryeherman | 0:de88dc2515d3 | 52 | crc = crc >> 1; |
henryeherman | 0:de88dc2515d3 | 53 | } |
henryeherman | 0:de88dc2515d3 | 54 | } |
henryeherman | 0:de88dc2515d3 | 55 | //printf("return CRC: 0x%x\r\n", crc); |
henryeherman | 0:de88dc2515d3 | 56 | return crc; |
henryeherman | 0:de88dc2515d3 | 57 | } |
henryeherman | 0:de88dc2515d3 | 58 | |
henryeherman | 0:de88dc2515d3 | 59 | void LinActBuf::calc_crc() { |
henryeherman | 0:de88dc2515d3 | 60 | |
henryeherman | 0:de88dc2515d3 | 61 | unsigned short crc = 0xffff; |
henryeherman | 0:de88dc2515d3 | 62 | |
henryeherman | 0:de88dc2515d3 | 63 | for(int i=0;i<len;i++) { |
henryeherman | 0:de88dc2515d3 | 64 | crc = crc_update(crc, buf[i]); |
henryeherman | 0:de88dc2515d3 | 65 | } |
henryeherman | 0:de88dc2515d3 | 66 | |
henryeherman | 0:de88dc2515d3 | 67 | if (len+2 > LINACT_BUFLEN) { |
henryeherman | 0:de88dc2515d3 | 68 | // this is an error so log it |
henryeherman | 0:de88dc2515d3 | 69 | LINBUFERR("tried to write beyond buffer length\n"); |
henryeherman | 0:de88dc2515d3 | 70 | return; |
henryeherman | 0:de88dc2515d3 | 71 | } |
henryeherman | 0:de88dc2515d3 | 72 | //printf("0x%04x\r\n", crc); |
henryeherman | 0:de88dc2515d3 | 73 | buf[len] = (crc & 0xff); |
henryeherman | 0:de88dc2515d3 | 74 | buf[len+1] = ((crc & 0xff00) >> 8); |
henryeherman | 0:de88dc2515d3 | 75 | len=len+2; |
henryeherman | 0:de88dc2515d3 | 76 | } |
henryeherman | 0:de88dc2515d3 | 77 | |
henryeherman | 0:de88dc2515d3 | 78 | char * LinActBuf::as_string() { |
henryeherman | 0:de88dc2515d3 | 79 | strbuf[0]='\0'; |
henryeherman | 0:de88dc2515d3 | 80 | char tmpbuf[4] = ""; |
henryeherman | 0:de88dc2515d3 | 81 | for(int i=0;i<len;i++) { |
henryeherman | 0:de88dc2515d3 | 82 | sprintf(tmpbuf, "%02X", (unsigned char)buf[i]); |
henryeherman | 0:de88dc2515d3 | 83 | |
henryeherman | 0:de88dc2515d3 | 84 | strcat(strbuf,tmpbuf); |
henryeherman | 0:de88dc2515d3 | 85 | //printf("%d:%s\r\n",i, tmpbuf); |
henryeherman | 0:de88dc2515d3 | 86 | } |
henryeherman | 0:de88dc2515d3 | 87 | return strbuf; |
henryeherman | 0:de88dc2515d3 | 88 | } |
henryeherman | 0:de88dc2515d3 | 89 | |
henryeherman | 0:de88dc2515d3 | 90 | char * LinActBuf::rx_as_string() { |
henryeherman | 0:de88dc2515d3 | 91 | rxstrbuf[0]='\0'; |
henryeherman | 0:de88dc2515d3 | 92 | char tmpbuf[4] = ""; |
henryeherman | 0:de88dc2515d3 | 93 | for(int i=0;i<len;i++) { |
henryeherman | 0:de88dc2515d3 | 94 | sprintf(tmpbuf, "%02X", (unsigned char)buf[i]); |
henryeherman | 0:de88dc2515d3 | 95 | |
henryeherman | 0:de88dc2515d3 | 96 | strcat(rxstrbuf,tmpbuf); |
henryeherman | 0:de88dc2515d3 | 97 | //printf("%d:%s\r\n",i, tmpbuf); |
henryeherman | 0:de88dc2515d3 | 98 | } |
henryeherman | 0:de88dc2515d3 | 99 | return rxstrbuf; |
henryeherman | 0:de88dc2515d3 | 100 | } |
henryeherman | 0:de88dc2515d3 | 101 | |
henryeherman | 0:de88dc2515d3 | 102 | void LinActBuf::readRegsisterStr(unsigned short startreg, unsigned short count) { |
henryeherman | 0:de88dc2515d3 | 103 | buf[GWADDRPOS] = GWADDR; |
henryeherman | 0:de88dc2515d3 | 104 | buf[CMDPOS] = LINACT_READ_MULTI; |
henryeherman | 0:de88dc2515d3 | 105 | buf[2] = (unsigned char)((startreg & 0xFF00) >> 8); |
henryeherman | 0:de88dc2515d3 | 106 | buf[3] = (unsigned char)(startreg & 0x00FF); |
henryeherman | 0:de88dc2515d3 | 107 | buf[4] = (unsigned char)((count & 0xFF00) >> 8); |
henryeherman | 0:de88dc2515d3 | 108 | buf[5] = (unsigned char)(count & 0x00FF); |
henryeherman | 0:de88dc2515d3 | 109 | len = 6; |
henryeherman | 0:de88dc2515d3 | 110 | calc_crc(); |
henryeherman | 0:de88dc2515d3 | 111 | |
henryeherman | 0:de88dc2515d3 | 112 | // See page 151 |
henryeherman | 0:de88dc2515d3 | 113 | // Header + crc + 2*numreg |
henryeherman | 0:de88dc2515d3 | 114 | datalen = 2 * count; |
henryeherman | 0:de88dc2515d3 | 115 | exprxlen = 5 + datalen; // Len in bytes |
henryeherman | 0:de88dc2515d3 | 116 | |
henryeherman | 0:de88dc2515d3 | 117 | rxmsgptr = rxbuf + 3; |
henryeherman | 0:de88dc2515d3 | 118 | } |
henryeherman | 0:de88dc2515d3 | 119 | |
henryeherman | 0:de88dc2515d3 | 120 | void LinActBuf::writeRegisterStr(unsigned short reg, unsigned short value) { |
henryeherman | 0:de88dc2515d3 | 121 | buf[GWADDRPOS] = GWADDR; |
henryeherman | 0:de88dc2515d3 | 122 | buf[CMDPOS] = LINACT_WRITE; |
henryeherman | 0:de88dc2515d3 | 123 | buf[2] = (unsigned char)((reg & 0xFF00) >> 8); |
henryeherman | 0:de88dc2515d3 | 124 | buf[3] = (unsigned char)(reg & 0x00FF); |
henryeherman | 0:de88dc2515d3 | 125 | buf[4] = (unsigned char)((value & 0xFF00) >>8); |
henryeherman | 0:de88dc2515d3 | 126 | buf[5] = (unsigned char)(value & 0x00FF); |
henryeherman | 0:de88dc2515d3 | 127 | len = 6; |
henryeherman | 0:de88dc2515d3 | 128 | calc_crc(); |
henryeherman | 0:de88dc2515d3 | 129 | rxmsgptr = 0; |
henryeherman | 0:de88dc2515d3 | 130 | // See Page 163 for expected len |
henryeherman | 0:de88dc2515d3 | 131 | exprxlen = 8; //Len in bytes |
henryeherman | 0:de88dc2515d3 | 132 | } |
henryeherman | 0:de88dc2515d3 | 133 | |
henryeherman | 0:de88dc2515d3 | 134 | void LinActBuf::writeMultiRegisterStr(unsigned short reg, |
henryeherman | 0:de88dc2515d3 | 135 | unsigned short reglen, unsigned char * data, unsigned char dlen) { |
henryeherman | 0:de88dc2515d3 | 136 | |
henryeherman | 0:de88dc2515d3 | 137 | buf[GWADDRPOS] = GWADDR; |
henryeherman | 0:de88dc2515d3 | 138 | buf[CMDPOS] = LINACT_WRITE_MULTI; |
henryeherman | 0:de88dc2515d3 | 139 | buf[2] = (unsigned char)((reg & 0xFF00) >> 8); |
henryeherman | 0:de88dc2515d3 | 140 | buf[3] = (unsigned char)(reg & 0x00FF); |
henryeherman | 0:de88dc2515d3 | 141 | buf[4] = (unsigned char)((reglen & 0xFF00) >> 8); |
henryeherman | 0:de88dc2515d3 | 142 | buf[5] = (unsigned char)(reglen & 0x00FF); |
henryeherman | 0:de88dc2515d3 | 143 | buf[6] = dlen; |
henryeherman | 0:de88dc2515d3 | 144 | |
henryeherman | 0:de88dc2515d3 | 145 | |
henryeherman | 0:de88dc2515d3 | 146 | for(int i=0;i<dlen;i++) { |
henryeherman | 0:de88dc2515d3 | 147 | buf[7+i] = data[i]; |
henryeherman | 0:de88dc2515d3 | 148 | } |
henryeherman | 0:de88dc2515d3 | 149 | len = dlen+7; |
henryeherman | 0:de88dc2515d3 | 150 | calc_crc(); |
henryeherman | 0:de88dc2515d3 | 151 | |
henryeherman | 0:de88dc2515d3 | 152 | // See page 180 |
henryeherman | 0:de88dc2515d3 | 153 | exprxlen = 8; // Len in bytes |
henryeherman | 0:de88dc2515d3 | 154 | rxmsgptr = 0; |
henryeherman | 0:de88dc2515d3 | 155 | } |
henryeherman | 0:de88dc2515d3 | 156 | |
henryeherman | 0:de88dc2515d3 | 157 | |
henryeherman | 0:de88dc2515d3 | 158 | unsigned char * LinActBuf::rxdata() { |
henryeherman | 0:de88dc2515d3 | 159 | return rxmsgptr; |
henryeherman | 0:de88dc2515d3 | 160 | } |
henryeherman | 0:de88dc2515d3 | 161 | |
henryeherman | 0:de88dc2515d3 | 162 | unsigned int LinActBuf::rxdatalen() { |
henryeherman | 0:de88dc2515d3 | 163 | return datalen; |
henryeherman | 0:de88dc2515d3 | 164 | } |
henryeherman | 0:de88dc2515d3 | 165 | |
henryeherman | 0:de88dc2515d3 | 166 | // Check the checksum |
henryeherman | 0:de88dc2515d3 | 167 | int LinActBuf::rxvalidate() { |
henryeherman | 0:de88dc2515d3 | 168 | |
henryeherman | 0:de88dc2515d3 | 169 | if(rxlen != exprxlen) |
henryeherman | 0:de88dc2515d3 | 170 | return RXMSGLENERR; |
henryeherman | 0:de88dc2515d3 | 171 | |
henryeherman | 0:de88dc2515d3 | 172 | switch(buf[CMDPOS]) { |
henryeherman | 0:de88dc2515d3 | 173 | case LINACT_WRITE: |
henryeherman | 0:de88dc2515d3 | 174 | if((buf[len-1] == rxbuf[rxlen-1]) && |
henryeherman | 0:de88dc2515d3 | 175 | (buf[len-2] == rxbuf[rxlen-2])) |
henryeherman | 0:de88dc2515d3 | 176 | return RXWRCRCOK; |
henryeherman | 0:de88dc2515d3 | 177 | case LINACT_WRITE_MULTI: |
henryeherman | 0:de88dc2515d3 | 178 | if(checkrxcrc()) |
henryeherman | 0:de88dc2515d3 | 179 | return RXMULWRCRCOK; |
henryeherman | 0:de88dc2515d3 | 180 | case LINACT_READ_MULTI: |
henryeherman | 0:de88dc2515d3 | 181 | if(checkrxcrc()) |
henryeherman | 0:de88dc2515d3 | 182 | return RXREADCRCOK; |
henryeherman | 0:de88dc2515d3 | 183 | default: |
henryeherman | 0:de88dc2515d3 | 184 | break; |
henryeherman | 0:de88dc2515d3 | 185 | } |
henryeherman | 0:de88dc2515d3 | 186 | return RXINVALIDCRCERR; |
henryeherman | 0:de88dc2515d3 | 187 | } |
henryeherman | 0:de88dc2515d3 | 188 | |
henryeherman | 0:de88dc2515d3 | 189 | int LinActBuf::checkrxcrc() { |
henryeherman | 0:de88dc2515d3 | 190 | unsigned short crc = 0xffff; |
henryeherman | 0:de88dc2515d3 | 191 | |
henryeherman | 0:de88dc2515d3 | 192 | if (rxlen < 2) |
henryeherman | 0:de88dc2515d3 | 193 | return 0; |
henryeherman | 0:de88dc2515d3 | 194 | |
henryeherman | 0:de88dc2515d3 | 195 | // Check all bytes but last 2 |
henryeherman | 0:de88dc2515d3 | 196 | for(int i=0;i<(rxlen-2);i++) { |
henryeherman | 0:de88dc2515d3 | 197 | crc = crc_update(crc, rxbuf[i]); |
henryeherman | 0:de88dc2515d3 | 198 | } |
henryeherman | 0:de88dc2515d3 | 199 | |
henryeherman | 0:de88dc2515d3 | 200 | if(rxbuf[rxlen-2] == (crc & 0xff) && |
henryeherman | 0:de88dc2515d3 | 201 | rxbuf[rxlen-1] == ((crc & 0xff00) >> 8)) |
henryeherman | 0:de88dc2515d3 | 202 | return 1; |
henryeherman | 0:de88dc2515d3 | 203 | LINBUFDBG("Expected CRC 0x%04X\r\n", crc); |
henryeherman | 0:de88dc2515d3 | 204 | return 0; |
henryeherman | 0:de88dc2515d3 | 205 | } |
henryeherman | 0:de88dc2515d3 | 206 | |
henryeherman | 0:de88dc2515d3 | 207 | |
henryeherman | 0:de88dc2515d3 | 208 | } // End IAI Namespace |
henryeherman | 0:de88dc2515d3 | 209 |