Provide REG/CMD macros to hide 'ugly' conversion function 'toVal'.

Author: aentinger

src/lib/motors/SmartServo.cpp

@@ -60,7 +60,7 @@ void SmartServoClass::writeCmd(uint8_t const id, SmartServoOperation const instr
 void SmartServoClass::writeByteCmd(uint8_t const id, uint8_t const address, uint8_t const data) {
   _txPacket.id = id;
   _txPacket.length = 2+2;
-  _txPacket.instruction = toVal(SmartServoOperation::WRITE);
+  _txPacket.instruction = CMD(SmartServoOperation::WRITE);
   _txPacket.payload[0]=address;
   _txPacket.payload[1]=data;
   sendPacket();
@@ -69,7 +69,7 @@ void SmartServoClass::writeByteCmd(uint8_t const id, uint8_t const address, uint
 void SmartServoClass::writeWordCmd(uint8_t const id, uint8_t const address, uint16_t const data) {
   _txPacket.id = id;
   _txPacket.length = 2+3;
-  _txPacket.instruction = toVal(SmartServoOperation::WRITE);
+  _txPacket.instruction = CMD(SmartServoOperation::WRITE);
   _txPacket.payload[0]=address;
   _txPacket.payload[1]=data>>8;
   _txPacket.payload[2]=data;
@@ -93,7 +93,7 @@ void SmartServoClass::receiveResponse(int const howMany) {
 int SmartServoClass::readBuffer(uint8_t const id, uint8_t const address,uint8_t const len) {
   _txPacket.id = id;
   _txPacket.length = 2+2;
-  _txPacket.instruction = toVal(SmartServoOperation::READ);
+  _txPacket.instruction = CMD(SmartServoOperation::READ);
   _txPacket.payload[0]=address;
   _txPacket.payload[1]=len;
   sendPacket();
@@ -171,8 +171,8 @@ int SmartServoClass::begin() {
     _txPacket.header[1] = 0xff;
     _RS485.begin(115200, 0, 90);
     _RS485.receive();
-    writeByteCmd(BROADCAST, toVal(SmartServoRegister::SERVO_MOTOR_MODE), 1);
-    writeByteCmd(BROADCAST, toVal(SmartServoRegister::TORQUE_SWITCH) ,1);
+    writeByteCmd(BROADCAST, REG(SmartServoRegister::SERVO_MOTOR_MODE), 1);
+    writeByteCmd(BROADCAST, REG(SmartServoRegister::TORQUE_SWITCH) ,1);
     _positionMode = PositionMode::IMMEDIATE;
     return 0;
   } else {
@@ -186,7 +186,7 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t
     _targetPosition[id] = angleToPosition(angle);
     _targetSpeed[id] = speed;
     if (_positionMode==PositionMode::IMMEDIATE) {
-      writeWordCmd(id, toVal(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle));
+      writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle));
     }
   }  
   mutex.unlock();
@@ -196,24 +196,24 @@ float SmartServoClass::getPosition(uint8_t const id) {
   mutex.lock();
   float ret = -1;
   if (id<MAX_MOTORS) {
-    ret = positionToAngle(readWordCmd(id, toVal(SmartServoRegister::POSITION_H)));
+    ret = positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H)));
   }
   mutex.unlock();
   return ret;
 }
 
 void SmartServoClass::center(uint8_t const id, uint16_t const position) {
   mutex.lock();
-  writeWordCmd(id, toVal(SmartServoRegister::CENTER_POINT_ADJ_H), position);
+  writeWordCmd(id, REG(SmartServoRegister::CENTER_POINT_ADJ_H), position);
   mutex.unlock();
 }
 
 void SmartServoClass::synchronize() {
   mutex.lock();
   _txPacket.id = 0xFE;
   _txPacket.length = (4+1)*MAX_MOTORS +4;
-  _txPacket.instruction = toVal(SmartServoOperation::SYNC_WRITE);
-  _txPacket.payload[0] = toVal(SmartServoRegister::TARGET_POSITION_H);
+  _txPacket.instruction = CMD(SmartServoOperation::SYNC_WRITE);
+  _txPacket.payload[0] = REG(SmartServoRegister::TARGET_POSITION_H);
   _txPacket.payload[1] = 4;
   int index = 2;
 
@@ -230,92 +230,92 @@ void SmartServoClass::synchronize() {
 
 void  SmartServoClass::setTorque(bool const torque) {
   mutex.lock();
-  writeByteCmd(BROADCAST, toVal(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0);
+  writeByteCmd(BROADCAST, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0);
   mutex.unlock();
 }
 
 void  SmartServoClass::setTorque(uint8_t const id, bool const torque) {
   mutex.lock();
-  writeByteCmd(id, toVal(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0);
+  writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0);
   mutex.unlock();
 }
 
 void  SmartServoClass::setTime(uint8_t const id, uint16_t const time) {
   mutex.lock();
-  writeWordCmd(id, toVal(SmartServoRegister::RUN_TIME_H), time);
+  writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time);
   mutex.unlock();
 }
 
 void  SmartServoClass::setMaxTorque(uint16_t const torque) {
   mutex.lock();
-  writeWordCmd(BROADCAST, toVal(SmartServoRegister::MAX_TORQUE_H), torque);
+  writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_TORQUE_H), torque);
   mutex.unlock();
 }
 
 void  SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const torque) {
   mutex.lock();
-  writeWordCmd(id+1, toVal(SmartServoRegister::MAX_TORQUE_H), torque);
+  writeWordCmd(id+1, REG(SmartServoRegister::MAX_TORQUE_H), torque);
   mutex.unlock();
 }
 
 void  SmartServoClass::setID(uint8_t const id) {
   mutex.lock();
-  writeByteCmd(BROADCAST, toVal(SmartServoRegister::ID), id);
+  writeByteCmd(BROADCAST, REG(SmartServoRegister::ID), id);
   mutex.unlock();
 }
 
 void  SmartServoClass::engage(uint8_t const id) {
   mutex.lock();
-  writeByteCmd(id, toVal(SmartServoRegister::TORQUE_SWITCH), 0x1);
+  writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), 0x1);
   mutex.unlock();
 }
 
 void  SmartServoClass::disengage(uint8_t const id) {
   mutex.lock();
-  writeByteCmd(id, toVal(SmartServoRegister::TORQUE_SWITCH), 0);
+  writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), 0);
   mutex.unlock();
 }
 
 bool SmartServoClass::isEngaged(uint8_t const id) {
   mutex.lock();
-  int ret = readByteCmd(id, toVal(SmartServoRegister::TORQUE_SWITCH));
+  int ret = readByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH));
   mutex.unlock();
   return ret != 0;
 }
 
 void  SmartServoClass::setStallProtectionTime(uint8_t const time) {
   mutex.lock();
-  writeByteCmd(BROADCAST, toVal(SmartServoRegister::STALL_PROTECTION_TIME), time);
+  writeByteCmd(BROADCAST, REG(SmartServoRegister::STALL_PROTECTION_TIME), time);
   mutex.unlock();
 }
 
 void  SmartServoClass::setStallProtectionTime(uint8_t const id, uint8_t const time) {
   mutex.lock();
-  writeByteCmd(id, toVal(SmartServoRegister::STALL_PROTECTION_TIME), time);
+  writeByteCmd(id, REG(SmartServoRegister::STALL_PROTECTION_TIME), time);
   mutex.unlock();
 }
 
 void  SmartServoClass::setMinAngle(float const angle) {
   mutex.lock();
-  writeByteCmd(BROADCAST, toVal(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle));
+  writeByteCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle));
   mutex.unlock();
 }
 
 void  SmartServoClass::setMinAngle(uint8_t const id, float const angle) {
   mutex.lock();
-  writeByteCmd(id, toVal(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle));
+  writeByteCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle));
   mutex.unlock();
 }
 
 void  SmartServoClass::setMaxAngle(float const angle) {
   mutex.lock();
-  writeByteCmd(BROADCAST, toVal(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle));
+  writeByteCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle));
   mutex.unlock();
 }
 
 void  SmartServoClass::setMaxAngle(uint8_t const id, float const angle) {
   mutex.lock();
-  writeByteCmd(id, toVal(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle));
+  writeByteCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle));
   mutex.unlock();
 }
 

src/lib/motors/SmartServoConst.h

@@ -92,4 +92,11 @@ constexpr auto toVal(Enumeration const value) -> typename std::underlying_type<E
   return static_cast<typename std::underlying_type<Enumeration>::type>(value);
 }
 
+/**************************************************************************************
+ * MACROS
+ **************************************************************************************/
+
+#define REG(enum_val) toVal(enum_val)
+#define CMD(enum_val) toVal(enum_val)
+
 #endif /* SMART_SERVO_CONST_H_ */