Henry Herman / linact

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?

UserRevisionLine numberNew 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