From 5007a3780e4d122e3f41051bef7f04beb69a263d Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 11 Jan 2022 13:02:52 +0100 Subject: [PATCH 01/21] Deleting superfluous file ArduinoRS485.h. --- src/lib/motors/ArduinoRS485.h | 25 ------------------------- src/lib/motors/SmartServo.h | 2 +- 2 files changed, 1 insertion(+), 26 deletions(-) delete mode 100644 src/lib/motors/ArduinoRS485.h diff --git a/src/lib/motors/ArduinoRS485.h b/src/lib/motors/ArduinoRS485.h deleted file mode 100644 index d24771f..0000000 --- a/src/lib/motors/ArduinoRS485.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - This file is part of the ArduinoRS485 library. - Copyright (c) 2018 Arduino SA. All rights reserved. - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _ARDUINO_RS485_H_INCLUDED -#define _ARDUINO_RS485_H_INCLUDED - -#include "RS485.h" - -#endif diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 0becdba..897732a 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -2,7 +2,7 @@ #define _SMARTMOTOR_H_ #include -#include "ArduinoRS485.h" +#include "RS485.h" typedef enum { pmIMMEDIATE, From 20e1d008f98b2ffd1b060f888b40994a66fdf097 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 11 Jan 2022 13:07:53 +0100 Subject: [PATCH 02/21] Turning SmartServo from a template class in a normal C++ class since the number of motors is known (and fixed). --- src/Braccio++.h | 10 +-- .../{SmartServo.cpp.impl => SmartServo.cpp} | 76 ++++++++++--------- src/lib/motors/SmartServo.h | 6 +- 3 files changed, 47 insertions(+), 45 deletions(-) rename src/lib/motors/{SmartServo.cpp.impl => SmartServo.cpp} (61%) diff --git a/src/Braccio++.h b/src/Braccio++.h index e85d931..4823936 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -38,7 +38,7 @@ using namespace std::chrono; class MotorsWrapper { public: - MotorsWrapper(SmartServoClass<7>* servos, int idx) : _servos(servos), _idx(idx) {} + MotorsWrapper(SmartServoClass* servos, int idx) : _servos(servos), _idx(idx) {} MotorsWrapper& to(float angle) { _servos->setPosition(_idx, angle, _speed); return *this; @@ -73,7 +73,7 @@ class MotorsWrapper { } private: - SmartServoClass<7>* _servos; + SmartServoClass* _servos; int _idx; int _speed = 100; }; @@ -132,11 +132,11 @@ class BraccioClass { runTime = speed_grade; } - void disengage(int id = SmartServoClass<7>::BROADCAST) { + void disengage(int id = SmartServoClass::BROADCAST) { servos->disengage(id); } - void engage(int id = SmartServoClass<7>::BROADCAST) { + void engage(int id = SmartServoClass::BROADCAST) { servos->engage(id); } @@ -170,7 +170,7 @@ class BraccioClass { private: RS485Class serial485 = RS485Class(Serial1, 0, 7, 8); // TX, DE, RE - SmartServoClass<7>* servos = new SmartServoClass<7>(serial485); + SmartServoClass* servos = new SmartServoClass(serial485); PD_UFP_log_c PD_UFP = PD_UFP_log_c(PD_LOG_LEVEL_VERBOSE); TCA6424A expander = TCA6424A(TCA6424A_ADDRESS_ADDR_HIGH); diff --git a/src/lib/motors/SmartServo.cpp.impl b/src/lib/motors/SmartServo.cpp similarity index 61% rename from src/lib/motors/SmartServo.cpp.impl rename to src/lib/motors/SmartServo.cpp index 31ce6ae..8ba69a3 100644 --- a/src/lib/motors/SmartServo.cpp.impl +++ b/src/lib/motors/SmartServo.cpp @@ -1,8 +1,10 @@ #include -template SmartServoClass::SmartServoClass( RS485Class& RS485) : _RS485(RS485) {} +#include "SmartServo.h" -template int SmartServoClass::calcChecksum() { +SmartServoClass::SmartServoClass( RS485Class& RS485) : _RS485(RS485) {} + +int SmartServoClass::calcChecksum() { char csum =0xff-_txPacket.id-_txPacket.length-_txPacket.instruction; int i=0; for (i=0;(i<(_txPacket.length-2))&&(i<(MAX_TX_PAYLOAD_LEN-1));i++) { @@ -12,7 +14,7 @@ template int SmartServoClass::calcChecksum() { return i+6; } -template void SmartServoClass::sendPacket() +void SmartServoClass::sendPacket() { char *buffer = (char *) &_txPacket; int len = calcChecksum(); @@ -33,14 +35,14 @@ template void SmartServoClass::sendPacket() } -template void SmartServoClass::writeCmd(uint8_t id,uint8_t instruction) { +void SmartServoClass::writeCmd(uint8_t id,uint8_t instruction) { _txPacket.id = id; _txPacket.length = 2; _txPacket.instruction = instruction; sendPacket(); } -template void SmartServoClass::writeByteCmd(uint8_t id,uint8_t address, uint8_t data) { +void SmartServoClass::writeByteCmd(uint8_t id,uint8_t address, uint8_t data) { _txPacket.id = id; _txPacket.length = 2+2; _txPacket.instruction = OP_WRITE; @@ -49,7 +51,7 @@ template void SmartServoClass::writeByteCmd(uint8_t sendPacket(); } -template void SmartServoClass::writeWordCmd(uint8_t id, uint8_t address, uint16_t data) { +void SmartServoClass::writeWordCmd(uint8_t id, uint8_t address, uint16_t data) { _txPacket.id = id; _txPacket.length = 2+3; _txPacket.instruction = OP_WRITE; @@ -59,7 +61,7 @@ template void SmartServoClass::writeWordCmd(uint8_t sendPacket(); } -template void SmartServoClass::receiveResponse(int howMany) { +void SmartServoClass::receiveResponse(int howMany) { _rxLen=0; memset(_rxBuf, 0, sizeof(_rxBuf)); long start = millis(); @@ -73,7 +75,7 @@ template void SmartServoClass::receiveResponse(int } } -template int SmartServoClass::readBuffer(uint8_t id, uint8_t address,uint8_t len) { +int SmartServoClass::readBuffer(uint8_t id, uint8_t address,uint8_t len) { _txPacket.id = id; _txPacket.length = 2+2; _txPacket.instruction = OP_READ; @@ -94,21 +96,21 @@ template int SmartServoClass::readBuffer(uint8_t id } -template int SmartServoClass::readWordCmd(uint8_t id, uint8_t address) { +int SmartServoClass::readWordCmd(uint8_t id, uint8_t address) { if (readBuffer(id,address,2) == 0) { return (_rxBuf[5]<<8)|_rxBuf[6]; } return -1; } -template int SmartServoClass::readByteCmd(uint8_t id, uint8_t address) { +int SmartServoClass::readByteCmd(uint8_t id, uint8_t address) { if (readBuffer(id,address,1) == 0) { return _rxBuf[5]; } return -1; } -template int SmartServoClass::ping(uint8_t id) { +int SmartServoClass::ping(uint8_t id) { mutex.lock(); writeCmd(id, OP_PING); // TODO: check return @@ -131,20 +133,20 @@ template int SmartServoClass::ping(uint8_t id) { /* // ATTENTION: RESET also changes the ID of the motor -template void SmartServoClass::reset(uint8_t id) { +void SmartServoClass::reset(uint8_t id) { mutex.lock(); writeCmd(id, OP_RESET); mutex.unlock(); } */ -template void SmartServoClass::action(uint8_t id) { +void SmartServoClass::action(uint8_t id) { mutex.lock(); writeCmd(id, OP_ACTION); mutex.unlock(); } -template int SmartServoClass::begin() { +int SmartServoClass::begin() { if (_RS485) { _txPacket.header[0] = 0xff; _txPacket.header[1] = 0xff; @@ -159,19 +161,19 @@ template int SmartServoClass::begin() { } } -template void SmartServoClass::setPositionMode(positionMode mode) { +void SmartServoClass::setPositionMode(positionMode mode) { _positionMode = mode; } -template uint16_t SmartServoClass::angleToPosition (float angle) { +uint16_t SmartServoClass::angleToPosition (float angle) { return angle*MAX_POSITION/360.0; } -template float SmartServoClass::positionToAngle (uint16_t position) { +float SmartServoClass::positionToAngle (uint16_t position) { return (360.0*position)/MAX_POSITION; } -template void SmartServoClass::setPosition(uint8_t id, float angle, uint16_t speed) { +void SmartServoClass::setPosition(uint8_t id, float angle, uint16_t speed) { mutex.lock(); if (id void SmartServoClass::setPosition(uint8_t mutex.unlock(); } -template float SmartServoClass::getPosition(uint8_t id) { +float SmartServoClass::getPosition(uint8_t id) { mutex.lock(); float ret = -1; if (id float SmartServoClass::getPosition(uint8_t return ret; } -template void SmartServoClass::center(uint8_t id, uint16_t position) { +void SmartServoClass::center(uint8_t id, uint16_t position) { mutex.lock(); writeWordCmd(id, CENTER_POINT_ADJ_H, position); mutex.unlock(); } -template void SmartServoClass::synchronize() { +void SmartServoClass::synchronize() { mutex.lock(); _txPacket.id = 0xFE; _txPacket.length = (4+1)*MAX_MOTORS +4; @@ -219,98 +221,98 @@ template void SmartServoClass::synchronize() { mutex.unlock(); } -template void SmartServoClass::setTorque(bool torque) { +void SmartServoClass::setTorque(bool torque) { mutex.lock(); writeByteCmd(BROADCAST,TORQUE_SWITCH,torque ? 1 : 0); mutex.unlock(); } -template void SmartServoClass::setTorque(uint8_t id, bool torque) { +void SmartServoClass::setTorque(uint8_t id, bool torque) { mutex.lock(); writeByteCmd(id,TORQUE_SWITCH,torque ? 1 : 0); mutex.unlock(); } -template void SmartServoClass::setTime(uint8_t id, uint16_t time) { +void SmartServoClass::setTime(uint8_t id, uint16_t time) { mutex.lock(); writeWordCmd(id, RUN_TIME_H, time); mutex.unlock(); } -template void SmartServoClass::setMaxTorque(uint16_t torque) { +void SmartServoClass::setMaxTorque(uint16_t torque) { mutex.lock(); writeWordCmd(BROADCAST,MAX_TORQUE_H,torque ); mutex.unlock(); } -template void SmartServoClass::setMaxTorque(uint8_t id, uint16_t torque) { +void SmartServoClass::setMaxTorque(uint8_t id, uint16_t torque) { mutex.lock(); writeWordCmd(id+1,MAX_TORQUE_H,torque ); mutex.unlock(); } -template void SmartServoClass::setID(uint8_t id) { +void SmartServoClass::setID(uint8_t id) { mutex.lock(); writeByteCmd(BROADCAST,ID,id); mutex.unlock(); } -template void SmartServoClass::engage(uint8_t id) { +void SmartServoClass::engage(uint8_t id) { mutex.lock(); writeByteCmd(id,TORQUE_SWITCH, 0x1); mutex.unlock(); } -template void SmartServoClass::disengage(uint8_t id) { +void SmartServoClass::disengage(uint8_t id) { mutex.lock(); writeByteCmd(id,TORQUE_SWITCH, 0); mutex.unlock(); } -template bool SmartServoClass::isEngaged(uint8_t id) { +bool SmartServoClass::isEngaged(uint8_t id) { mutex.lock(); int ret = readByteCmd(id,TORQUE_SWITCH); mutex.unlock(); return ret != 0; } -template void SmartServoClass::setStallProtectionTime(uint8_t time) { +void SmartServoClass::setStallProtectionTime(uint8_t time) { mutex.lock(); writeByteCmd(BROADCAST,STALL_PROTECTION_TIME,time); mutex.unlock(); } -template void SmartServoClass::setStallProtectionTime(uint8_t id, uint8_t time) { +void SmartServoClass::setStallProtectionTime(uint8_t id, uint8_t time) { mutex.lock(); writeByteCmd(id,STALL_PROTECTION_TIME,time); mutex.unlock(); } -template void SmartServoClass::setMinAngle(float angle) { +void SmartServoClass::setMinAngle(float angle) { mutex.lock(); writeByteCmd(BROADCAST,MIN_ANGLE_LIMIT_H,angleToPosition(angle)); mutex.unlock(); } -template void SmartServoClass::setMinAngle(uint8_t id, float angle) { +void SmartServoClass::setMinAngle(uint8_t id, float angle) { mutex.lock(); writeByteCmd(id,MIN_ANGLE_LIMIT_H,angleToPosition(angle)); mutex.unlock(); } -template void SmartServoClass::setMaxAngle(float angle) { +void SmartServoClass::setMaxAngle(float angle) { mutex.lock(); writeByteCmd(BROADCAST,MAX_ANGLE_LIMIT_H,angleToPosition(angle)); mutex.unlock(); } -template void SmartServoClass::setMaxAngle(uint8_t id, float angle) { +void SmartServoClass::setMaxAngle(uint8_t id, float angle) { mutex.lock(); writeByteCmd(id,MAX_ANGLE_LIMIT_H,angleToPosition(angle)); mutex.unlock(); } -template void SmartServoClass::getInfo(Stream& stream, uint8_t id) { +void SmartServoClass::getInfo(Stream& stream, uint8_t id) { uint8_t regs[65]; memset(regs, 0x55, sizeof(regs)); int i = 0; diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 897732a..d0de0fb 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -2,6 +2,7 @@ #define _SMARTMOTOR_H_ #include +#include #include "RS485.h" typedef enum { @@ -9,7 +10,8 @@ typedef enum { pmSYNC } positionMode; -template +static int constexpr MAX_MOTORS = 6; + class SmartServoClass { public: @@ -184,6 +186,4 @@ class SmartServoClass rtos::Mutex mutex; }; -#include "SmartServo.cpp.impl" - #endif // _SMARTMOTOR_H_ \ No newline at end of file From f6e6be795a714ccb075c4a0d2be21a65a1812ee7 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 11 Jan 2022 13:14:43 +0100 Subject: [PATCH 03/21] Moving member variable initialisation from class body to constructor. --- src/Braccio++.h | 20 +++++++++++--------- src/Braccio.cpp | 13 +++++++++++++ 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/Braccio++.h b/src/Braccio++.h index 4823936..c8baf1f 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -78,9 +78,12 @@ class MotorsWrapper { int _speed = 100; }; -class BraccioClass { +class BraccioClass +{ public: - BraccioClass() {} + + BraccioClass(); + bool begin(voidFuncPtr customMenu = nullptr); // setters @@ -169,16 +172,15 @@ class BraccioClass { private: - RS485Class serial485 = RS485Class(Serial1, 0, 7, 8); // TX, DE, RE - SmartServoClass* servos = new SmartServoClass(serial485); - - PD_UFP_log_c PD_UFP = PD_UFP_log_c(PD_LOG_LEVEL_VERBOSE); - TCA6424A expander = TCA6424A(TCA6424A_ADDRESS_ADDR_HIGH); + RS485Class serial485; + SmartServoClass* servos; + PD_UFP_log_c PD_UFP; + TCA6424A expander; Backlight bl; - speed_grade_t runTime = SLOW; //ms + speed_grade_t runTime; //ms - voidFuncPtr _customMenu = nullptr; + voidFuncPtr _customMenu; const int BTN_LEFT = 3; const int BTN_RIGHT = 4; diff --git a/src/Braccio.cpp b/src/Braccio.cpp index 50b6cc5..b74eceb 100644 --- a/src/Braccio.cpp +++ b/src/Braccio.cpp @@ -15,6 +15,19 @@ void my_print( const char * dsc ) using namespace std::chrono_literals; +BraccioClass::BraccioClass() +: serial485{Serial1, 0, 7, 8} /* TX, DE, RE */ +, servos{new SmartServoClass(serial485)} +, PD_UFP{PD_LOG_LEVEL_VERBOSE} +, expander{TCA6424A_ADDRESS_ADDR_HIGH} +, bl{} +, runTime{SLOW} +, _customMenu{nullptr} + +{ + +} + bool BraccioClass::begin(voidFuncPtr customMenu) { Wire.begin(); From f1f6f775a3c9246603aa37bda942d8f36057bdd8 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 11 Jan 2022 13:18:06 +0100 Subject: [PATCH 04/21] There's no need to create an instance of SmartServoClass on the heap, it can be safely stored on the stack. --- src/Braccio++.h | 64 ++++++++++++++++++++++++------------------------- src/Braccio.cpp | 8 +++---- 2 files changed, 36 insertions(+), 36 deletions(-) diff --git a/src/Braccio++.h b/src/Braccio++.h index c8baf1f..2a8fcd5 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -38,42 +38,42 @@ using namespace std::chrono; class MotorsWrapper { public: - MotorsWrapper(SmartServoClass* servos, int idx) : _servos(servos), _idx(idx) {} + MotorsWrapper(SmartServoClass & servos, int idx) : _servos(servos), _idx(idx) {} MotorsWrapper& to(float angle) { - _servos->setPosition(_idx, angle, _speed); + _servos.setPosition(_idx, angle, _speed); return *this; } MotorsWrapper& in(std::chrono::milliseconds len) { - _servos->setTime(_idx, len.count()); + _servos.setTime(_idx, len.count()); return *this; } MotorsWrapper& move() { return *this; } float position() { - return _servos->getPosition(_idx); + return _servos.getPosition(_idx); } bool connected() { - return _servos->ping(_idx) == 0; + return _servos.ping(_idx) == 0; } operator bool() { return connected(); } void info(Stream& stream) { - _servos->getInfo(stream, _idx); + _servos.getInfo(stream, _idx); } void disengage() { - _servos->disengage(_idx); + _servos.disengage(_idx); } void engage() { - _servos->engage(_idx); + _servos.engage(_idx); } bool engaged() { - return _servos->isEngaged(_idx); + return _servos.isEngaged(_idx); } private: - SmartServoClass* _servos; + SmartServoClass & _servos; int _idx; int _speed = 100; }; @@ -95,36 +95,36 @@ class BraccioClass return move(joint_index); } void moveTo(int joint_index, int position) { - //servos->setPosition(joint_index, position, 100); + //servos.setPosition(joint_index, position, 100); } void moveTo(int joint_index, float angle) { - servos->setPosition(joint_index, angle, 100); + servos.setPosition(joint_index, angle, 100); } void moveTo(float a1, float a2, float a3, float a4, float a5, float a6) { - servos->setPositionMode(pmSYNC); - servos->setPosition(1, a1, runTime); - servos->setPosition(2, a2, runTime); - servos->setPosition(3, a3, runTime); - servos->setPosition(4, a4, runTime); - servos->setPosition(5, a5, runTime); - servos->setPosition(6, a6, runTime); - servos->synchronize(); - servos->setPositionMode(pmIMMEDIATE); + servos.setPositionMode(pmSYNC); + servos.setPosition(1, a1, runTime); + servos.setPosition(2, a2, runTime); + servos.setPosition(3, a3, runTime); + servos.setPosition(4, a4, runTime); + servos.setPosition(5, a5, runTime); + servos.setPosition(6, a6, runTime); + servos.synchronize(); + servos.setPositionMode(pmIMMEDIATE); } // getters void positions(float* buffer) { for (int i = 1; i < 7; i++) { - *buffer++ = servos->getPosition(i); + *buffer++ = servos.getPosition(i); } } void positions(float& a1, float& a2, float& a3, float& a4, float& a5, float& a6) { // TODO: add check if motors are actually connected - a1 = servos->getPosition(1); - a2 = servos->getPosition(2); - a3 = servos->getPosition(3); - a4 = servos->getPosition(4); - a5 = servos->getPosition(5); - a6 = servos->getPosition(6); + a1 = servos.getPosition(1); + a2 = servos.getPosition(2); + a3 = servos.getPosition(3); + a4 = servos.getPosition(4); + a5 = servos.getPosition(5); + a6 = servos.getPosition(6); } float position(int joint_index); bool connected(int joint_index) { @@ -136,11 +136,11 @@ class BraccioClass } void disengage(int id = SmartServoClass::BROADCAST) { - servos->disengage(id); + servos.disengage(id); } void engage(int id = SmartServoClass::BROADCAST) { - servos->engage(id); + servos.engage(id); } int getKey(); @@ -167,13 +167,13 @@ class BraccioClass void defaultMenu(); void setID(int id) { - servos->setID(id); + servos.setID(id); } private: RS485Class serial485; - SmartServoClass* servos; + SmartServoClass servos; PD_UFP_log_c PD_UFP; TCA6424A expander; Backlight bl; diff --git a/src/Braccio.cpp b/src/Braccio.cpp index b74eceb..ee15d5f 100644 --- a/src/Braccio.cpp +++ b/src/Braccio.cpp @@ -17,7 +17,7 @@ using namespace std::chrono_literals; BraccioClass::BraccioClass() : serial485{Serial1, 0, 7, 8} /* TX, DE, RE */ -, servos{new SmartServoClass(serial485)} +, servos{serial485} , PD_UFP{PD_LOG_LEVEL_VERBOSE} , expander{TCA6424A_ADDRESS_ADDR_HIGH} , bl{} @@ -156,8 +156,8 @@ bool BraccioClass::begin(voidFuncPtr customMenu) { display_th.start(mbed::callback(this, &BraccioClass::display_thread)); #endif - servos->begin(); - servos->setPositionMode(pmIMMEDIATE); + servos.begin(); + servos.setPositionMode(pmIMMEDIATE); #ifdef __MBED__ static rtos::Thread connected_th; @@ -240,7 +240,7 @@ void BraccioClass::motors_connected_thread() { while (1) { if (ping_allowed) { for (int i = 1; i < 7; i++) { - _connected[i] = (servos->ping(i) == 0); + _connected[i] = (servos.ping(i) == 0); //Serial.print(String(i) + ": "); //Serial.println(_connected[i]); } From 8bccedd721f054f6c44f825f67e72cfff3346bea Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 11 Jan 2022 13:23:47 +0100 Subject: [PATCH 05/21] Replacing enum 'positionMode' with enum class 'PositionMode' for increase type-safety and readability." --- src/Braccio++.h | 4 ++-- src/Braccio.cpp | 2 +- src/lib/motors/SmartServo.cpp | 6 +++--- src/lib/motors/SmartServo.h | 13 +++++++------ 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/Braccio++.h b/src/Braccio++.h index 2a8fcd5..acf3ee2 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -101,7 +101,7 @@ class BraccioClass servos.setPosition(joint_index, angle, 100); } void moveTo(float a1, float a2, float a3, float a4, float a5, float a6) { - servos.setPositionMode(pmSYNC); + servos.setPositionMode(PositionMode::SYNC); servos.setPosition(1, a1, runTime); servos.setPosition(2, a2, runTime); servos.setPosition(3, a3, runTime); @@ -109,7 +109,7 @@ class BraccioClass servos.setPosition(5, a5, runTime); servos.setPosition(6, a6, runTime); servos.synchronize(); - servos.setPositionMode(pmIMMEDIATE); + servos.setPositionMode(PositionMode::IMMEDIATE); } // getters void positions(float* buffer) { diff --git a/src/Braccio.cpp b/src/Braccio.cpp index ee15d5f..263d478 100644 --- a/src/Braccio.cpp +++ b/src/Braccio.cpp @@ -157,7 +157,7 @@ bool BraccioClass::begin(voidFuncPtr customMenu) { #endif servos.begin(); - servos.setPositionMode(pmIMMEDIATE); + servos.setPositionMode(PositionMode::IMMEDIATE); #ifdef __MBED__ static rtos::Thread connected_th; diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 8ba69a3..4596e0a 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -154,14 +154,14 @@ int SmartServoClass::begin() { _RS485.receive(); writeByteCmd(BROADCAST,SERVO_MOTOR_MODE,1); writeByteCmd(BROADCAST,TORQUE_SWITCH,1); - _positionMode = pmIMMEDIATE; + _positionMode = PositionMode::IMMEDIATE; return 0; } else { return -1; } } -void SmartServoClass::setPositionMode(positionMode mode) { +void SmartServoClass::setPositionMode(PositionMode mode) { _positionMode = mode; } @@ -178,7 +178,7 @@ void SmartServoClass::setPosition(uint8_t id, float angle, uint16_t speed) { if (id #include "RS485.h" -typedef enum { - pmIMMEDIATE, - pmSYNC -} positionMode; +enum class PositionMode +{ + IMMEDIATE, + SYNC +}; static int constexpr MAX_MOTORS = 6; @@ -20,7 +21,7 @@ class SmartServoClass int begin(); void end(); - void setPositionMode(positionMode mode); + void setPositionMode(PositionMode mode); void setPosition(uint8_t id, float angle, uint16_t speed); @@ -181,7 +182,7 @@ class SmartServoClass uint8_t _rxLen; uint16_t _targetPosition[MAX_MOTORS]; uint16_t _targetSpeed[MAX_MOTORS]; - positionMode _positionMode; + PositionMode _positionMode; rtos::Mutex mutex; }; From d555e32c0705a9a9517694c1d0956859d08fb06c Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 11 Jan 2022 13:27:20 +0100 Subject: [PATCH 06/21] Inline short functions. --- src/lib/motors/SmartServo.cpp | 12 ------------ src/lib/motors/SmartServo.h | 8 ++++---- 2 files changed, 4 insertions(+), 16 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 4596e0a..981dee1 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -161,18 +161,6 @@ int SmartServoClass::begin() { } } -void SmartServoClass::setPositionMode(PositionMode mode) { - _positionMode = mode; -} - -uint16_t SmartServoClass::angleToPosition (float angle) { - return angle*MAX_POSITION/360.0; -} - -float SmartServoClass::positionToAngle (uint16_t position) { - return (360.0*position)/MAX_POSITION; -} - void SmartServoClass::setPosition(uint8_t id, float angle, uint16_t speed) { mutex.lock(); if (id onError; From 7dcae143830504cc44bedc008f531002171ce3b0 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 06:42:35 +0100 Subject: [PATCH 07/21] Applying const-correctness is a first step towards detecting programmer error. --- src/lib/motors/SmartServo.cpp | 68 ++++++++++++++++-------------- src/lib/motors/SmartServo.h | 78 +++++++++++++++++------------------ 2 files changed, 73 insertions(+), 73 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 981dee1..76a4241 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -2,7 +2,11 @@ #include "SmartServo.h" -SmartServoClass::SmartServoClass( RS485Class& RS485) : _RS485(RS485) {} +SmartServoClass::SmartServoClass(RS485Class & RS485) +: _RS485{RS485} +{ + +} int SmartServoClass::calcChecksum() { char csum =0xff-_txPacket.id-_txPacket.length-_txPacket.instruction; @@ -35,14 +39,14 @@ void SmartServoClass::sendPacket() } -void SmartServoClass::writeCmd(uint8_t id,uint8_t instruction) { +void SmartServoClass::writeCmd(uint8_t const id, uint8_t const instruction) { _txPacket.id = id; _txPacket.length = 2; _txPacket.instruction = instruction; sendPacket(); } -void SmartServoClass::writeByteCmd(uint8_t id,uint8_t address, uint8_t data) { +void SmartServoClass::writeByteCmd(uint8_t const id, uint8_t const address, uint8_t const data) { _txPacket.id = id; _txPacket.length = 2+2; _txPacket.instruction = OP_WRITE; @@ -51,7 +55,7 @@ void SmartServoClass::writeByteCmd(uint8_t id,uint8_t address, uint8_t data) { sendPacket(); } -void SmartServoClass::writeWordCmd(uint8_t id, uint8_t address, uint16_t data) { +void SmartServoClass::writeWordCmd(uint8_t const id, uint8_t const address, uint16_t const data) { _txPacket.id = id; _txPacket.length = 2+3; _txPacket.instruction = OP_WRITE; @@ -61,7 +65,7 @@ void SmartServoClass::writeWordCmd(uint8_t id, uint8_t address, uint16_t data) { sendPacket(); } -void SmartServoClass::receiveResponse(int howMany) { +void SmartServoClass::receiveResponse(int const howMany) { _rxLen=0; memset(_rxBuf, 0, sizeof(_rxBuf)); long start = millis(); @@ -75,7 +79,7 @@ void SmartServoClass::receiveResponse(int howMany) { } } -int SmartServoClass::readBuffer(uint8_t id, uint8_t address,uint8_t len) { +int SmartServoClass::readBuffer(uint8_t const id, uint8_t const address,uint8_t const len) { _txPacket.id = id; _txPacket.length = 2+2; _txPacket.instruction = OP_READ; @@ -96,21 +100,21 @@ int SmartServoClass::readBuffer(uint8_t id, uint8_t address,uint8_t len) { } -int SmartServoClass::readWordCmd(uint8_t id, uint8_t address) { +int SmartServoClass::readWordCmd(uint8_t const id, uint8_t const address) { if (readBuffer(id,address,2) == 0) { return (_rxBuf[5]<<8)|_rxBuf[6]; } return -1; } -int SmartServoClass::readByteCmd(uint8_t id, uint8_t address) { +int SmartServoClass::readByteCmd(uint8_t const id, uint8_t const address) { if (readBuffer(id,address,1) == 0) { return _rxBuf[5]; } return -1; } -int SmartServoClass::ping(uint8_t id) { +int SmartServoClass::ping(uint8_t const id) { mutex.lock(); writeCmd(id, OP_PING); // TODO: check return @@ -133,14 +137,14 @@ int SmartServoClass::ping(uint8_t id) { /* // ATTENTION: RESET also changes the ID of the motor -void SmartServoClass::reset(uint8_t id) { +void SmartServoClass::reset(uint8_t const id) { mutex.lock(); writeCmd(id, OP_RESET); mutex.unlock(); } */ -void SmartServoClass::action(uint8_t id) { +void SmartServoClass::action(uint8_t const id) { mutex.lock(); writeCmd(id, OP_ACTION); mutex.unlock(); @@ -161,7 +165,7 @@ int SmartServoClass::begin() { } } -void SmartServoClass::setPosition(uint8_t id, float angle, uint16_t speed) { +void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed) { mutex.lock(); if (id _onError) { - onError = _onError; - } + inline void onErrorCb(mbed::Callback _onError) { onError = _onError; } - int getErrors() { - return errors; - } + inline int getErrors() const { return errors; } static const int BROADCAST = 0xFE; @@ -151,18 +147,18 @@ class SmartServoClass int calcChecksum (); void sendPacket (); - void writeCmd (uint8_t id,uint8_t instruction); - void writeByteCmd (uint8_t id,uint8_t address, uint8_t data); - void writeWordCmd (uint8_t id, uint8_t address, uint16_t data); - void receiveResponse (int howMany = MAX_RX_PAYLOAD_LEN); - int readBuffer (uint8_t id, uint8_t address,uint8_t len); - int readWordCmd (uint8_t id, uint8_t address); - int readByteCmd (uint8_t id, uint8_t address); - void action (uint8_t id); - void writeSyncCmd (uint8_t *id, uint8_t num, uint8_t address, uint8_t len, uint8_t * data); - - inline uint16_t angleToPosition(float angle) { return (angle*MAX_POSITION)/360.0; } - inline float positionToAngle(uint16_t position) { return (360.0*position)/MAX_POSITION; } + void writeCmd (uint8_t const id, uint8_t const instruction); + void writeByteCmd (uint8_t const id, uint8_t const address, uint8_t const data); + void writeWordCmd (uint8_t const id, uint8_t const address, uint16_t const data); + void receiveResponse (int const howMany = MAX_RX_PAYLOAD_LEN); + int readBuffer (uint8_t const id, uint8_t const address, uint8_t const len); + int readWordCmd (uint8_t const id, uint8_t const address); + int readByteCmd (uint8_t const id, uint8_t const address); + void action (uint8_t const id); + void writeSyncCmd (uint8_t *id, uint8_t const num, uint8_t const address, uint8_t const len, uint8_t const * data); + + inline uint16_t angleToPosition(float const angle) { return (angle*MAX_POSITION)/360.0; } + inline float positionToAngle(uint16_t const position) { return (360.0*position)/MAX_POSITION; } mbed::Callback onError; From 0be311989ed7265b80c25b13e47241943646ad01 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 06:48:06 +0100 Subject: [PATCH 08/21] Extracting enum SERVO_REG into separate file 'SmartServoConst.h'. This reduces the pollution of 'SmartServo.h'. --- src/lib/motors/SmartServo.cpp | 1 + src/lib/motors/SmartServo.h | 53 --------------------------- src/lib/motors/SmartServoConst.h | 62 ++++++++++++++++++++++++++++++++ 3 files changed, 63 insertions(+), 53 deletions(-) create mode 100644 src/lib/motors/SmartServoConst.h diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 76a4241..11a3c2c 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -1,6 +1,7 @@ #include #include "SmartServo.h" +#include "SmartServoConst.h" SmartServoClass::SmartServoClass(RS485Class & RS485) : _RS485{RS485} diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 158740a..deb980d 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -92,59 +92,6 @@ class SmartServoClass int timestamps[6 * 6] = {0}; - typedef enum { - _NI_MFG_YEAR, //0 (0x00) Servo factory: year read/write -- - _NI_MFG_MONTH, //1 (0x01) Servo factory: month read/write -- - _NI_MFG_DAY, //2 (0x02) Servo factory: date read/write -- - SWVER_H, //3 (0x03) Software version(H) read -- Software version - SWVER_L, //4 (0x04) Software version(L) read -- - ID, //5 (0x05) ID read/write 0x01 - STALL_PROTECTION_TIME, //6 (0x06) Stall protection time read/write 0x03 unit second (default 3s) - _NI_BOOT_TIMES_H, //7 (0x07) Boot times (H) read/write 0x00 Record the boot times - _NI_BOOT_TIMES_L, //8 (0x08) Boot times(L) read/write 0x00 - MIN_ANGLE_LIMIT_H, //9 (0x09) Minimum angle limit (H) read/write 0x00 (0) Default of Minimum angle limit is 0 - MIN_ANGLE_LIMIT_L, //10 (0x0A) Minimum angle limit (L) read/write 0x00 - MAX_ANGLE_LIMIT_H, //11 (0x0B) Maximum angle limit (H) read/write 0x10 (4000) Default of Maximum angle limit is 4000 - MAX_ANGLE_LIMIT_L, //12 (0x0C) Maximum angle limit (L) read/write 0x00 - _NI_HIGH_TEMP_LIMIT, //13 (0x0D) High temperature limit read/write 0x50(80°) - _NI_HIGH_VOLTAGE_LIMIT, //14 (0x0E) High Votage limit read/write 0x08 - _NI_LOW_VOLTAGE_LIMIT, //15 (0x0F) Low Votage limit read/write 0x04 - MAX_TORQUE_H, //16 (0x10) Maximum torque (H) read/write 0x03(800) The torque control range is 0-1000 (the greater the value, the greater the output torque) - MAX_TORQUE_L, //17 (0x11) Maximum torque (L) read/write 0x20 - _NI_SPEED_CONTROL, //18 (0x12) Speed control read/write 0X1E - _NI_UNLOAD_CONDITION, //19 (0x13) Unload condition read/write 0x00 - CENTER_POINT_ADJ_H, //20 (0x14) Center point adjustment(H) read/write 0x00 Center point of servo motor - CENTER_POINT_ADJ_L, //21 (0x15) Center point adjustment(L) read/write 0x00 - _NI_LED0_SWITCH, //22 (0x16) LED0 Switch read/write 0X00 0x01:on 0x00:off - _NI_LED1_SWITCH, //23 (0x17) LED1 Switch read/write 0X00 - _NI_LED2_SWITCH, //24 (0x18) LED2 Switch read/write 0X00 - _NI_LED0_ON_OFF_TIME, //25 (0x19) LED0 light on and off time read/write 0x32 (50ms) LED light on and off time interval - _NI_LED1_ON_OFF_TIME, //26 (0x1A) LED1 light on and off time read/write 0x32 (50ms) - _NI_LED2_ON_OFF_TIME, //27 (0x1B) LED3 light on and off time read/write 0x32 (50ms) - SERVO_MOTOR_MODE, //28 (0x1C) Servo/motor mode switch read/write 0xFF 0x01:servo mode 0x00:motor mode - MOTOR_DIRECTION, //29 (0x1D) Motor direction switch read/write 0xFF 0x01:clockwise 0x00:counter-clockwise - TORQUE_SWITCH = 40, //40 (0x28) Torque switch read/write 0x00:off non-zero:on - _NI_LED, //41 (0x29) LED read/write 0X00 - TARGET_POSITION_H, //42 (0x2A) Target position (H) read/write Set the target position of the servo - TARGET_POSITION_L, //43 (0x2B) Target position (L) read/write - RUN_TIME_H, //44 (0x2C) Run time (H) read/write unit: ms - RUN_TIME_L, //45 (0x2D) Run time (L) read/write - _NI_CURRENT_H, //46 (0x2E) Current (H) read - _NI_CURRENT_L, //47 (0x2F) Current (L) read - _NI_LOCK, //48 (0x30) Lock read/write 00 - POSITION_H = 56, //56 (0x38) Position (H) read Current position - POSITION_L, //57 (0x39) Position (L) read - SPEED_H, //58 (0x3A) Speed (H) read Run speed - SPEED_L, //59 (0x3B) Speed (L) read - _NI_LOADING_H, //60 (0x3C) Loading (H) read - _NI_LOADING_L, //61 (0x3D) Loading (L) read - _NI_VOLTAGE, //62 (0x3E) Votage read - _NI_TEMPERATURE, //63 (0x3F) Temperature read - REG_WRITE_SIGN, //64 (0x40) REG WRITE sign read 00 - SPEED_CONTROL_H, //65 (0x41) Speed control (H) read/write Low-order 8 bits of data actually used - SPEED_CONTROL_L //66 (0x42) Speed control (L) read/write - } SERVO_REG; - int calcChecksum (); void sendPacket (); void writeCmd (uint8_t const id, uint8_t const instruction); diff --git a/src/lib/motors/SmartServoConst.h b/src/lib/motors/SmartServoConst.h new file mode 100644 index 0000000..7d4ae74 --- /dev/null +++ b/src/lib/motors/SmartServoConst.h @@ -0,0 +1,62 @@ +#ifndef SMART_SERVO_CONST_H_ +#define SMART_SERVO_CONST_H_ + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +typedef enum +{ + _NI_MFG_YEAR = 0, //0 (0x00) Servo factory: year read/write -- + _NI_MFG_MONTH, //1 (0x01) Servo factory: month read/write -- + _NI_MFG_DAY, //2 (0x02) Servo factory: date read/write -- + SWVER_H, //3 (0x03) Software version(H) read -- Software version + SWVER_L, //4 (0x04) Software version(L) read -- + ID, //5 (0x05) ID read/write 0x01 + STALL_PROTECTION_TIME, //6 (0x06) Stall protection time read/write 0x03 unit second (default 3s) + _NI_BOOT_TIMES_H, //7 (0x07) Boot times (H) read/write 0x00 Record the boot times + _NI_BOOT_TIMES_L, //8 (0x08) Boot times(L) read/write 0x00 + MIN_ANGLE_LIMIT_H, //9 (0x09) Minimum angle limit (H) read/write 0x00 (0) Default of Minimum angle limit is 0 + MIN_ANGLE_LIMIT_L, //10 (0x0A) Minimum angle limit (L) read/write 0x00 + MAX_ANGLE_LIMIT_H, //11 (0x0B) Maximum angle limit (H) read/write 0x10 (4000) Default of Maximum angle limit is 4000 + MAX_ANGLE_LIMIT_L, //12 (0x0C) Maximum angle limit (L) read/write 0x00 + _NI_HIGH_TEMP_LIMIT, //13 (0x0D) High temperature limit read/write 0x50(80°) + _NI_HIGH_VOLTAGE_LIMIT, //14 (0x0E) High Votage limit read/write 0x08 + _NI_LOW_VOLTAGE_LIMIT, //15 (0x0F) Low Votage limit read/write 0x04 + MAX_TORQUE_H, //16 (0x10) Maximum torque (H) read/write 0x03(800) The torque control range is 0-1000 (the greater the value, the greater the output torque) + MAX_TORQUE_L, //17 (0x11) Maximum torque (L) read/write 0x20 + _NI_SPEED_CONTROL, //18 (0x12) Speed control read/write 0X1E + _NI_UNLOAD_CONDITION, //19 (0x13) Unload condition read/write 0x00 + CENTER_POINT_ADJ_H, //20 (0x14) Center point adjustment(H) read/write 0x00 Center point of servo motor + CENTER_POINT_ADJ_L, //21 (0x15) Center point adjustment(L) read/write 0x00 + _NI_LED0_SWITCH, //22 (0x16) LED0 Switch read/write 0X00 0x01:on 0x00:off + _NI_LED1_SWITCH, //23 (0x17) LED1 Switch read/write 0X00 + _NI_LED2_SWITCH, //24 (0x18) LED2 Switch read/write 0X00 + _NI_LED0_ON_OFF_TIME, //25 (0x19) LED0 light on and off time read/write 0x32 (50ms) LED light on and off time interval + _NI_LED1_ON_OFF_TIME, //26 (0x1A) LED1 light on and off time read/write 0x32 (50ms) + _NI_LED2_ON_OFF_TIME, //27 (0x1B) LED3 light on and off time read/write 0x32 (50ms) + SERVO_MOTOR_MODE, //28 (0x1C) Servo/motor mode switch read/write 0xFF 0x01:servo mode 0x00:motor mode + MOTOR_DIRECTION, //29 (0x1D) Motor direction switch read/write 0xFF 0x01:clockwise 0x00:counter-clockwise + TORQUE_SWITCH = 40, //40 (0x28) Torque switch read/write 0x00:off non-zero:on + _NI_LED, //41 (0x29) LED read/write 0X00 + TARGET_POSITION_H, //42 (0x2A) Target position (H) read/write Set the target position of the servo + TARGET_POSITION_L, //43 (0x2B) Target position (L) read/write + RUN_TIME_H, //44 (0x2C) Run time (H) read/write unit: ms + RUN_TIME_L, //45 (0x2D) Run time (L) read/write + _NI_CURRENT_H, //46 (0x2E) Current (H) read + _NI_CURRENT_L, //47 (0x2F) Current (L) read + _NI_LOCK, //48 (0x30) Lock read/write 00 + POSITION_H = 56, //56 (0x38) Position (H) read Current position + POSITION_L, //57 (0x39) Position (L) read + SPEED_H, //58 (0x3A) Speed (H) read Run speed + SPEED_L, //59 (0x3B) Speed (L) read + _NI_LOADING_H, //60 (0x3C) Loading (H) read + _NI_LOADING_L, //61 (0x3D) Loading (L) read + _NI_VOLTAGE, //62 (0x3E) Votage read + _NI_TEMPERATURE, //63 (0x3F) Temperature read + REG_WRITE_SIGN, //64 (0x40) REG WRITE sign read 00 + SPEED_CONTROL_H, //65 (0x41) Speed control (H) read/write Low-order 8 bits of data actually used + SPEED_CONTROL_L //66 (0x42) Speed control (L) read/write +} SmartServoRegister; + +#endif /* SMART_SERVO_CONST_H_ */ \ No newline at end of file From 7e4fd4c89ef619f05d1551be12f77440dded477a Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 06:56:02 +0100 Subject: [PATCH 09/21] Turning SmartServoConstants into an enum class prevents misusage, as enum automatically default to int. --- src/lib/motors/SmartServo.cpp | 42 ++++++++++++++++---------------- src/lib/motors/SmartServoConst.h | 20 +++++++++++++-- 2 files changed, 39 insertions(+), 23 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 11a3c2c..e86ff38 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -157,8 +157,8 @@ int SmartServoClass::begin() { _txPacket.header[1] = 0xff; _RS485.begin(115200, 0, 90); _RS485.receive(); - writeByteCmd(BROADCAST,SERVO_MOTOR_MODE,1); - writeByteCmd(BROADCAST,TORQUE_SWITCH,1); + writeByteCmd(BROADCAST, toVal(SmartServoRegister::SERVO_MOTOR_MODE), 1); + writeByteCmd(BROADCAST, toVal(SmartServoRegister::TORQUE_SWITCH) ,1); _positionMode = PositionMode::IMMEDIATE; return 0; } else { @@ -172,7 +172,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,TARGET_POSITION_H,angleToPosition(angle)); + writeWordCmd(id, toVal(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle)); } } mutex.unlock(); @@ -182,7 +182,7 @@ float SmartServoClass::getPosition(uint8_t const id) { mutex.lock(); float ret = -1; if (id + /************************************************************************************** * TYPEDEF **************************************************************************************/ -typedef enum +enum class SmartServoRegister : uint8_t { _NI_MFG_YEAR = 0, //0 (0x00) Servo factory: year read/write -- _NI_MFG_MONTH, //1 (0x01) Servo factory: month read/write -- @@ -57,6 +63,16 @@ typedef enum REG_WRITE_SIGN, //64 (0x40) REG WRITE sign read 00 SPEED_CONTROL_H, //65 (0x41) Speed control (H) read/write Low-order 8 bits of data actually used SPEED_CONTROL_L //66 (0x42) Speed control (L) read/write -} SmartServoRegister; +}; + +/************************************************************************************** + * CONVERSION FUNCTIONS + **************************************************************************************/ + +template +constexpr auto toVal(Enumeration const value) -> typename std::underlying_type::type +{ + return static_cast::type>(value); +} #endif /* SMART_SERVO_CONST_H_ */ \ No newline at end of file From 2b39743f8a2f2a7a4d0d44d9ae795ff983981d11 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 07:03:19 +0100 Subject: [PATCH 10/21] Extract servo operation commands into enum class. --- src/lib/motors/SmartServo.cpp | 14 +++++++------- src/lib/motors/SmartServo.h | 8 -------- src/lib/motors/SmartServoConst.h | 11 +++++++++++ 3 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index e86ff38..37a9422 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -50,7 +50,7 @@ void SmartServoClass::writeCmd(uint8_t const id, uint8_t const instruction) { void SmartServoClass::writeByteCmd(uint8_t const id, uint8_t const address, uint8_t const data) { _txPacket.id = id; _txPacket.length = 2+2; - _txPacket.instruction = OP_WRITE; + _txPacket.instruction = toVal(SmartServoOperation::WRITE); _txPacket.payload[0]=address; _txPacket.payload[1]=data; sendPacket(); @@ -59,7 +59,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 = OP_WRITE; + _txPacket.instruction = toVal(SmartServoOperation::WRITE); _txPacket.payload[0]=address; _txPacket.payload[1]=data>>8; _txPacket.payload[2]=data; @@ -83,7 +83,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 = OP_READ; + _txPacket.instruction = toVal(SmartServoOperation::READ); _txPacket.payload[0]=address; _txPacket.payload[1]=len; sendPacket(); @@ -117,7 +117,7 @@ int SmartServoClass::readByteCmd(uint8_t const id, uint8_t const address) { int SmartServoClass::ping(uint8_t const id) { mutex.lock(); - writeCmd(id, OP_PING); + writeCmd(id, toVal(SmartServoOperation::PING)); // TODO: check return receiveResponse(6); if (_rxLen==6 && @@ -140,14 +140,14 @@ int SmartServoClass::ping(uint8_t const id) { void SmartServoClass::reset(uint8_t const id) { mutex.lock(); - writeCmd(id, OP_RESET); + writeCmd(id, toVal(SmartServoOperation::RESET)); mutex.unlock(); } */ void SmartServoClass::action(uint8_t const id) { mutex.lock(); - writeCmd(id, OP_ACTION); + writeCmd(id, toVal(SmartServoOperation::ACTION)); mutex.unlock(); } @@ -198,7 +198,7 @@ void SmartServoClass::synchronize() { mutex.lock(); _txPacket.id = 0xFE; _txPacket.length = (4+1)*MAX_MOTORS +4; - _txPacket.instruction = OP_SYNC_WRITE; + _txPacket.instruction = toVal(SmartServoOperation::SYNC_WRITE); _txPacket.payload[0] = toVal(SmartServoRegister::TARGET_POSITION_H); _txPacket.payload[1] = 4; int index = 2; diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index deb980d..af333b6 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -80,14 +80,6 @@ class SmartServoClass static const int MAX_TX_PAYLOAD_LEN = (5*MAX_MOTORS+4); static const int MAX_RX_PAYLOAD_LEN = 10; - static const int OP_PING = 1; - static const int OP_READ = 2; - static const int OP_WRITE = 3; - static const int OP_REG_WRITE = 4; - static const int OP_ACTION = 5; - static const int OP_RESET = 6; - static const int OP_SYNC_WRITE = 0x83; - static const int MAX_POSITION = 4000; int timestamps[6 * 6] = {0}; diff --git a/src/lib/motors/SmartServoConst.h b/src/lib/motors/SmartServoConst.h index 3078508..dba13b8 100644 --- a/src/lib/motors/SmartServoConst.h +++ b/src/lib/motors/SmartServoConst.h @@ -65,6 +65,17 @@ enum class SmartServoRegister : uint8_t SPEED_CONTROL_L //66 (0x42) Speed control (L) read/write }; +enum class SmartServoOperation : uint8_t +{ + PING = 1, + READ = 2, + WRITE = 3, + REG_WRITE = 4, + ACTION = 5, + RESET = 6, + SYNC_WRITE = 0x83, +}; + /************************************************************************************** * CONVERSION FUNCTIONS **************************************************************************************/ From 49263cbc5e6ff8199b3a328b28db75bbe8460314 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 07:06:27 +0100 Subject: [PATCH 11/21] Replacing uint8_t with enum class SmartServoOperation ensures that this API can only be used with a defined servo operation command. --- src/lib/motors/SmartServo.cpp | 11 +++++------ src/lib/motors/SmartServo.h | 4 +++- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 37a9422..94d2433 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -1,7 +1,6 @@ #include #include "SmartServo.h" -#include "SmartServoConst.h" SmartServoClass::SmartServoClass(RS485Class & RS485) : _RS485{RS485} @@ -40,10 +39,10 @@ void SmartServoClass::sendPacket() } -void SmartServoClass::writeCmd(uint8_t const id, uint8_t const instruction) { +void SmartServoClass::writeCmd(uint8_t const id, SmartServoOperation const instruction) { _txPacket.id = id; _txPacket.length = 2; - _txPacket.instruction = instruction; + _txPacket.instruction = toVal(instruction); sendPacket(); } @@ -117,7 +116,7 @@ int SmartServoClass::readByteCmd(uint8_t const id, uint8_t const address) { int SmartServoClass::ping(uint8_t const id) { mutex.lock(); - writeCmd(id, toVal(SmartServoOperation::PING)); + writeCmd(id, SmartServoOperation::PING); // TODO: check return receiveResponse(6); if (_rxLen==6 && @@ -140,14 +139,14 @@ int SmartServoClass::ping(uint8_t const id) { void SmartServoClass::reset(uint8_t const id) { mutex.lock(); - writeCmd(id, toVal(SmartServoOperation::RESET)); + writeCmd(id, SmartServoOperation::RESET); mutex.unlock(); } */ void SmartServoClass::action(uint8_t const id) { mutex.lock(); - writeCmd(id, toVal(SmartServoOperation::ACTION)); + writeCmd(id, SmartServoOperation::ACTION); mutex.unlock(); } diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index af333b6..57f029b 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -5,6 +5,8 @@ #include #include "RS485.h" +#include "SmartServoConst.h" + enum class PositionMode { IMMEDIATE, @@ -86,7 +88,7 @@ class SmartServoClass int calcChecksum (); void sendPacket (); - void writeCmd (uint8_t const id, uint8_t const instruction); + void writeCmd (uint8_t const id, SmartServoOperation const instruction); void writeByteCmd (uint8_t const id, uint8_t const address, uint8_t const data); void writeWordCmd (uint8_t const id, uint8_t const address, uint16_t const data); void receiveResponse (int const howMany = MAX_RX_PAYLOAD_LEN); From 077184605b48c5fd03f87b67ec499d725b1574ad Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 07:11:00 +0100 Subject: [PATCH 12/21] Extract PositionMode into header file 'SmartServoConst.h'. --- src/lib/motors/SmartServo.h | 6 ------ src/lib/motors/SmartServoConst.h | 6 ++++++ 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 57f029b..299df39 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -7,12 +7,6 @@ #include "SmartServoConst.h" -enum class PositionMode -{ - IMMEDIATE, - SYNC -}; - static int constexpr MAX_MOTORS = 6; class SmartServoClass diff --git a/src/lib/motors/SmartServoConst.h b/src/lib/motors/SmartServoConst.h index dba13b8..4d23b8c 100644 --- a/src/lib/motors/SmartServoConst.h +++ b/src/lib/motors/SmartServoConst.h @@ -76,6 +76,12 @@ enum class SmartServoOperation : uint8_t SYNC_WRITE = 0x83, }; +enum class PositionMode +{ + IMMEDIATE, + SYNC +}; + /************************************************************************************** * CONVERSION FUNCTIONS **************************************************************************************/ From 02a455972e7aba4499b022a9d7ef1a50e90f0307 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 07:12:03 +0100 Subject: [PATCH 13/21] Turning const MAX_MOTORS private, replacing const with constexpr to enable compiler optimisation. --- src/lib/motors/SmartServo.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 299df39..31f18d9 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -7,8 +7,6 @@ #include "SmartServoConst.h" -static int constexpr MAX_MOTORS = 6; - class SmartServoClass { public: @@ -74,9 +72,10 @@ class SmartServoClass private: - static const int MAX_TX_PAYLOAD_LEN = (5*MAX_MOTORS+4); - static const int MAX_RX_PAYLOAD_LEN = 10; - static const int MAX_POSITION = 4000; + static int constexpr MAX_MOTORS = 6; + static int constexpr MAX_TX_PAYLOAD_LEN = (5*MAX_MOTORS+4); + static int constexpr MAX_RX_PAYLOAD_LEN = 10; + static int constexpr MAX_POSITION = 4000; int timestamps[6 * 6] = {0}; From 8f5f6329520c0e3648be02ff1d1e43fc082912a9 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 07:15:05 +0100 Subject: [PATCH 14/21] Add separators for better readability. --- src/lib/motors/SmartServo.cpp | 17 ++++++++++++++++- src/lib/motors/SmartServo.h | 10 +++++++++- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 94d2433..20ee942 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -1,13 +1,25 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + #include #include "SmartServo.h" +/************************************************************************************** + * CTOR/DTOR + **************************************************************************************/ + SmartServoClass::SmartServoClass(RS485Class & RS485) : _RS485{RS485} { } +/************************************************************************************** + * PRIVATE MEMBER FUNCTIONS + **************************************************************************************/ + int SmartServoClass::calcChecksum() { char csum =0xff-_txPacket.id-_txPacket.length-_txPacket.instruction; int i=0; @@ -38,7 +50,6 @@ void SmartServoClass::sendPacket() } } - void SmartServoClass::writeCmd(uint8_t const id, SmartServoOperation const instruction) { _txPacket.id = id; _txPacket.length = 2; @@ -114,6 +125,10 @@ int SmartServoClass::readByteCmd(uint8_t const id, uint8_t const address) { return -1; } +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + int SmartServoClass::ping(uint8_t const id) { mutex.lock(); writeCmd(id, SmartServoOperation::PING); diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 31f18d9..45f0d69 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -1,12 +1,20 @@ #ifndef _SMARTMOTOR_H_ #define _SMARTMOTOR_H_ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + #include #include -#include "RS485.h" +#include "RS485.h" #include "SmartServoConst.h" +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + class SmartServoClass { public: From 4dde2a69376d45a6a8f09e5a733ca0dbf62d591e Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 07:19:10 +0100 Subject: [PATCH 15/21] Initialize private member 'errors' within constructor, prefix with '_'. --- src/lib/motors/SmartServo.cpp | 5 +++-- src/lib/motors/SmartServo.h | 5 ++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 20ee942..d147b0c 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -12,6 +12,7 @@ SmartServoClass::SmartServoClass(RS485Class & RS485) : _RS485{RS485} +, _errors{0} { } @@ -105,7 +106,7 @@ int SmartServoClass::readBuffer(uint8_t const id, uint8_t const address,uint8_t _rxBuf[3]==len+2) { return 0; } - errors++; + _errors++; if (onError) onError(); return -1; } @@ -144,7 +145,7 @@ int SmartServoClass::ping(uint8_t const id) { return _rxBuf[4]; } mutex.unlock(); - errors++; + _errors++; if (onError) onError(); return -1; } diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 45f0d69..e134cf5 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -74,7 +74,7 @@ class SmartServoClass inline void onErrorCb(mbed::Callback _onError) { onError = _onError; } - inline int getErrors() const { return errors; } + inline int getErrors() const { return _errors; } static const int BROADCAST = 0xFE; @@ -104,9 +104,8 @@ class SmartServoClass mbed::Callback onError; - int errors = 0; - RS485Class& _RS485; + int _errors; struct { uint8_t header [2]; From 2232b99fea8cf716606738de8517d21bca1d2a83 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 07:20:56 +0100 Subject: [PATCH 16/21] Prefix private member 'onError' with '_', initialise within constructor. --- src/lib/motors/SmartServo.cpp | 5 +++-- src/lib/motors/SmartServo.h | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index d147b0c..436bf76 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -13,6 +13,7 @@ SmartServoClass::SmartServoClass(RS485Class & RS485) : _RS485{RS485} , _errors{0} +, _onError{} { } @@ -107,7 +108,7 @@ int SmartServoClass::readBuffer(uint8_t const id, uint8_t const address,uint8_t return 0; } _errors++; - if (onError) onError(); + if (_onError) _onError(); return -1; } @@ -146,7 +147,7 @@ int SmartServoClass::ping(uint8_t const id) { } mutex.unlock(); _errors++; - if (onError) onError(); + if (_onError) _onError(); return -1; } diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index e134cf5..541ab71 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -72,7 +72,7 @@ class SmartServoClass void getInfo(Stream & stream, uint8_t const id); - inline void onErrorCb(mbed::Callback _onError) { onError = _onError; } + inline void onErrorCb(mbed::Callback onError) { _onError = onError; } inline int getErrors() const { return _errors; } @@ -102,10 +102,11 @@ class SmartServoClass inline uint16_t angleToPosition(float const angle) { return (angle*MAX_POSITION)/360.0; } inline float positionToAngle(uint16_t const position) { return (360.0*position)/MAX_POSITION; } - mbed::Callback onError; RS485Class& _RS485; int _errors; + mbed::Callback _onError; + struct { uint8_t header [2]; From acbe869395075cb2a0a12ea81f6c9feaf1150283 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 07:22:05 +0100 Subject: [PATCH 17/21] Deleting superfluous variable 'timestamps'. --- src/lib/motors/SmartServo.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 541ab71..842b600 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -85,8 +85,6 @@ class SmartServoClass static int constexpr MAX_RX_PAYLOAD_LEN = 10; static int constexpr MAX_POSITION = 4000; - int timestamps[6 * 6] = {0}; - int calcChecksum (); void sendPacket (); void writeCmd (uint8_t const id, SmartServoOperation const instruction); From ef93888c32514de670a0afa3d60aab68689f5157 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 07:25:37 +0100 Subject: [PATCH 18/21] The struct needs to be packed, otherwise the little trick with converting to a char pointer can backfire badly. --- src/lib/motors/SmartServo.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 842b600..4e01ad2 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -106,7 +106,7 @@ class SmartServoClass mbed::Callback _onError; - struct { + struct __attribute__((packed)) { uint8_t header [2]; uint8_t id; uint8_t length; From 7f7378c30cbc6aa65a65e916f427c1f2e051f4e2 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 07:27:45 +0100 Subject: [PATCH 19/21] Replace tabs with spaces (why does anyone still uses tabs in their editor ;( ). --- src/Braccio++.h | 334 ++++++++++++++++++++++++------------------------ 1 file changed, 167 insertions(+), 167 deletions(-) diff --git a/src/Braccio++.h b/src/Braccio++.h index acf3ee2..bd32a74 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -23,14 +23,14 @@ extern const lv_img_dsc_t img_bulb_gif; extern "C" { - void braccio_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p); - void read_keypad(lv_indev_drv_t * indev, lv_indev_data_t * data); + void braccio_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p); + void read_keypad(lv_indev_drv_t * indev, lv_indev_data_t * data); }; enum speed_grade_t { - FAST = 10, - MEDIUM = 100, - SLOW = 1000, + FAST = 10, + MEDIUM = 100, + SLOW = 1000, }; #include @@ -38,195 +38,195 @@ using namespace std::chrono; class MotorsWrapper { public: - MotorsWrapper(SmartServoClass & servos, int idx) : _servos(servos), _idx(idx) {} - MotorsWrapper& to(float angle) { - _servos.setPosition(_idx, angle, _speed); - return *this; - } - MotorsWrapper& in(std::chrono::milliseconds len) { - _servos.setTime(_idx, len.count()); - return *this; - } - MotorsWrapper& move() { - return *this; - } - float position() { - return _servos.getPosition(_idx); - } - bool connected() { - return _servos.ping(_idx) == 0; - } - operator bool() { - return connected(); - } - void info(Stream& stream) { - _servos.getInfo(stream, _idx); - } - void disengage() { - _servos.disengage(_idx); - } - void engage() { - _servos.engage(_idx); - } - bool engaged() { - return _servos.isEngaged(_idx); - } + MotorsWrapper(SmartServoClass & servos, int idx) : _servos(servos), _idx(idx) {} + MotorsWrapper& to(float angle) { + _servos.setPosition(_idx, angle, _speed); + return *this; + } + MotorsWrapper& in(std::chrono::milliseconds len) { + _servos.setTime(_idx, len.count()); + return *this; + } + MotorsWrapper& move() { + return *this; + } + float position() { + return _servos.getPosition(_idx); + } + bool connected() { + return _servos.ping(_idx) == 0; + } + operator bool() { + return connected(); + } + void info(Stream& stream) { + _servos.getInfo(stream, _idx); + } + void disengage() { + _servos.disengage(_idx); + } + void engage() { + _servos.engage(_idx); + } + bool engaged() { + return _servos.isEngaged(_idx); + } private: - SmartServoClass & _servos; - int _idx; - int _speed = 100; + SmartServoClass & _servos; + int _idx; + int _speed = 100; }; class BraccioClass { public: - BraccioClass(); - - bool begin(voidFuncPtr customMenu = nullptr); - - // setters - MotorsWrapper move(int joint_index) { - MotorsWrapper wrapper(servos, joint_index); - return wrapper; - } - MotorsWrapper get(int joint_index) { - return move(joint_index); - } - void moveTo(int joint_index, int position) { - //servos.setPosition(joint_index, position, 100); - } - void moveTo(int joint_index, float angle) { - servos.setPosition(joint_index, angle, 100); - } - void moveTo(float a1, float a2, float a3, float a4, float a5, float a6) { - servos.setPositionMode(PositionMode::SYNC); - servos.setPosition(1, a1, runTime); - servos.setPosition(2, a2, runTime); - servos.setPosition(3, a3, runTime); - servos.setPosition(4, a4, runTime); - servos.setPosition(5, a5, runTime); - servos.setPosition(6, a6, runTime); - servos.synchronize(); - servos.setPositionMode(PositionMode::IMMEDIATE); - } - // getters - void positions(float* buffer) { - for (int i = 1; i < 7; i++) { - *buffer++ = servos.getPosition(i); - } - } - void positions(float& a1, float& a2, float& a3, float& a4, float& a5, float& a6) { - // TODO: add check if motors are actually connected - a1 = servos.getPosition(1); - a2 = servos.getPosition(2); - a3 = servos.getPosition(3); - a4 = servos.getPosition(4); - a5 = servos.getPosition(5); - a6 = servos.getPosition(6); - } - float position(int joint_index); - bool connected(int joint_index) { - return _connected[joint_index]; - } - - void speed(speed_grade_t speed_grade) { - runTime = speed_grade; - } - - void disengage(int id = SmartServoClass::BROADCAST) { - servos.disengage(id); - } - - void engage(int id = SmartServoClass::BROADCAST) { - servos.engage(id); - } - - int getKey(); - void connectJoystickTo(lv_obj_t* obj); - - TFT_eSPI gfx = TFT_eSPI(); - - bool ping_allowed = true; - - static BraccioClass& get_default_instance() { - static BraccioClass dev; - return dev; - } + BraccioClass(); + + bool begin(voidFuncPtr customMenu = nullptr); + + // setters + MotorsWrapper move(int joint_index) { + MotorsWrapper wrapper(servos, joint_index); + return wrapper; + } + MotorsWrapper get(int joint_index) { + return move(joint_index); + } + void moveTo(int joint_index, int position) { + //servos.setPosition(joint_index, position, 100); + } + void moveTo(int joint_index, float angle) { + servos.setPosition(joint_index, angle, 100); + } + void moveTo(float a1, float a2, float a3, float a4, float a5, float a6) { + servos.setPositionMode(PositionMode::SYNC); + servos.setPosition(1, a1, runTime); + servos.setPosition(2, a2, runTime); + servos.setPosition(3, a3, runTime); + servos.setPosition(4, a4, runTime); + servos.setPosition(5, a5, runTime); + servos.setPosition(6, a6, runTime); + servos.synchronize(); + servos.setPositionMode(PositionMode::IMMEDIATE); + } + // getters + void positions(float* buffer) { + for (int i = 1; i < 7; i++) { + *buffer++ = servos.getPosition(i); + } + } + void positions(float& a1, float& a2, float& a3, float& a4, float& a5, float& a6) { + // TODO: add check if motors are actually connected + a1 = servos.getPosition(1); + a2 = servos.getPosition(2); + a3 = servos.getPosition(3); + a4 = servos.getPosition(4); + a5 = servos.getPosition(5); + a6 = servos.getPosition(6); + } + float position(int joint_index); + bool connected(int joint_index) { + return _connected[joint_index]; + } + + void speed(speed_grade_t speed_grade) { + runTime = speed_grade; + } + + void disengage(int id = SmartServoClass::BROADCAST) { + servos.disengage(id); + } + + void engage(int id = SmartServoClass::BROADCAST) { + servos.engage(id); + } + + int getKey(); + void connectJoystickTo(lv_obj_t* obj); + + TFT_eSPI gfx = TFT_eSPI(); + + bool ping_allowed = true; + + static BraccioClass& get_default_instance() { + static BraccioClass dev; + return dev; + } protected: - // ioexpander APIs - void digitalWrite(int pin, uint8_t value); + // ioexpander APIs + void digitalWrite(int pin, uint8_t value); - // default display APIs - void drawMenu(); - void splashScreen(int duration = 1000); - void hideMenu(); - void drawImage(char* image); - void defaultMenu(); + // default display APIs + void drawMenu(); + void splashScreen(int duration = 1000); + void hideMenu(); + void drawImage(char* image); + void defaultMenu(); - void setID(int id) { - servos.setID(id); - } + void setID(int id) { + servos.setID(id); + } private: RS485Class serial485; SmartServoClass servos; - PD_UFP_log_c PD_UFP; - TCA6424A expander; - Backlight bl; + PD_UFP_log_c PD_UFP; + TCA6424A expander; + Backlight bl; - speed_grade_t runTime; //ms + speed_grade_t runTime; //ms - voidFuncPtr _customMenu; + voidFuncPtr _customMenu; - const int BTN_LEFT = 3; - const int BTN_RIGHT = 4; - const int BTN_UP = 5; - const int BTN_DOWN = 2; - const int BTN_SEL = A0; - const int BTN_ENTER = A1; + const int BTN_LEFT = 3; + const int BTN_RIGHT = 4; + const int BTN_UP = 5; + const int BTN_DOWN = 2; + const int BTN_SEL = A0; + const int BTN_ENTER = A1; lv_disp_drv_t disp_drv; lv_indev_drv_t indev_drv; - lv_disp_draw_buf_t disp_buf; - lv_color_t buf[240 * 240 / 10]; - lv_group_t* p_objGroup; - lv_indev_t *kb_indev; + lv_disp_draw_buf_t disp_buf; + lv_color_t buf[240 * 240 / 10]; + lv_group_t* p_objGroup; + lv_indev_t *kb_indev; bool _connected[8]; #ifdef __MBED__ - rtos::EventFlags pd_events; - rtos::Mutex i2c_mutex; - mbed::Ticker pd_timer; - - unsigned int start_pd_burst = 0xFFFFFFFF; - - void unlock_pd_semaphore_irq() { - start_pd_burst = millis(); - pd_events.set(2); - } - - void unlock_pd_semaphore() { - pd_events.set(1); - } - - void setGreen(int i) { - expander.writePin(i * 2 - 1, 0); - expander.writePin(i * 2 - 2, 1); - } - - void setRed(int i) { - expander.writePin(i * 2 - 1, 1); - expander.writePin(i * 2 - 2, 0); - } - - void pd_thread(); - void display_thread(); - void motors_connected_thread(); + rtos::EventFlags pd_events; + rtos::Mutex i2c_mutex; + mbed::Ticker pd_timer; + + unsigned int start_pd_burst = 0xFFFFFFFF; + + void unlock_pd_semaphore_irq() { + start_pd_burst = millis(); + pd_events.set(2); + } + + void unlock_pd_semaphore() { + pd_events.set(1); + } + + void setGreen(int i) { + expander.writePin(i * 2 - 1, 0); + expander.writePin(i * 2 - 2, 1); + } + + void setRed(int i) { + expander.writePin(i * 2 - 1, 1); + expander.writePin(i * 2 - 2, 0); + } + + void pd_thread(); + void display_thread(); + void motors_connected_thread(); #endif }; From f564e9fc3d1170ee55f55725f9d6f9fe7efd8f46 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 09:28:37 +0100 Subject: [PATCH 20/21] Remove superfluous inclusion of 'Arduino.h' in .cpp file (already included in .h). --- src/lib/motors/SmartServo.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 436bf76..92fd5ea 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -2,8 +2,6 @@ * INCLUDE **************************************************************************************/ -#include - #include "SmartServo.h" /************************************************************************************** From 6a000cb727378b2f7b37a34b525858715decd7e2 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 12 Jan 2022 09:53:37 +0100 Subject: [PATCH 21/21] Provide REG/CMD macros to hide 'ugly' conversion function 'toVal'. --- src/lib/motors/SmartServo.cpp | 50 ++++++++++++++++---------------- src/lib/motors/SmartServoConst.h | 7 +++++ 2 files changed, 32 insertions(+), 25 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 92fd5ea..80dea68 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/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,7 +196,7 @@ float SmartServoClass::getPosition(uint8_t const id) { mutex.lock(); float ret = -1; if (id typename std::underlying_type::type>(value); } +/************************************************************************************** + * MACROS + **************************************************************************************/ + +#define REG(enum_val) toVal(enum_val) +#define CMD(enum_val) toVal(enum_val) + #endif /* SMART_SERVO_CONST_H_ */ \ No newline at end of file