/home/benoit/sketchbook/libraries/HerkuleXLib/HkxCommunication.h

Go to the documentation of this file.

#ifndef _HKXCOMMUNICATION_H
#define _HKXCOMMUNICATION_H

#include "Arduino.h"
#include "HkxStatus.h"

const bool HKX_DEV = true;
const uint8_t HKX_WAIT_TIME = 10; 
const uint8_t HKX_NUMBER_SEND_TRIALS = 5; 
const uint8_t HKX_NUMBER_CONNECT_TRIALS = 2; 
const uint8_t HKX_SERIAL_BUFFER = 64; 

const uint8_t HKX_MAX_ID = 0xFD;
const uint8_t HKX_ID_ALL = 0xFE;
const uint8_t HKX_MAX_POLICY = 0x7F;
const uint8_t HKX_MAX_TEMPERATURE = 0xFE;
const uint8_t HKX_MAX_VOLTAGE = 0xFE;
const uint8_t HKX_MAX_ACCELERATION_RATIO = 50;
const uint8_t HKX_MAX_TIME = 0xFE;
const uint8_t HKX_MAX_POSITIONNING = 0xFE;
const uint8_t HKX_MAX_SATURATOR_OFFSET = 0xFE;
const uint8_t HKX_MAX_MIN_PWM = 0xFE;
const uint16_t HKX_MAX_PWM = 0x3FF;
const uint16_t HKX_MAX_POSITION = 0x3FF;
const uint16_t HKX_MAX_CONTROL_PARAM = 0x7FFF;
const uint8_t HKX_MAX_CHECK_PERIOD = 0xFE;


enum hkxCommand:uint8_t{
  HKX_CMD_ROM_WRITE   = 0x01,
  HKX_CMD_ROM_READ    = 0x02,
  HKX_CMD_RAM_WRITE   = 0x03,
  HKX_CMD_RAM_READ    = 0x04,
  HKX_CMD_I_JOG       = 0x05,
  HKX_CMD_S_JOG       = 0x06,
  HKX_CMD_STAT        = 0x07,
  HKX_CMD_ROLLBACK    = 0x08,
  HKX_CMD_REBOOT      = 0x09,
};

enum hkxMemory:bool{
  HKX_ROM  = 0,
  HKX_RAM = 1,
};
enum hkxBaudrate:uint32_t{
  HKX_57600 = 57600,
  HKX_115200 = 115200,
  HKX_200000 = 200000,
  HKX_250000 = 250000,
  HKX_400000 = 400000,
  HKX_500000 = 500000,
  HKX_666666 = 666666,
};

enum hkxTorqueControl:uint8_t{
  HKX_TORQUE_FREE  = 0x00,
  HKX_TORQUE_BREAK = 0x40,
  HKX_TORQUE_ON = 0x60,
};

enum hkxLEDControl:uint8_t{
  HKX_LED_OFF  = 0x00,
  HKX_LED_GREEN = 0x01,
  HKX_LED_BLUE = 0x02,
  HKX_LED_RED = 0x04,
  HKX_LED_CYAN = 0x03,
  HKX_LED_YELLOW = 0x05,
  HKX_LED_PINK = 0x06,
  HKX_LED_WHITE = 0x07,
};

enum hkxControlMode:uint8_t{
  HKX_CTRL_POSITION = 0x00,
  HKX_CTRL_ROTATION = 0x01,
};

static const int16_t temperatureConversion[] = {-7947,-7178,-6320,-5781,-5380,-5058,-4786,-4549,-4340,-4151,-3979,-3820,-3673,-3535, 
    -3406,-3283,-3167,-3057,-2951,-2850,-2753,-2659,-2569,-2482,-2397,-2315,-2236,-2159,-2083,-2010,-1938,-1868,-1800,-1733,-1667,-1603,
    -1539,-1477,-1417,-1357,-1298,-1240,-1183,-1126,-1071,-1016,-962,-909,-856,-804,-753,-702,-652,-602,-553,-504,-456,-408,-361,-314,
    -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,
    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,
    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,
    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,
    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,
    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,
    8411,8542,8679,8820,8966,9118,9276,9441,9613,9793,9982,10181,10390,10611,10845,11093,11359,11643,11949,12280,12641,13036,13472,
    13959,14509,15139,15873,16750,17829,19218,21132,24101,30091
}; 
enum hkxMaybeHasNoValue:bool{
  HKX_NO_VALUE,
};

template <typename T>
class HkxMaybe {
private:
  const bool _empty; 
  T* const _value;

public:
  HkxMaybe() : _empty(true), _value(0){};

  HkxMaybe(hkxMaybeHasNoValue noValue) : _empty(true), _value(0){};

  HkxMaybe(T value) : _empty(false), _value(new T(value)){};

