Marco De Silva
a
Diff: RoboClaw/RoboClaw.cpp
- Revision:
- 0:6b67f1bb9c76
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboClaw/RoboClaw.cpp Tue Sep 28 10:42:56 2021 +0000
@@ -0,0 +1,291 @@
+#include "RoboClaw.h"
+#include <stdarg.h>
+
+#define MAXTRY 1
+#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(uint8_t adr, int baudrate, PinName rx, PinName tx, double _read_timeout) : _roboclaw(rx, tx){
+ _roboclaw.set_baud(baudrate);//_roboclaw.baud(baudrate);
+
+ rx_in = 0;
+ address = adr;
+ //readTime = 0.0;
+ //readTimer.start();
+ read_timeout = _read_timeout;
+
+}
+
+void RoboClaw::crc_clear(){
+ crc = 0;
+}
+
+
+void RoboClaw::flushSerialBuffer(void) {
+ char char1[1];
+
+ while (_roboclaw.readable()) {
+ _roboclaw.read_timeout(char1,1,read_timeout);
+ }
+return; }
+
+void RoboClaw::crc_update (uint8_t data){
+ int i;
+ crc = crc ^ ((uint16_t)data << 8);
+ for (i=0; i<8; i++) {
+ if (crc & 0x8000)
+ crc = (crc << 1) ^ 0x1021;
+ else
+ crc <<= 1;
+ }
+}
+
+uint16_t RoboClaw::crc_get(){
+ return crc;
+}
+
+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);
+ for (uint8_t bit = 0; bit < 8; bit++) {
+ if (crc_ & 0x8000) {
+ crc_ = (crc_ << 1) ^ 0x1021;
+ } else {
+ crc_ = crc_ << 1;
+ }
+ }
+ }
+ return crc_;
+}
+
+void RoboClaw::write_n(uint8_t cnt, ... ){
+ uint8_t count = 0;
+ char dat[1];
+ char r_dat[1];
+ r_dat[0] = ' ';
+ char crc_[2];
+ do {
+ _roboclaw.flush();
+
+ 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);
+ dat[0] = data;
+ crc_update(data);
+ _roboclaw.write(dat,1);
+ }
+ va_end( marker );
+ uint16_t crc = crc_get();
+ crc_[0]=crc>>8;
+ crc_[1]=crc;
+ _roboclaw.write(crc_,2);
+ _roboclaw.read_timeout(r_dat,1,read_timeout);
+
+ count ++;
+
+ }while((uint16_t)r_dat[0] != 0xFF&& count <=MAXTRY);
+
+}
+
+void RoboClaw::write_(uint8_t command, uint8_t data, bool reading, bool crcon){
+ // _roboclaw.putc(address);
+ // _roboclaw.putc(command);
+
+ 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 {
+ uint8_t packet[3] = {address, command, data};
+ uint16_t checksum = crc16(packet,3);
+ // _roboclaw.putc(data);
+ // _roboclaw.putc(checksum>>8);
+ // _roboclaw.putc(checksum);
+ }
+ }
+}
+
+bool RoboClaw::read_n(uint8_t address, uint8_t cmd, int n_byte, int32_t &value){
+ uint16_t read_byte[n_byte];
+ uint16_t received_crc;
+ int dataRead=0;
+ char tx_buffer[2];
+
+ tx_buffer[0] = address;
+ tx_buffer[1] = cmd;
+
+ rx_in = 0;
+ crc_clear();
+
+ _roboclaw.flush();
+ _roboclaw.write(tx_buffer,2);
+
+ crc_update(address);
+ crc_update(cmd);
+
+ dataRead = _roboclaw.read_timeout(rx_buffer,7,read_timeout);
+ //printf("d: %d\r\n", dataRead);
+ //printf("add: %x, cmd: %x, 0: %x, 1: %x, 2: %x, 3: %x, 4: %x, 5: %x, 6: %x \r\n",(uint16_t)tx_buffer[0],(uint16_t)tx_buffer[1],(uint16_t)rx_buffer[0],(uint16_t)rx_buffer[1],(uint16_t)rx_buffer[2],(uint16_t)rx_buffer[3],(uint16_t)rx_buffer[4],(uint16_t)rx_buffer[5],(uint16_t)rx_buffer[6]);
+ //printf("dopo del read\r\n");
+ for(int i = 0; i<=n_byte-3; i++){
+ crc_update((uint16_t)rx_buffer[i]);
+ }
+
+ received_crc = (uint16_t)rx_buffer[n_byte-2]<<8;
+ received_crc |= (uint16_t)rx_buffer[n_byte-1];
+
+ if(received_crc == crc_get()){
+ value = (uint16_t)rx_buffer[0]<<24;
+ value |= (uint16_t)rx_buffer[1]<<16;
+ value |= (uint16_t)rx_buffer[2]<<8;
+ value |= (uint16_t)rx_buffer[3];
+ return true;
+ }else{
+ //printf("crc: %x \r\n", crc_get());
+ //printf("received crc: %x \r\n", received_crc);
+ printf("err \r\n");
+
+
+ return false;
+ }
+}
+
+
+void RoboClaw::ForwardM1(int speed){
+ write_(M1FORWARD,speed,false,false);
+}
+
+void RoboClaw::BackwardM1(int speed){
+ write_(M1BACKWARD,speed,false,false);
+}
+
+void RoboClaw::ForwardM2(int speed){
+ write_(M2FORWARD,speed,false,false);
+}
+
+void RoboClaw::BackwardM2(int speed){
+ write_(M2BACKWARD,speed,false,false);
+}
+
+void RoboClaw::Forward(int speed){
+ write_(MIXEDFORWARD,speed,false,false);
+}
+
+void RoboClaw::Backward(int speed){
+ write_(MIXEDBACKWARD,speed,false,false);
+}
+
+void RoboClaw::ReadFirm(){
+ write_(GETVERSION,0x00,true,false);
+}
+
+bool RoboClaw::ReadEncM1(int32_t &encPulse){
+ int32_t value=0;
+ if (read_n(address, GETM1ENC, 7, value) == true){
+ encPulse = value;
+ return true;
+ }else{
+ return false;
+ }
+}
+
+bool RoboClaw::ReadEncM2(int32_t &encPulse){
+ int32_t value=0;
+ if (read_n(address, GETM2ENC, 7, value) == true){
+ encPulse = value;
+ return true;
+ }else{
+ return false;
+ }
+}
+
+bool RoboClaw::ReadSpeedM1(int32_t &speed){
+ int32_t value=0;
+ if (read_n(address, GETM1SPEED, 7, value) == true){
+ speed = value;
+ return true;
+ }else{
+ return false;
+ }
+}
+
+bool RoboClaw::ReadSpeedM2(int32_t &speed){
+ int32_t value=0;
+ if (read_n(address, GETM2SPEED, 7, value) == true){
+ speed = value;
+ return true;
+ }else{
+ return false;
+ }
+}
+
+
+bool RoboClaw::ReadCurrentM1M2(int32_t& currentM1, int32_t& currentM2){
+ int32_t value=0;
+ if (read_n(address, GETCURRENTS, 6, value) == true){
+ currentM1 = value>>16;
+ currentM2 = value&0xFFFF;
+ return true;
+ }else{
+ return false;
+ }
+
+}
+
+void RoboClaw::ResetEnc(){
+ write_n(2,address,RESETENC);
+}
+
+void RoboClaw::SpeedM1(int32_t speed){
+ write_n(6,address,M1SPEED,SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedM2(int32_t speed){
+ write_n(6,address,M2SPEED,SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedAccelM1(int32_t accel, int32_t speed){
+ write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedAccelM2(int32_t accel, int32_t speed){
+ write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedAccelM1M2(int32_t accel, int32_t speed1, int32_t speed2){
+ write_n(14,address,MIXEDSPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed1),SetDWORDval(speed2));
+}
+
+void RoboClaw::SpeedDistanceM1(int32_t speed, uint32_t distance, uint8_t buffer){
+ write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
+}
+
+void RoboClaw::SpeedDistanceM2(int32_t speed, uint32_t distance, uint8_t buffer){
+ write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
+}
+
+void RoboClaw::SpeedAccelDistanceM1(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(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(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(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(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);
+}
+