ARES
oklm
Dependents: RoboClaw TestMyPathFind Robot2016_2-0_STATIC Stressed ... more
Diff: RoboClaw.cpp
- Revision:
- 2:a2c141d922b3
- Parent:
- 1:f76058f9f548
- Child:
- 3:5c6a9045c8f7
--- a/RoboClaw.cpp Tue Nov 24 15:01:49 2015 +0000
+++ b/RoboClaw.cpp Tue Nov 24 21:41:45 2015 +0000
@@ -1,8 +1,9 @@
#include "RoboClaw.h"
#include <stdarg.h>
-#define SetDWORDval(arg) (unsigned char)(arg>>24),(unsigned char)(arg>>16),(unsigned char)(arg>>8),(unsigned char)arg
-#define SetWORDval(arg) (unsigned char)(arg>>8),(unsigned char)arg
+#define MAXTRY 2
+#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)
{
@@ -14,10 +15,10 @@
crc = 0;
}
-void RoboClaw::crc_update (unsigned char data)
+void RoboClaw::crc_update (uint8_t data)
{
int i;
- crc = crc ^ ((unsigned int)data << 8);
+ crc = crc ^ ((uint16_t)data << 8);
for (i=0; i<8; i++)
{
if (crc & 0x8000)
@@ -27,29 +28,32 @@
}
}
-unsigned int RoboClaw::crc_get()
+uint16_t RoboClaw::crc_get()
{
return crc;
}
-void RoboClaw::write_n(unsigned char cnt, ... )
+void RoboClaw::write_n(uint8_t cnt, ... )
{
- crc_clear();
- va_list marker;
- va_start( marker, cnt );
- for(unsigned char index=0;index<cnt;index++)
- {
- unsigned char data = va_arg(marker, unsigned int);
- crc_update(data);
- _roboclaw.putc(data);
- }
- va_end( marker );
- unsigned int crc = crc_get();
- _roboclaw.putc(crc>>8);
- _roboclaw.putc(crc);
+ uint8_t retry = MAXTRY;
+ do {
+ crc_clear();
+ va_list marker;
+ va_start( marker, cnt );
+ for(uint8_t index=0;index<cnt;index++)
+ {
+ uint8_t data = va_arg(marker, unsigned int);
+ crc_update(data);
+ _roboclaw.putc(data);
+ }
+ va_end( marker );
+ uint16_t crc = crc_get();
+ _roboclaw.putc(crc>>8);
+ _roboclaw.putc(crc);
+ } while(retry--);
}
-void RoboClaw::write_(unsigned char address, unsigned char command, unsigned char 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);
@@ -58,15 +62,15 @@
{
if(crcon == true)
{
- unsigned char packet[2] = {address, command};
- unsigned int checksum = crc16(packet,2);
+ uint8_t packet[2] = {address, command};
+ uint16_t checksum = crc16(packet,2);
_roboclaw.putc(checksum>>8);
_roboclaw.putc(checksum);
}
else
{
- unsigned char packet[3] = {address, command, data};
- unsigned int checksum = crc16(packet,3);
+ uint8_t packet[3] = {address, command, data};
+ uint16_t checksum = crc16(packet,3);
_roboclaw.putc(data);
_roboclaw.putc(checksum>>8);
_roboclaw.putc(checksum);
@@ -74,11 +78,11 @@
}
}
-unsigned int RoboClaw::crc16(unsigned char *packet, int nBytes) {
- unsigned int crc_;
+uint16_t RoboClaw::crc16(uint8_t *packet, int nBytes) {
+ uint16_t crc_;
for (int byte = 0; byte < nBytes; byte++) {
- crc_ = crc_ ^ ((unsigned int)packet[byte] << 8);
- for (unsigned char bit = 0; bit < 8; bit++) {
+ crc_ = crc_ ^ ((uint16_t)packet[byte] << 8);
+ for (uint8_t bit = 0; bit < 8; bit++) {
if (crc_ & 0x8000) {
crc_ = (crc_ << 1) ^ 0x1021;
} else {
@@ -89,51 +93,51 @@
return crc_;
}
-unsigned char RoboClaw::read_(void)
+uint8_t RoboClaw::read_(void)
{
return(_roboclaw.getc());
}
-void RoboClaw::ForwardM1(unsigned char address, int speed){
+void RoboClaw::ForwardM1(uint8_t address, int speed){
write_(address,M1FORWARD,speed,false,false);
}
-void RoboClaw::BackwardM1(unsigned char address, int speed){
+void RoboClaw::BackwardM1(uint8_t address, int speed){
write_(address,M1BACKWARD,speed,false,false);
}
-void RoboClaw::ForwardM2(unsigned char address, int speed){
+void RoboClaw::ForwardM2(uint8_t address, int speed){
write_(address,M2FORWARD,speed,false,false);
}
-void RoboClaw::BackwardM2(unsigned char address, int speed){
+void RoboClaw::BackwardM2(uint8_t address, int speed){
write_(address,M2BACKWARD,speed,false,false);
}
-void RoboClaw::Forward(unsigned char address, int speed){
+void RoboClaw::Forward(uint8_t address, int speed){
write_(address,MIXEDFORWARD,speed,false,false);
}
-void RoboClaw::Backward(unsigned char address, int speed){
+void RoboClaw::Backward(uint8_t address, int speed){
write_(address,MIXEDBACKWARD,speed,false,false);
}
-void RoboClaw::ReadFirm(unsigned char address){
+void RoboClaw::ReadFirm(uint8_t address){
write_(address,GETVERSION,0x00,true,false);
}
-long RoboClaw::ReadEncM1(unsigned char address){
- long enc1;
- unsigned int read_byte[7];
+int32_t RoboClaw::ReadEncM1(uint8_t address){
+ int32_t enc1;
+ uint16_t read_byte[7];
write_n(2,address,GETM1ENC);
- read_byte[0] = (unsigned int)_roboclaw.getc();
- read_byte[1] = (unsigned int)_roboclaw.getc();
- read_byte[2] = (unsigned int)_roboclaw.getc();
- read_byte[3] = (unsigned int)_roboclaw.getc();
- read_byte[4] = (unsigned int)_roboclaw.getc();
- read_byte[5] = (unsigned int)_roboclaw.getc();
- read_byte[6] = (unsigned int)_roboclaw.getc();
+ read_byte[0] = (uint16_t)_roboclaw.getc();
+ read_byte[1] = (uint16_t)_roboclaw.getc();
+ read_byte[2] = (uint16_t)_roboclaw.getc();
+ read_byte[3] = (uint16_t)_roboclaw.getc();
+ 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;
@@ -143,18 +147,18 @@
return enc1;
}
-long RoboClaw::ReadEncM2(unsigned char address){
- long enc2;
- unsigned int read_byte2[7];
+int32_t RoboClaw::ReadEncM2(uint8_t address){
+ int32_t enc2;
+ uint16_t read_byte2[7];
write_(address,GETM2ENC,0x00, true,false);
- read_byte2[0] = (unsigned int)_roboclaw.getc();
- read_byte2[1] = (unsigned int)_roboclaw.getc();
- read_byte2[2] = (unsigned int)_roboclaw.getc();
- read_byte2[3] = (unsigned int)_roboclaw.getc();
- read_byte2[4] = (unsigned int)_roboclaw.getc();
- read_byte2[5] = (unsigned int)_roboclaw.getc();
- read_byte2[6] = (unsigned int)_roboclaw.getc();
+ read_byte2[0] = (uint16_t)_roboclaw.getc();
+ read_byte2[1] = (uint16_t)_roboclaw.getc();
+ read_byte2[2] = (uint16_t)_roboclaw.getc();
+ read_byte2[3] = (uint16_t)_roboclaw.getc();
+ 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;
@@ -164,18 +168,18 @@
return enc2;
}
-long RoboClaw::ReadSpeedM1(unsigned char address){
- long speed1;
- unsigned int read_byte[7];
+int32_t RoboClaw::ReadSpeedM1(uint8_t address){
+ int32_t speed1;
+ uint16_t read_byte[7];
write_n(2,address,GETM1SPEED);
- read_byte[0] = (unsigned int)_roboclaw.getc();
- read_byte[1] = (unsigned int)_roboclaw.getc();
- read_byte[2] = (unsigned int)_roboclaw.getc();
- read_byte[3] = (unsigned int)_roboclaw.getc();
- read_byte[4] = (unsigned int)_roboclaw.getc();
- read_byte[5] = (unsigned int)_roboclaw.getc();
- read_byte[6] = (unsigned int)_roboclaw.getc();
+ read_byte[0] = (uint16_t)_roboclaw.getc();
+ read_byte[1] = (uint16_t)_roboclaw.getc();
+ read_byte[2] = (uint16_t)_roboclaw.getc();
+ read_byte[3] = (uint16_t)_roboclaw.getc();
+ 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;
@@ -185,18 +189,18 @@
return speed1;
}
-long RoboClaw::ReadSpeedM2(unsigned char address){
- long speed2;
- unsigned int read_byte2[7];
+int32_t RoboClaw::ReadSpeedM2(uint8_t address){
+ int32_t speed2;
+ uint16_t read_byte2[7];
write_n(2,address,GETM2SPEED);
- read_byte2[0] = (unsigned int)_roboclaw.getc();
- read_byte2[1] = (unsigned int)_roboclaw.getc();
- read_byte2[2] = (unsigned int)_roboclaw.getc();
- read_byte2[3] = (unsigned int)_roboclaw.getc();
- read_byte2[4] = (unsigned int)_roboclaw.getc();
- read_byte2[5] = (unsigned int)_roboclaw.getc();
- read_byte2[6] = (unsigned int)_roboclaw.getc();
+ read_byte2[0] = (uint16_t)_roboclaw.getc();
+ read_byte2[1] = (uint16_t)_roboclaw.getc();
+ read_byte2[2] = (uint16_t)_roboclaw.getc();
+ read_byte2[3] = (uint16_t)_roboclaw.getc();
+ 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;
@@ -206,51 +210,51 @@
return speed2;
}
-void RoboClaw::ResetEnc(unsigned char address){
+void RoboClaw::ResetEnc(uint8_t address){
write_n(2,address,RESETENC);
}
-void RoboClaw::SpeedM1(unsigned char address, long speed){
+void RoboClaw::SpeedM1(uint8_t address, int32_t speed){
write_n(6,address,M1SPEED,SetDWORDval(speed));
}
-void RoboClaw::SpeedM2(unsigned char address, long speed){
+void RoboClaw::SpeedM2(uint8_t address, int32_t speed){
write_n(6,address,M2SPEED,SetDWORDval(speed));
}
-void RoboClaw::SpeedAccelM1(unsigned char address, long accel, long speed){
+void RoboClaw::SpeedAccelM1(uint8_t address, int32_t accel, int32_t speed){
write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
}
-void RoboClaw::SpeedAccelM2(unsigned char address, long accel, long speed){
+void RoboClaw::SpeedAccelM2(uint8_t address, int32_t accel, int32_t speed){
write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
}
-void RoboClaw::SpeedDistanceM1(unsigned char address, long speed, unsigned long distance, unsigned char buffer){
+void RoboClaw::SpeedDistanceM1(uint8_t address, int32_t speed, uint32_t distance, uint8_t buffer){
write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
}
-void RoboClaw::SpeedDistanceM2(unsigned char address, long speed, unsigned long distance, unsigned char buffer){
+void RoboClaw::SpeedDistanceM2(uint8_t address, int32_t speed, uint32_t distance, uint8_t buffer){
write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
}
-void RoboClaw::SpeedAccelDistanceM1(unsigned char address, long accel, long speed, unsigned long distance, unsigned char buffer){
+void RoboClaw::SpeedAccelDistanceM1(uint8_t address, int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
}
-void RoboClaw::SpeedAccelDistanceM2(unsigned char address, long accel, long speed, unsigned long distance, unsigned char buffer){
+void RoboClaw::SpeedAccelDistanceM2(uint8_t address, int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
}
-void RoboClaw::SpeedAccelDeccelPositionM1(unsigned char address, unsigned long accel, long speed, unsigned long deccel, long position, unsigned char flag){
+void RoboClaw::SpeedAccelDeccelPositionM1(uint8_t address, uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
}
-void RoboClaw::SpeedAccelDeccelPositionM2(unsigned char address, unsigned long accel, long speed, unsigned long deccel, long position, unsigned char flag){
+void RoboClaw::SpeedAccelDeccelPositionM2(uint8_t address, uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
}
-void RoboClaw::SpeedAccelDeccelPositionM1M2(unsigned char address,unsigned long accel1,unsigned long speed1,unsigned long deccel1, long position1,unsigned long accel2,unsigned long speed2,unsigned long deccel2, long position2,unsigned char flag){
+void RoboClaw::SpeedAccelDeccelPositionM1M2(uint8_t address,uint32_t accel1,uint32_t speed1,uint32_t deccel1, int32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2, int32_t position2,uint8_t flag){
write_n(35,address,MIXEDSPEEDACCELDECCELPOS,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(deccel1),SetDWORDval(position1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(deccel2),SetDWORDval(position2),flag);
}