  HkxMaybe(T* addressValue) : _empty(false), _value(addressValue){};

  ~HkxMaybe(){_value->~T();};

  bool isEmpty() const {return _empty; };

  T& getValue() const {if(!_empty){return *_value;}};

  void setValue(T value) {if(!_empty){*_value = value;}};
};
class HkxPrint {
public:
  HkxPrint() : _errorMessages(false), _warningMessages(false), _infoMessages(false), _serialPrint(0){};

  HkxPrint(HardwareSerial& serialPrint, long baudrate) : _errorMessages(true), _warningMessages(false), _infoMessages(false), _serialPrint(&serialPrint){
    serialPrint.begin(baudrate);
    infoPrint(F("start > Start the communication for printing"));
  };

  HkxPrint(HardwareSerial& serialPrint, long baudrate, bool errorMessages, bool warningMessages, bool infoMessages) : _errorMessages(errorMessages), _warningMessages(warningMessages), _infoMessages(infoMessages), _serialPrint(&serialPrint){
    serialPrint.begin(baudrate);
    infoPrint(F("start > Start the communication for printing"));
  };

  // for Arduino Micro and Leonardo only
  #if defined (__AVR_ATmega32U4__)

  HkxPrint(Serial_& serialPrint, long baudrate) : _errorMessages(true), _warningMessages(false), _infoMessages(false), _serialPrint(&serialPrint){
    serialPrint.begin(baudrate);
    infoPrint(F("start > Start the communication for printing"));
  };

  HkxPrint(Serial_& serialPrint, long baudrate, bool errorMessages, bool warningMessages, bool infoMessages) : _errorMessages(errorMessages), _warningMessages(warningMessages), _infoMessages(infoMessages), _serialPrint(&serialPrint){
    serialPrint.begin(baudrate);
    infoPrint(F("start > Start the communication for printing"));
  };
  #endif

  HkxPrint(HkxPrint hkxPrint, bool errorMessages, bool warningMessages, bool infoMessages) : _errorMessages(errorMessages), _warningMessages(warningMessages), _infoMessages(infoMessages), _serialPrint(hkxPrint._serialPrint){};
  
  void errorPrint(const String& message) const {if(_errorMessages){_serialPrint->println(String(F("Error:   ")) + message);}};

  void warningPrint(const String& message) const {if(_warningMessages){_serialPrint->println(String(F("Warning: ")) + message);}};

  void infoPrint(const String& message) const {if(_infoMessages){_serialPrint->println(String(F("Info:    ")) + message);}};
  const boolean _errorMessages;      
  const boolean _warningMessages;    
  const boolean _infoMessages;
  Stream* const _serialPrint;
   
};

class HkxUnitConversion {
public:
  static int16_t voltageRawToValue(uint8_t voltageRaw){return (uint16_t)voltageRaw*74;};

  static uint8_t voltageValueToRaw(int16_t voltageValue){return (uint8_t)(voltageValue/74);};

  static int16_t temperatureRawToValue(uint8_t temperatureRaw){return temperatureConversion[temperatureRaw];};

  static uint8_t temperatureValueToRaw(int16_t temperatureValue){
    uint8_t temperatureRaw = 128;
    uint8_t sup = 254;
    uint8_t inf = 0;
    while((temperatureConversion[temperatureRaw] != temperatureValue) && ((sup - inf) > 1) ){ //dichotomy
      if(temperatureConversion[temperatureRaw] > temperatureValue) {sup = temperatureRaw;}
      else {inf = temperatureRaw;}
      temperatureRaw = (sup + inf)/2;
    }
    return temperatureRaw; 
  }; // lower value approximation

  static int16_t angleRawToValue(int16_t angleRaw){return ((int32_t)angleRaw)*326/100;};

  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

  static int16_t positionRawToValue(uint16_t positionRaw){return angleRawToValue((int16_t)positionRaw - 512);};

  static uint16_t positionValueToRaw(int16_t positionValue){return min(1023,max(0,angleValueToRaw(simpleAngle(positionValue)) + 512));};

  static int16_t simpleAngle(int16_t angle){return ((angle + 1800) % 3600) - 1800;};

  static int16_t velocityRawToValue(int16_t velocityRaw){return ((int32_t)velocityRaw)*2909/100;};

  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

  static uint16_t timeRawToValue(uint8_t timeRaw){return ((uint16_t)timeRaw*112)/10;};

  static uint8_t timeValueToRaw(uint16_t timeValue){return (uint8_t)((timeValue*10)/112)+1;}; // +1 because of the rounding

  static uint16_t slopeRawToValue(uint16_t slopeRaw){return ((uint32_t)(slopeRaw?slopeRaw:32768)*1000)/835;};

