HerkuleXLib  1.2
Arduino library to drive HerkuleX DRS-0101 and DRS-0201
/home/benoit/sketchbook/libraries/HerkuleXLib/HkxCommunication.h
Go to the documentation of this file.
1 #ifndef _HKXCOMMUNICATION_H
2 #define _HKXCOMMUNICATION_H
3 
13 #include "Arduino.h"
14 #include "HkxStatus.h"
15 
18 const bool HKX_DEV = true;
20 const uint8_t HKX_WAIT_TIME = 10;
22 const uint8_t HKX_NUMBER_SEND_TRIALS = 5;
24 const uint8_t HKX_NUMBER_CONNECT_TRIALS = 2;
26 const uint8_t HKX_SERIAL_BUFFER = 64;
27 
29 const uint8_t HKX_MAX_ID = 0xFD;
31 const uint8_t HKX_ID_ALL = 0xFE;
33 const uint8_t HKX_MAX_POLICY = 0x7F;
35 const uint8_t HKX_MAX_TEMPERATURE = 0xFE;
37 const uint8_t HKX_MAX_VOLTAGE = 0xFE;
39 const uint8_t HKX_MAX_ACCELERATION_RATIO = 50;
41 const uint8_t HKX_MAX_TIME = 0xFE;
43 const uint8_t HKX_MAX_POSITIONNING = 0xFE;
45 const uint8_t HKX_MAX_SATURATOR_OFFSET = 0xFE;
47 const uint8_t HKX_MAX_MIN_PWM = 0xFE;
49 const uint16_t HKX_MAX_PWM = 0x3FF;
51 const uint16_t HKX_MAX_POSITION = 0x3FF;
53 const uint16_t HKX_MAX_CONTROL_PARAM = 0x7FFF;
55 const uint8_t HKX_MAX_CHECK_PERIOD = 0xFE;
56 
57 
72 enum hkxCommand:uint8_t{
73  HKX_CMD_ROM_WRITE = 0x01,
74  HKX_CMD_ROM_READ = 0x02,
75  HKX_CMD_RAM_WRITE = 0x03,
76  HKX_CMD_RAM_READ = 0x04,
77  HKX_CMD_I_JOG = 0x05,
78  HKX_CMD_S_JOG = 0x06,
79  HKX_CMD_STAT = 0x07,
80  HKX_CMD_ROLLBACK = 0x08,
81  HKX_CMD_REBOOT = 0x09,
82 };
83 
91 enum hkxMemory:bool{
92  HKX_ROM = 0,
93  HKX_RAM = 1,
94 };
109 enum hkxBaudrate:uint32_t{
110  HKX_57600 = 57600,
111  HKX_115200 = 115200,
112  HKX_200000 = 200000,
113  HKX_250000 = 250000,
114  HKX_400000 = 400000,
115  HKX_500000 = 500000,
116  HKX_666666 = 666666,
117 };
118 
127 enum hkxTorqueControl:uint8_t{
128  HKX_TORQUE_FREE = 0x00,
129  HKX_TORQUE_BREAK = 0x40,
130  HKX_TORQUE_ON = 0x60,
131 };
132 
146 enum hkxLEDControl:uint8_t{
147  HKX_LED_OFF = 0x00,
148  HKX_LED_GREEN = 0x01,
149  HKX_LED_BLUE = 0x02,
150  HKX_LED_RED = 0x04,
151  HKX_LED_CYAN = 0x03,
152  HKX_LED_YELLOW = 0x05,
153  HKX_LED_PINK = 0x06,
154  HKX_LED_WHITE = 0x07,
155 };
156 
164 enum hkxControlMode:uint8_t{
165  HKX_CTRL_POSITION = 0x00,
166  HKX_CTRL_ROTATION = 0x01,
167 };
168 
171 static const int16_t temperatureConversion[] = {-7947,-7178,-6320,-5781,-5380,-5058,-4786,-4549,-4340,-4151,-3979,-3820,-3673,-3535,
172  -3406,-3283,-3167,-3057,-2951,-2850,-2753,-2659,-2569,-2482,-2397,-2315,-2236,-2159,-2083,-2010,-1938,-1868,-1800,-1733,-1667,-1603,
173  -1539,-1477,-1417,-1357,-1298,-1240,-1183,-1126,-1071,-1016,-962,-909,-856,-804,-753,-702,-652,-602,-553,-504,-456,-408,-361,-314,
174  -267,-221,-175,-129,-84,-39,5,49,93,137,181,224,267,310,352,394,437,478,520,562,603,644,686,727,767,808,849,889,929,970,1010,1050,
175  1090,1130,1170,1209,1249,1289,1328,1368,1407,1447,1486,1526,1565,1605,1644,1684,1723,1762,1802,1841,1881,1920,1960,1999,2039,2079,
176  2119,2158,2198,2238,2278,2318,2359,2399,2439,2480,2520,2561,2602,2643,2684,2725,2766,2808,2850,2891,2933,2976,3018,3060,3103,3146,
177  3189,3232,3276,3320,3364,3408,3453,3497,3542,3588,3633,3679,3725,3772,3818,3866,3913,3961,4009,4057,4106,4156,4205,4256,4306,4357,
178  4409,4461,4513,4566,4619,4673,4728,4783,4839,4895,4952,5009,5068,5127,5186,5247,5308,5370,5433,5496,5561,5626,5693,5760,5828,5898,
179  5968,6040,6113,6187,6263,6339,6417,6497,6578,6861,6746,6832,6920,7010,7102,7196,7292,7391,7492,7596,7703,7812,7925,8041,8160,8284,
180  8411,8542,8679,8820,8966,9118,9276,9441,9613,9793,9982,10181,10390,10611,10845,11093,11359,11643,11949,12280,12641,13036,13472,
181  13959,14509,15139,15873,16750,17829,19218,21132,24101,30091
182 };
192  HKX_NO_VALUE,
193 };
194 
201 template <typename T>
202 class HkxMaybe {
203 private:
205  const bool _empty;
207  T* const _value;
208 
209 public:
214  HkxMaybe() : _empty(true), _value(0){};
215 
224  HkxMaybe(hkxMaybeHasNoValue noValue) : _empty(true), _value(0){};
225 
231  HkxMaybe(T value) : _empty(false), _value(new T(value)){};
232 
238  HkxMaybe(T* addressValue) : _empty(false), _value(addressValue){};
239 
244  ~HkxMaybe(){_value->~T();};
245 
253  bool isEmpty() const {return _empty; };
254 
262  T& getValue() const {if(!_empty){return *_value;}};
263 
269  void setValue(T value) {if(!_empty){*_value = value;}};
270 };
278 class HkxPrint {
279 public:
287  HkxPrint() : _errorMessages(false), _warningMessages(false), _infoMessages(false), _serialPrint(0){};
288 
298  HkxPrint(HardwareSerial& serialPrint, long baudrate) : _errorMessages(true), _warningMessages(false), _infoMessages(false), _serialPrint(&serialPrint){
299  serialPrint.begin(baudrate);
300  infoPrint(F("start > Start the communication for printing"));
301  };
302 
315  HkxPrint(HardwareSerial& serialPrint, long baudrate, bool errorMessages, bool warningMessages, bool infoMessages) : _errorMessages(errorMessages), _warningMessages(warningMessages), _infoMessages(infoMessages), _serialPrint(&serialPrint){
316  serialPrint.begin(baudrate);
317  infoPrint(F("start > Start the communication for printing"));
318  };
319 
320  // for Arduino Micro and Leonardo only
321  #if defined (__AVR_ATmega32U4__)
322 
331  HkxPrint(Serial_& serialPrint, long baudrate) : _errorMessages(true), _warningMessages(false), _infoMessages(false), _serialPrint(&serialPrint){
332  serialPrint.begin(baudrate);
333  infoPrint(F("start > Start the communication for printing"));
334  };
335 
348  HkxPrint(Serial_& serialPrint, long baudrate, bool errorMessages, bool warningMessages, bool infoMessages) : _errorMessages(errorMessages), _warningMessages(warningMessages), _infoMessages(infoMessages), _serialPrint(&serialPrint){
349  serialPrint.begin(baudrate);
350  infoPrint(F("start > Start the communication for printing"));
351  };
352  #endif
353 
368  HkxPrint(HkxPrint hkxPrint, bool errorMessages, bool warningMessages, bool infoMessages) : _errorMessages(errorMessages), _warningMessages(warningMessages), _infoMessages(infoMessages), _serialPrint(hkxPrint._serialPrint){};
369 
381  void errorPrint(const String& message) const {if(_errorMessages){_serialPrint->println(String(F("Error: ")) + message);}};
382 
394  void warningPrint(const String& message) const {if(_warningMessages){_serialPrint->println(String(F("Warning: ")) + message);}};
395 
407  void infoPrint(const String& message) const {if(_infoMessages){_serialPrint->println(String(F("Info: ")) + message);}};
411  const boolean _errorMessages;
413  const boolean _warningMessages;
415  const boolean _infoMessages;
417  Stream* const _serialPrint;
418 
420 };
421 
428 class HkxUnitConversion {
429 public:
437  static int16_t voltageRawToValue(uint8_t voltageRaw){return (uint16_t)voltageRaw*74;};
438 
446  static uint8_t voltageValueToRaw(int16_t voltageValue){return (uint8_t)(voltageValue/74);};
447 
454  static int16_t temperatureRawToValue(uint8_t temperatureRaw){return temperatureConversion[temperatureRaw];};
455 
462  static uint8_t temperatureValueToRaw(int16_t temperatureValue){
463  uint8_t temperatureRaw = 128;
464  uint8_t sup = 254;
465  uint8_t inf = 0;
466  while((temperatureConversion[temperatureRaw] != temperatureValue) && ((sup - inf) > 1) ){ //dichotomy
467  if(temperatureConversion[temperatureRaw] > temperatureValue) {sup = temperatureRaw;}
468  else {inf = temperatureRaw;}
469  temperatureRaw = (sup + inf)/2;
470  }
471  return temperatureRaw;
472  }; // lower value approximation
473 
481  static int16_t angleRawToValue(int16_t angleRaw){return ((int32_t)angleRaw)*326/100;};
482 
490  static int16_t angleValueToRaw(int16_t angleValue){return ((int32_t)angleValue*100/326)+(angleValue<0?-1:(angleValue>0?1:0));}; // +1 because of the rounding
491 
499  static int16_t positionRawToValue(uint16_t positionRaw){return angleRawToValue((int16_t)positionRaw - 512);};
500 
508  static uint16_t positionValueToRaw(int16_t positionValue){return min(1023,max(0,angleValueToRaw(simpleAngle(positionValue)) + 512));};
509 
510  static int16_t simpleAngle(int16_t angle){return ((angle + 1800) % 3600) - 1800;};
511 
519  static int16_t velocityRawToValue(int16_t velocityRaw){return ((int32_t)velocityRaw)*2909/100;};
520 
528  static int16_t velocityValueRaw(int16_t velocityValue){return ((int32_t)velocityValue)*100/2909+(velocityValue<0?-1:(velocityValue>0?1:0));}; // +1 because of the rounding
529 
537  static uint16_t timeRawToValue(uint8_t timeRaw){return ((uint16_t)timeRaw*112)/10;};
538 
546  static uint8_t timeValueToRaw(uint16_t timeValue){return (uint8_t)((timeValue*10)/112)+1;}; // +1 because of the rounding
547 
559  static uint16_t slopeRawToValue(uint16_t slopeRaw){return ((uint32_t)(slopeRaw?slopeRaw:32768)*1000)/835;};
560 
572  static uint16_t slopeValueToRaw(uint16_t slopeValue){
573  uint16_t slopeRaw = (((uint32_t)slopeValue*835)/1000) + 1; // +1 because of the rounding (+ avoid 0 reserved for no slope)
574  return slopeRaw>32767?0:slopeRaw;
575  };
576 
592  static hkxBaudrate baudrateRawToValue(uint8_t baudrateRaw){
593  switch (baudrateRaw){
594  case 0x02 : return HKX_666666;
595  case 0x03 : return HKX_500000;
596  case 0x04 : return HKX_400000;
597  case 0x07 : return HKX_250000;
598  case 0x09 : return HKX_200000;
599  case 0x10 : return HKX_115200;
600  case 0x22 : return HKX_57600; }
601  };
602 
618  static uint8_t baudrateValueToRaw(hkxBaudrate baudrate){
619  switch (baudrate){
620  case HKX_666666 : return 0x02;
621  case HKX_500000 : return 0x03;
622  case HKX_400000 : return 0x04;
623  case HKX_250000 : return 0x07;
624  case HKX_200000 : return 0x09;
625  case HKX_115200 : return 0x10;
626  case HKX_57600 : return 0x22; }
627  };
628 };
629 
638 private:
640  uint8_t _packetRead[HKX_SERIAL_BUFFER];
642  HardwareSerial& _serialServos;
644  HkxPrint& _print;
646  const String _className;
647 
665  void sendPacket(uint8_t ID, hkxCommand CMD, const uint8_t data[], uint8_t dataLength);
666 
678  uint8_t receivePacket(uint8_t& dataLength);
679 
703  uint8_t readPacket(uint8_t& ID, hkxCommand& CMD, uint8_t data[], uint8_t dataLength, HkxMaybe<HkxStatus> statusED);
704 
726  uint8_t checkSum1(uint8_t packetSize, uint8_t ID, hkxCommand CMD, const uint8_t data[], uint8_t dataLength){
727  uint8_t cks = packetSize ^ ID ^ CMD;
728  for(int i = 0 ; i < dataLength ; i++){
729  cks = cks ^ data[i];
730  }
731  return cks & 0xFE;
732  };
733 
742  uint8_t checkSum2(uint8_t checkSum1){return (~checkSum1) & 0xFE;};
743 
748  void reinitPacketRead(){memset(_packetRead,0,HKX_SERIAL_BUFFER);};
749 
754  void cleanSerialBuffer(){while(this->_serialServos.available()){this->_serialServos.read();}};
755 
761  void errorPrint(const String& message){if(_print._errorMessages){_print.errorPrint(_className + message);}};
762 
768  void warningPrint(const String& message){if(_print._errorMessages){_print.warningPrint(_className + message);}};
769 
775  void infoPrint(const String& message){if(_print._errorMessages){_print.infoPrint(_className + message);}};
776 
778 public:
779 
801  HkxCommunication(hkxBaudrate baudrate, HardwareSerial& serialServos, HkxPrint& print);
802 
821  uint8_t readRequest(uint8_t ID, hkxMemory mem, uint8_t start, uint8_t length, uint8_t returnData[], HkxMaybe<HkxStatus> statusED);
822 
837  uint8_t writeRequest(uint8_t ID, hkxMemory mem, uint8_t start, uint8_t length, const uint8_t writeData[]);
838 
850  uint8_t statusRequest(uint8_t ID, HkxStatus& statusED);
851 
870  uint8_t iJogRequest(uint8_t length, const uint16_t jog[], const uint8_t set[], const uint8_t ID[], const uint8_t playTime[]);
871 
890  uint8_t sJogRequest(uint8_t playTime, uint8_t length, const uint16_t jog[], const uint8_t set[], const uint8_t ID[]);
891 
905  void start(hkxBaudrate newBaudrate){
906  _serialServos.begin(newBaudrate);
907  infoPrint(F("start > Start the communication with the servos"));
908  }
909 
914  void stop(){
915  this->_serialServos.end();
916  infoPrint(F("stop > Stop the communication with the servos"));
917  }
918 
927  uint8_t rebootRequest(uint8_t ID){
928  if(HKX_DEV && ID > 0xFE){
929  errorPrint(F("rebootRequest > input values not correct"));
930  return 1;
931  }
932  uint8_t sData[0];
933  sendPacket(ID, HKX_CMD_REBOOT, sData, 0);
934  return 0;
935  };
936 
947  uint8_t rollbackRequest(uint8_t ID, boolean IDskip, boolean bandSkip){
948  if(HKX_DEV && ID > 0xFE){
949  errorPrint(F("rollbackRequest > input values not correct"));
950  return 1;
951  }
952  uint8_t data[] = {IDskip?(uint8_t)1:(uint8_t)0 , bandSkip?(uint8_t)1:(uint8_t)0};
953  sendPacket(ID, HKX_CMD_ROLLBACK, data, 2);
954  return 0;
955  };
956 
962  HardwareSerial& getSerial(){return _serialServos;};
964 };
965 
966 
967 #endif // _HKXCOMMUNICATION_H
968 
HkxPrint(HardwareSerial &serialPrint, long baudrate)
Active constructor with default values.
Definition: HkxCommunication.h:298
void warningPrint(const String &message) const
Print warning.
Definition: HkxCommunication.h:394
HkxCommunication(hkxBaudrate baudrate, HardwareSerial &serialServos, HkxPrint &print)
Constructor.
Definition: HkxCommunication.cpp:186
void errorPrint(const String &message) const
Print error.
Definition: HkxCommunication.h:381
HkxPrint()
Default constructor.
Definition: HkxCommunication.h:287
Status management of the Arduino library for HerkuleX servo (DRS-0101 and DRS-0201 models) ...
hkxBaudrate
Baud rate.
Definition: HkxCommunication.h:109
hkxTorqueControl
Torque control.
Definition: HkxCommunication.h:127
Class to manage the communication.
Definition: HkxCommunication.h:636
Class to manage the status of the servos.
Definition: HkxStatus.h:55
HkxPrint(HardwareSerial &serialPrint, long baudrate, bool errorMessages, bool warningMessages, bool infoMessages)
Active constructor.
Definition: HkxCommunication.h:315
hkxLEDControl
LED control.
Definition: HkxCommunication.h:146
Class to manage the print of the messages.
Definition: HkxCommunication.h:278
void infoPrint(const String &message) const
Print info.
Definition: HkxCommunication.h:407
hkxControlMode
Control mode.
Definition: HkxCommunication.h:164
hkxMaybeHasNoValue
No value.
Definition: HkxCommunication.h:191
HkxPrint(HkxPrint hkxPrint, bool errorMessages, bool warningMessages, bool infoMessages)
Copy constructor.
Definition: HkxCommunication.h:368