diff --git a/src/Braccio++.h b/src/Braccio++.h index e85d931..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,193 +38,195 @@ using namespace std::chrono; class MotorsWrapper { public: - MotorsWrapper(SmartServoClass<7>* 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<7>* _servos; - int _idx; - int _speed = 100; + SmartServoClass & _servos; + int _idx; + int _speed = 100; }; -class BraccioClass { +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(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); - } - } - 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<7>::BROADCAST) { - servos->disengage(id); - } - - void engage(int id = SmartServoClass<7>::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 = RS485Class(Serial1, 0, 7, 8); // TX, DE, RE - SmartServoClass<7>* servos = new SmartServoClass<7>(serial485); - - PD_UFP_log_c PD_UFP = PD_UFP_log_c(PD_LOG_LEVEL_VERBOSE); - TCA6424A expander = TCA6424A(TCA6424A_ADDRESS_ADDR_HIGH); - Backlight bl; + 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; - 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 }; diff --git a/src/Braccio.cpp b/src/Braccio.cpp index 50b6cc5..263d478 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{serial485} +, PD_UFP{PD_LOG_LEVEL_VERBOSE} +, expander{TCA6424A_ADDRESS_ADDR_HIGH} +, bl{} +, runTime{SLOW} +, _customMenu{nullptr} + +{ + +} + bool BraccioClass::begin(voidFuncPtr customMenu) { Wire.begin(); @@ -143,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(PositionMode::IMMEDIATE); #ifdef __MBED__ static rtos::Thread connected_th; @@ -227,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]); } 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.cpp b/src/lib/motors/SmartServo.cpp new file mode 100644 index 0000000..80dea68 --- /dev/null +++ b/src/lib/motors/SmartServo.cpp @@ -0,0 +1,339 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include "SmartServo.h" + +/************************************************************************************** + * CTOR/DTOR + **************************************************************************************/ + +SmartServoClass::SmartServoClass(RS485Class & RS485) +: _RS485{RS485} +, _errors{0} +, _onError{} +{ + +} + +/************************************************************************************** + * PRIVATE MEMBER FUNCTIONS + **************************************************************************************/ + +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++) { + csum -= _txPacket.payload[i]; + } + _txPacket.payload[i]=csum; + return i+6; +} + +void SmartServoClass::sendPacket() +{ + char *buffer = (char *) &_txPacket; + int len = calcChecksum(); + _RS485.beginTransmission(); + for (int i=0;i 0) { + _RS485.read(); + } +} + +void SmartServoClass::writeCmd(uint8_t const id, SmartServoOperation const instruction) { + _txPacket.id = id; + _txPacket.length = 2; + _txPacket.instruction = toVal(instruction); + sendPacket(); +} + +void SmartServoClass::writeByteCmd(uint8_t const id, uint8_t const address, uint8_t const data) { + _txPacket.id = id; + _txPacket.length = 2+2; + _txPacket.instruction = CMD(SmartServoOperation::WRITE); + _txPacket.payload[0]=address; + _txPacket.payload[1]=data; + sendPacket(); +} + +void SmartServoClass::writeWordCmd(uint8_t const id, uint8_t const address, uint16_t const data) { + _txPacket.id = id; + _txPacket.length = 2+3; + _txPacket.instruction = CMD(SmartServoOperation::WRITE); + _txPacket.payload[0]=address; + _txPacket.payload[1]=data>>8; + _txPacket.payload[2]=data; + sendPacket(); +} + +void SmartServoClass::receiveResponse(int const howMany) { + _rxLen=0; + memset(_rxBuf, 0, sizeof(_rxBuf)); + long start = millis(); + while ((millis() - start < howMany) && (_rxLen < howMany)) { // timeout_min = 70uS * howMany + if (_RS485.available()) { + uint8_t c = _RS485.read(); + //Serial.print(c, HEX); + //Serial.print(" "); + _rxBuf[_rxLen++]=c; + } + } +} + +int SmartServoClass::readBuffer(uint8_t const id, uint8_t const address,uint8_t const len) { + _txPacket.id = id; + _txPacket.length = 2+2; + _txPacket.instruction = CMD(SmartServoOperation::READ); + _txPacket.payload[0]=address; + _txPacket.payload[1]=len; + sendPacket(); + receiveResponse(6+len); + if (_rxLen==(6+len) && + _rxBuf[0]==0xff && + _rxBuf[1]==0xf5 && + _rxBuf[2]==id && + _rxBuf[3]==len+2) { + return 0; + } + _errors++; + if (_onError) _onError(); + return -1; +} + + +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 const id, uint8_t const address) { + if (readBuffer(id,address,1) == 0) { + return _rxBuf[5]; + } + return -1; +} + +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + +int SmartServoClass::ping(uint8_t const id) { + mutex.lock(); + writeCmd(id, SmartServoOperation::PING); + // TODO: check return + receiveResponse(6); + if (_rxLen==6 && + _rxBuf[0]==0xff && + _rxBuf[1]==0xf5 && + _rxBuf[2]==id && + _rxBuf[3]==2) { + + mutex.unlock(); + return _rxBuf[4]; + } + mutex.unlock(); + _errors++; + if (_onError) _onError(); + return -1; +} + +/* +// ATTENTION: RESET also changes the ID of the motor + +void SmartServoClass::reset(uint8_t const id) { + mutex.lock(); + writeCmd(id, SmartServoOperation::RESET); + mutex.unlock(); +} +*/ + +void SmartServoClass::action(uint8_t const id) { + mutex.lock(); + writeCmd(id, SmartServoOperation::ACTION); + mutex.unlock(); +} + +int SmartServoClass::begin() { + if (_RS485) { + _txPacket.header[0] = 0xff; + _txPacket.header[1] = 0xff; + _RS485.begin(115200, 0, 90); + _RS485.receive(); + writeByteCmd(BROADCAST, REG(SmartServoRegister::SERVO_MOTOR_MODE), 1); + writeByteCmd(BROADCAST, REG(SmartServoRegister::TORQUE_SWITCH) ,1); + _positionMode = PositionMode::IMMEDIATE; + return 0; + } else { + return -1; + } +} + +void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed) { + mutex.lock(); + if (id>8; + _txPacket.payload[index++] = _targetPosition[i]; + _txPacket.payload[index++] = _targetSpeed[i]>>8; + _txPacket.payload[index++] = _targetSpeed[i]; + } + sendPacket(); + mutex.unlock(); +} + +void SmartServoClass::setTorque(bool const torque) { + mutex.lock(); + 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, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0); + mutex.unlock(); +} + +void SmartServoClass::setTime(uint8_t const id, uint16_t const time) { + mutex.lock(); + writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time); + mutex.unlock(); +} + +void SmartServoClass::setMaxTorque(uint16_t const torque) { + mutex.lock(); + 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, REG(SmartServoRegister::MAX_TORQUE_H), torque); + mutex.unlock(); +} + +void SmartServoClass::setID(uint8_t const id) { + mutex.lock(); + writeByteCmd(BROADCAST, REG(SmartServoRegister::ID), id); + mutex.unlock(); +} + +void SmartServoClass::engage(uint8_t const id) { + mutex.lock(); + writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), 0x1); + mutex.unlock(); +} + +void SmartServoClass::disengage(uint8_t const id) { + mutex.lock(); + writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), 0); + mutex.unlock(); +} + +bool SmartServoClass::isEngaged(uint8_t const id) { + mutex.lock(); + int ret = readByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH)); + mutex.unlock(); + return ret != 0; +} + +void SmartServoClass::setStallProtectionTime(uint8_t const time) { + mutex.lock(); + 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, REG(SmartServoRegister::STALL_PROTECTION_TIME), time); + mutex.unlock(); +} + +void SmartServoClass::setMinAngle(float const angle) { + mutex.lock(); + 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, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle)); + mutex.unlock(); +} + +void SmartServoClass::setMaxAngle(float const angle) { + mutex.lock(); + 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, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle)); + mutex.unlock(); +} + +void SmartServoClass::getInfo(Stream & stream, uint8_t const id) { + uint8_t regs[65]; + memset(regs, 0x55, sizeof(regs)); + int i = 0; + mutex.lock(); + while (i < sizeof(regs)) { + if ((i > 29 && i < 40) || (i > 48 && i < 56)) { + i++; + continue; + } + regs[i++] = readByteCmd(id, i); + } + mutex.unlock(); + stream.println("regs map:"); + for (i = 0; i < sizeof(regs); i++) { + stream.println(String(i, HEX) + " : " + String(regs[i], HEX)); + } +} \ No newline at end of file diff --git a/src/lib/motors/SmartServo.cpp.impl b/src/lib/motors/SmartServo.cpp.impl deleted file mode 100644 index 31ce6ae..0000000 --- a/src/lib/motors/SmartServo.cpp.impl +++ /dev/null @@ -1,330 +0,0 @@ -#include - -template SmartServoClass::SmartServoClass( RS485Class& RS485) : _RS485(RS485) {} - -template 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++) { - csum -= _txPacket.payload[i]; - } - _txPacket.payload[i]=csum; - return i+6; -} - -template void SmartServoClass::sendPacket() -{ - char *buffer = (char *) &_txPacket; - int len = calcChecksum(); - _RS485.beginTransmission(); - for (int i=0;i 0) { - _RS485.read(); - } -} - - -template 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) { - _txPacket.id = id; - _txPacket.length = 2+2; - _txPacket.instruction = OP_WRITE; - _txPacket.payload[0]=address; - _txPacket.payload[1]=data; - sendPacket(); -} - -template void SmartServoClass::writeWordCmd(uint8_t id, uint8_t address, uint16_t data) { - _txPacket.id = id; - _txPacket.length = 2+3; - _txPacket.instruction = OP_WRITE; - _txPacket.payload[0]=address; - _txPacket.payload[1]=data>>8; - _txPacket.payload[2]=data; - sendPacket(); -} - -template void SmartServoClass::receiveResponse(int howMany) { - _rxLen=0; - memset(_rxBuf, 0, sizeof(_rxBuf)); - long start = millis(); - while ((millis() - start < howMany) && (_rxLen < howMany)) { // timeout_min = 70uS * howMany - if (_RS485.available()) { - uint8_t c = _RS485.read(); - //Serial.print(c, HEX); - //Serial.print(" "); - _rxBuf[_rxLen++]=c; - } - } -} - -template int SmartServoClass::readBuffer(uint8_t id, uint8_t address,uint8_t len) { - _txPacket.id = id; - _txPacket.length = 2+2; - _txPacket.instruction = OP_READ; - _txPacket.payload[0]=address; - _txPacket.payload[1]=len; - sendPacket(); - receiveResponse(6+len); - if (_rxLen==(6+len) && - _rxBuf[0]==0xff && - _rxBuf[1]==0xf5 && - _rxBuf[2]==id && - _rxBuf[3]==len+2) { - return 0; - } - errors++; - if (onError) onError(); - return -1; -} - - -template 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) { - if (readBuffer(id,address,1) == 0) { - return _rxBuf[5]; - } - return -1; -} - -template int SmartServoClass::ping(uint8_t id) { - mutex.lock(); - writeCmd(id, OP_PING); - // TODO: check return - receiveResponse(6); - if (_rxLen==6 && - _rxBuf[0]==0xff && - _rxBuf[1]==0xf5 && - _rxBuf[2]==id && - _rxBuf[3]==2) { - - mutex.unlock(); - return _rxBuf[4]; - } - mutex.unlock(); - errors++; - if (onError) onError(); - return -1; -} - -/* -// ATTENTION: RESET also changes the ID of the motor - -template void SmartServoClass::reset(uint8_t id) { - mutex.lock(); - writeCmd(id, OP_RESET); - mutex.unlock(); -} -*/ - -template void SmartServoClass::action(uint8_t id) { - mutex.lock(); - writeCmd(id, OP_ACTION); - mutex.unlock(); -} - -template int SmartServoClass::begin() { - if (_RS485) { - _txPacket.header[0] = 0xff; - _txPacket.header[1] = 0xff; - _RS485.begin(115200, 0, 90); - _RS485.receive(); - writeByteCmd(BROADCAST,SERVO_MOTOR_MODE,1); - writeByteCmd(BROADCAST,TORQUE_SWITCH,1); - _positionMode = pmIMMEDIATE; - return 0; - } else { - return -1; - } -} - -template void SmartServoClass::setPositionMode(positionMode mode) { - _positionMode = mode; -} - -template uint16_t SmartServoClass::angleToPosition (float angle) { - return angle*MAX_POSITION/360.0; -} - -template float SmartServoClass::positionToAngle (uint16_t position) { - return (360.0*position)/MAX_POSITION; -} - -template void SmartServoClass::setPosition(uint8_t id, float angle, uint16_t speed) { - mutex.lock(); - if (id float SmartServoClass::getPosition(uint8_t id) { - mutex.lock(); - float ret = -1; - if (id void SmartServoClass::center(uint8_t id, uint16_t position) { - mutex.lock(); - writeWordCmd(id, CENTER_POINT_ADJ_H, position); - mutex.unlock(); -} - -template void SmartServoClass::synchronize() { - mutex.lock(); - _txPacket.id = 0xFE; - _txPacket.length = (4+1)*MAX_MOTORS +4; - _txPacket.instruction = OP_SYNC_WRITE; - _txPacket.payload[0] = TARGET_POSITION_H; - _txPacket.payload[1] = 4; - int index = 2; - - for (int i=1;i < MAX_MOTORS;i++) { - _txPacket.payload[index++] = i; - _txPacket.payload[index++] = _targetPosition[i] >>8; - _txPacket.payload[index++] = _targetPosition[i]; - _txPacket.payload[index++] = _targetSpeed[i]>>8; - _txPacket.payload[index++] = _targetSpeed[i]; - } - sendPacket(); - mutex.unlock(); -} - -template 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) { - mutex.lock(); - writeByteCmd(id,TORQUE_SWITCH,torque ? 1 : 0); - mutex.unlock(); -} - -template 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) { - mutex.lock(); - writeWordCmd(BROADCAST,MAX_TORQUE_H,torque ); - mutex.unlock(); -} - -template 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) { - mutex.lock(); - writeByteCmd(BROADCAST,ID,id); - mutex.unlock(); -} - -template void SmartServoClass::engage(uint8_t id) { - mutex.lock(); - writeByteCmd(id,TORQUE_SWITCH, 0x1); - mutex.unlock(); -} - -template void SmartServoClass::disengage(uint8_t id) { - mutex.lock(); - writeByteCmd(id,TORQUE_SWITCH, 0); - mutex.unlock(); -} - -template 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) { - mutex.lock(); - writeByteCmd(BROADCAST,STALL_PROTECTION_TIME,time); - mutex.unlock(); -} - -template 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) { - mutex.lock(); - writeByteCmd(BROADCAST,MIN_ANGLE_LIMIT_H,angleToPosition(angle)); - mutex.unlock(); -} - -template 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) { - mutex.lock(); - writeByteCmd(BROADCAST,MAX_ANGLE_LIMIT_H,angleToPosition(angle)); - mutex.unlock(); -} - -template 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) { - uint8_t regs[65]; - memset(regs, 0x55, sizeof(regs)); - int i = 0; - mutex.lock(); - while (i < sizeof(regs)) { - if ((i > 29 && i < 40) || (i > 48 && i < 56)) { - i++; - continue; - } - regs[i++] = readByteCmd(id, i); - } - mutex.unlock(); - stream.println("regs map:"); - for (i = 0; i < sizeof(regs); i++) { - stream.println(String(i, HEX) + " : " + String(regs[i], HEX)); - } -} \ No newline at end of file diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 0becdba..4e01ad2 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -1,173 +1,112 @@ #ifndef _SMARTMOTOR_H_ #define _SMARTMOTOR_H_ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + #include -#include "ArduinoRS485.h" +#include + +#include "RS485.h" +#include "SmartServoConst.h" -typedef enum { - pmIMMEDIATE, - pmSYNC -} positionMode; +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ -template class SmartServoClass { public: - SmartServoClass( RS485Class& RS485); + SmartServoClass(RS485Class & RS485); int begin(); void end(); - void setPositionMode(positionMode mode); - void setPosition(uint8_t id, float angle, uint16_t speed); + inline void setPositionMode(PositionMode const mode) { _positionMode = mode; } - float getPosition(uint8_t id); + void setPosition(uint8_t const id, float const angle, uint16_t const speed); + + float getPosition(uint8_t const id); void synchronize(); - void setTorque(bool torque); + void setTorque(bool const torque); - void setTorque(uint8_t id, bool torque); + void setTorque(uint8_t const id, bool const torque); - void setMaxTorque(uint16_t torque); + void setMaxTorque(uint16_t const torque); - void setMaxTorque(uint8_t id, uint16_t torque); + void setMaxTorque(uint8_t const id, uint16_t const torque); - void setID(uint8_t id); + void setID(uint8_t const id); - void engage(uint8_t id); + void engage(uint8_t const id); - void disengage(uint8_t id); + void disengage(uint8_t const id); - void setStallProtectionTime(uint8_t time); + void setStallProtectionTime(uint8_t const time); - void setStallProtectionTime(uint8_t id, uint8_t time); + void setStallProtectionTime(uint8_t const id, uint8_t const time); - void setMinAngle(float angle); + void setMinAngle(float const angle); - void setMinAngle(uint8_t id, float angle); + void setMinAngle(uint8_t const id, float const angle); - void setMaxAngle(float angle); + void setMaxAngle(float const angle); - void setMaxAngle(uint8_t id, float angle); + void setMaxAngle(uint8_t const id, float const angle); - void setTime(uint8_t id, uint16_t time); + void setTime(uint8_t const id, uint16_t const time); - void center(uint8_t id, uint16_t position); + void center(uint8_t const id, uint16_t const position); - int ping(uint8_t id); + int ping(uint8_t const id); - //void reset(uint8_t id = BROADCAST); + //void reset(uint8_t const id = BROADCAST); - bool isEngaged(uint8_t id); + bool isEngaged(uint8_t const id); void printTimestamps(); - void getInfo(Stream& stream, uint8_t id); + void getInfo(Stream & stream, uint8_t const id); - void onErrorCb(mbed::Callback _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; private: - 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}; - - 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; + 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 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); - - uint16_t angleToPosition (float angle); + 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); + 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); - float positionToAngle (uint16_t position); + 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; - - int errors = 0; RS485Class& _RS485; + int _errors; + mbed::Callback _onError; + - struct { + struct __attribute__((packed)) { uint8_t header [2]; uint8_t id; uint8_t length; @@ -179,11 +118,9 @@ class SmartServoClass uint8_t _rxLen; uint16_t _targetPosition[MAX_MOTORS]; uint16_t _targetSpeed[MAX_MOTORS]; - positionMode _positionMode; + PositionMode _positionMode; rtos::Mutex mutex; }; -#include "SmartServo.cpp.impl" - #endif // _SMARTMOTOR_H_ \ No newline at end of file diff --git a/src/lib/motors/SmartServoConst.h b/src/lib/motors/SmartServoConst.h new file mode 100644 index 0000000..4269c5f --- /dev/null +++ b/src/lib/motors/SmartServoConst.h @@ -0,0 +1,102 @@ +#ifndef SMART_SERVO_CONST_H_ +#define SMART_SERVO_CONST_H_ + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +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 -- + _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 +}; + +enum class SmartServoOperation : uint8_t +{ + PING = 1, + READ = 2, + WRITE = 3, + REG_WRITE = 4, + ACTION = 5, + RESET = 6, + SYNC_WRITE = 0x83, +}; + +enum class PositionMode +{ + IMMEDIATE, + SYNC +}; + +/************************************************************************************** + * CONVERSION FUNCTIONS + **************************************************************************************/ + +template +constexpr auto toVal(Enumeration const value) -> typename std::underlying_type::type +{ + return static_cast::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