xx
RoboClaw.cpp
- Committer:
- sype
- Date:
- 2015-11-16
- Revision:
- 0:af5cf35e1a25
- Child:
- 1:f76058f9f548
File content as of revision 0:af5cf35e1a25:
#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 RoboClaw::RoboClaw(int baudrate, PinName rx, PinName tx) : _roboclaw(rx, tx) { _roboclaw.baud(baudrate); } void RoboClaw::crc_clear() { crc = 0; } void RoboClaw::crc_update (unsigned char data) { int i; crc = crc ^ ((unsigned int)data << 8); for (i=0; i<8; i++) { if (crc & 0x8000) crc = (crc << 1) ^ 0x1021; else crc <<= 1; } } unsigned int RoboClaw::crc_get() { return crc; } void RoboClaw::write_n(unsigned char 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); } void RoboClaw::write_(unsigned char address, unsigned char command, unsigned char data, bool reading, bool crcon) { _roboclaw.putc(address); _roboclaw.putc(command); if(reading == false) { if(crcon == true) { unsigned char packet[2] = {address, command}; unsigned int 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); _roboclaw.putc(data); _roboclaw.putc(checksum>>8); _roboclaw.putc(checksum); } } } unsigned int RoboClaw::crc16(unsigned char *packet, int nBytes) { unsigned int crc_; for (int byte = 0; byte < nBytes; byte++) { crc_ = crc_ ^ ((unsigned int)packet[byte] << 8); for (unsigned char bit = 0; bit < 8; bit++) { if (crc_ & 0x8000) { crc_ = (crc_ << 1) ^ 0x1021; } else { crc_ = crc_ << 1; } } } return crc_; } unsigned char RoboClaw::read_(void) { return(_roboclaw.getc()); } void RoboClaw::ForwardM1(unsigned char address, int speed){ write_(address,M1FORWARD,speed,false,false); } void RoboClaw::BackwardM1(unsigned char address, int speed){ write_(address,M1BACKWARD,speed,false,false); } void RoboClaw::ForwardM2(unsigned char address, int speed){ write_(address,M2FORWARD,speed,false,false); } void RoboClaw::BackwardM2(unsigned char address, int speed){ write_(address,M2BACKWARD,speed,false,false); } void RoboClaw::Forward(unsigned char address, int speed){ write_(address,MIXEDFORWARD,speed,false,false); } void RoboClaw::Backward(unsigned char address, int speed){ write_(address,MIXEDBACKWARD,speed,false,false); } void RoboClaw::ReadFirm(unsigned char address){ write_(address,GETVERSION,0x00,true,false); } long RoboClaw::ReadEncM1(unsigned char address){ long enc1; unsigned int 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(); enc1 = read_byte[1]<<24; enc1 |= read_byte[2]<<16; enc1 |= read_byte[3]<<8; enc1 |= read_byte[4]; return enc1; } long RoboClaw::ReadEncM2(unsigned char address){ long enc2; unsigned int 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(); enc2 = read_byte2[1]<<24; enc2 |= read_byte2[2]<<16; enc2 |= read_byte2[3]<<8; enc2 |= read_byte2[4]; return enc2; } long RoboClaw::ReadSpeedM1(unsigned char address){ long speed1; unsigned int 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(); speed1 = read_byte[1]<<24; speed1 |= read_byte[2]<<16; speed1 |= read_byte[3]<<8; speed1 |= read_byte[4]; return speed1; } long RoboClaw::ReadSpeedM2(unsigned char address){ long speed2; unsigned int 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(); speed2 = read_byte2[1]<<24; speed2 |= read_byte2[2]<<16; speed2 |= read_byte2[3]<<8; speed2 |= read_byte2[4]; return speed2; } void RoboClaw::ResetEnc(unsigned char address){ write_n(2,address,RESETENC); } void RoboClaw::SpeedM1(unsigned char address, long speed){ write_n(6,address,M1SPEED,SetDWORDval(speed)); } void RoboClaw::SpeedM2(unsigned char address, long speed){ write_n(6,address,M2SPEED,SetDWORDval(speed)); } void RoboClaw::SpeedAccelM1(unsigned char address, long accel, long speed){ write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed)); } void RoboClaw::SpeedAccelM2(unsigned char address, long accel, long speed){ write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed)); } void RoboClaw::SpeedDistanceM1(unsigned char address, long speed, unsigned long distance, unsigned char 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){ 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){ 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){ 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, unsigned long position, unsigned char flag){ return 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, unsigned long position, unsigned char flag){ return write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag); }