xx
Diff: RoboClaw.cpp
- Revision:
- 3:5c6a9045c8f7
- Parent:
- 2:a2c141d922b3
- Child:
- 4:e64524a61cfe
--- a/RoboClaw.cpp Tue Nov 24 21:41:45 2015 +0000 +++ b/RoboClaw.cpp Tue Nov 24 21:56:10 2015 +0000 @@ -5,22 +5,18 @@ #define SetDWORDval(arg) (uint8_t)(arg>>24),(uint8_t)(arg>>16),(uint8_t)(arg>>8),(uint8_t)arg #define SetWORDval(arg) (uint8_t)(arg>>8),(uint8_t)arg -RoboClaw::RoboClaw(int baudrate, PinName rx, PinName tx) : _roboclaw(rx, tx) -{ +RoboClaw::RoboClaw(int baudrate, PinName rx, PinName tx) : _roboclaw(rx, tx){ _roboclaw.baud(baudrate); } -void RoboClaw::crc_clear() -{ +void RoboClaw::crc_clear(){ crc = 0; } -void RoboClaw::crc_update (uint8_t data) -{ +void RoboClaw::crc_update (uint8_t data){ int i; crc = crc ^ ((uint16_t)data << 8); - for (i=0; i<8; i++) - { + for (i=0; i<8; i++) { if (crc & 0x8000) crc = (crc << 1) ^ 0x1021; else @@ -28,20 +24,17 @@ } } -uint16_t RoboClaw::crc_get() -{ +uint16_t RoboClaw::crc_get(){ return crc; } -void RoboClaw::write_n(uint8_t cnt, ... ) -{ +void RoboClaw::write_n(uint8_t cnt, ... ){ uint8_t retry = MAXTRY; do { crc_clear(); va_list marker; va_start( marker, cnt ); - for(uint8_t index=0;index<cnt;index++) - { + for(uint8_t index=0; index<cnt; index++) { uint8_t data = va_arg(marker, unsigned int); crc_update(data); _roboclaw.putc(data); @@ -53,22 +46,17 @@ } while(retry--); } -void RoboClaw::write_(uint8_t address, uint8_t command, uint8_t data, bool reading, bool crcon) -{ +void RoboClaw::write_(uint8_t address, uint8_t command, uint8_t data, bool reading, bool crcon){ _roboclaw.putc(address); _roboclaw.putc(command); - - if(reading == false) - { - if(crcon == true) - { + + if(reading == false) { + if(crcon == true) { uint8_t packet[2] = {address, command}; uint16_t checksum = crc16(packet,2); _roboclaw.putc(checksum>>8); _roboclaw.putc(checksum); - } - else - { + } else { uint8_t packet[3] = {address, command, data}; uint16_t checksum = crc16(packet,3); _roboclaw.putc(data); @@ -78,7 +66,7 @@ } } -uint16_t RoboClaw::crc16(uint8_t *packet, int nBytes) { +uint16_t RoboClaw::crc16(uint8_t *packet, int nBytes){ uint16_t crc_; for (int byte = 0; byte < nBytes; byte++) { crc_ = crc_ ^ ((uint16_t)packet[byte] << 8); @@ -93,8 +81,7 @@ return crc_; } -uint8_t RoboClaw::read_(void) -{ +uint8_t RoboClaw::read_(void){ return(_roboclaw.getc()); } @@ -138,12 +125,12 @@ read_byte[4] = (uint16_t)_roboclaw.getc(); read_byte[5] = (uint16_t)_roboclaw.getc(); read_byte[6] = (uint16_t)_roboclaw.getc(); - + enc1 = read_byte[1]<<24; enc1 |= read_byte[2]<<16; enc1 |= read_byte[3]<<8; enc1 |= read_byte[4]; - + return enc1; } @@ -159,12 +146,12 @@ read_byte2[4] = (uint16_t)_roboclaw.getc(); read_byte2[5] = (uint16_t)_roboclaw.getc(); read_byte2[6] = (uint16_t)_roboclaw.getc(); - + enc2 = read_byte2[1]<<24; enc2 |= read_byte2[2]<<16; enc2 |= read_byte2[3]<<8; enc2 |= read_byte2[4]; - + return enc2; } @@ -180,12 +167,12 @@ read_byte[4] = (uint16_t)_roboclaw.getc(); read_byte[5] = (uint16_t)_roboclaw.getc(); read_byte[6] = (uint16_t)_roboclaw.getc(); - + speed1 = read_byte[1]<<24; speed1 |= read_byte[2]<<16; speed1 |= read_byte[3]<<8; speed1 |= read_byte[4]; - + return speed1; } @@ -201,12 +188,12 @@ read_byte2[4] = (uint16_t)_roboclaw.getc(); read_byte2[5] = (uint16_t)_roboclaw.getc(); read_byte2[6] = (uint16_t)_roboclaw.getc(); - + speed2 = read_byte2[1]<<24; speed2 |= read_byte2[2]<<16; speed2 |= read_byte2[3]<<8; speed2 |= read_byte2[4]; - + return speed2; }