  static uint16_t slopeValueToRaw(uint16_t slopeValue){
    uint16_t slopeRaw = (((uint32_t)slopeValue*835)/1000) + 1; // +1 because of the rounding (+ avoid 0 reserved for no slope)
    return slopeRaw>32767?0:slopeRaw;
  };

  static hkxBaudrate baudrateRawToValue(uint8_t baudrateRaw){
    switch (baudrateRaw){
    case 0x02 : return HKX_666666;
    case 0x03 : return HKX_500000;
    case 0x04 : return HKX_400000;
    case 0x07 : return HKX_250000;
    case 0x09 : return HKX_200000;
    case 0x10 : return HKX_115200;
    case 0x22 : return HKX_57600; }
  };

  static uint8_t baudrateValueToRaw(hkxBaudrate baudrate){
    switch (baudrate){
    case HKX_666666 : return 0x02;
    case HKX_500000 : return 0x03;
    case HKX_400000 : return 0x04;
    case HKX_250000 : return 0x07;
    case HKX_200000 : return 0x09;
    case HKX_115200 : return 0x10;
    case HKX_57600  : return 0x22; }
  };
};

class HkxCommunication {
private:
  uint8_t _packetRead[HKX_SERIAL_BUFFER]; 
  HardwareSerial& _serialServos; 
  HkxPrint& _print; 
  const String _className; 

  void sendPacket(uint8_t ID, hkxCommand CMD, const uint8_t data[], uint8_t dataLength);

  uint8_t receivePacket(uint8_t& dataLength);

  uint8_t readPacket(uint8_t& ID, hkxCommand& CMD, uint8_t data[], uint8_t dataLength, HkxMaybe<HkxStatus> statusED);

  uint8_t checkSum1(uint8_t packetSize, uint8_t ID, hkxCommand CMD, const uint8_t data[], uint8_t dataLength){
    uint8_t cks = packetSize ^ ID ^ CMD;
    for(int i = 0 ; i < dataLength ; i++){
      cks = cks ^ data[i];
    }
    return cks & 0xFE;
  };

  uint8_t checkSum2(uint8_t checkSum1){return (~checkSum1) & 0xFE;};
  
  void reinitPacketRead(){memset(_packetRead,0,HKX_SERIAL_BUFFER);};

  void cleanSerialBuffer(){while(this->_serialServos.available()){this->_serialServos.read();}};

  void errorPrint(const String& message){if(_print._errorMessages){_print.errorPrint(_className + message);}};

  void warningPrint(const String& message){if(_print._errorMessages){_print.warningPrint(_className + message);}};

  void infoPrint(const String& message){if(_print._errorMessages){_print.infoPrint(_className + message);}};

public:

  HkxCommunication(hkxBaudrate baudrate, HardwareSerial& serialServos, HkxPrint& print);

  uint8_t readRequest(uint8_t ID, hkxMemory mem, uint8_t start, uint8_t length, uint8_t returnData[], HkxMaybe<HkxStatus> statusED);

  uint8_t writeRequest(uint8_t ID, hkxMemory mem, uint8_t start, uint8_t length, const uint8_t writeData[]);

  uint8_t statusRequest(uint8_t ID, HkxStatus& statusED);

  uint8_t iJogRequest(uint8_t length, const uint16_t jog[], const uint8_t set[], const uint8_t ID[], const uint8_t playTime[]);

  uint8_t sJogRequest(uint8_t playTime, uint8_t length, const uint16_t jog[], const uint8_t set[], const uint8_t ID[]);

  void start(hkxBaudrate newBaudrate){
    _serialServos.begin(newBaudrate);
    infoPrint(F("start > Start the communication with the servos"));
  }

  void stop(){
    this->_serialServos.end();
    infoPrint(F("stop > Stop the communication with the servos"));
  }

  uint8_t rebootRequest(uint8_t ID){
    if(HKX_DEV && ID > 0xFE){
      errorPrint(F("rebootRequest > input values not correct"));
      return 1;
    }
    uint8_t sData[0];
    sendPacket(ID, HKX_CMD_REBOOT, sData, 0);  
    return 0;
  };

  uint8_t rollbackRequest(uint8_t ID, boolean IDskip, boolean bandSkip){
    if(HKX_DEV && ID > 0xFE){
      errorPrint(F("rollbackRequest > input values not correct"));
      return 1;
    }
    uint8_t data[] = {IDskip?(uint8_t)1:(uint8_t)0 , bandSkip?(uint8_t)1:(uint8_t)0};
    sendPacket(ID, HKX_CMD_ROLLBACK, data, 2);
    return 0;
  };

  HardwareSerial& getSerial(){return _serialServos;};
};


#endif    // _HKXCOMMUNICATION_H