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;
 }