From 3f757d39a15b00a16284e6027626f4f62663387d Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 20 Jan 2022 07:06:33 +0100 Subject: [PATCH 1/2] Use speed() API to directly write desired speed to servos. --- src/Braccio++.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/Braccio++.h b/src/Braccio++.h index 77ae893..d590a1c 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -60,7 +60,9 @@ class BraccioClass void positions(float * buffer); void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6); - inline void speed (speed_grade_t const speed_grade) { runTime = speed_grade; } + inline void speed(speed_grade_t const speed_grade) { servos.setTime(SmartServoClass::BROADCAST, speed_grade); } + inline void speed(int const id, speed_grade_t const speed_grade) { servos.setTime(id, speed_grade); } + inline void disengage(int const id = SmartServoClass::BROADCAST) { servos.disengage(id); } inline void engage (int const id = SmartServoClass::BROADCAST) { servos.engage(id); } From ac5ec75cc6b8f0a83b829a06f4bb373f60f77a81 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 20 Jan 2022 07:16:49 +0100 Subject: [PATCH 2/2] Consolidating different APIs writing to the RUN_TIME register within a single API. Now everything is consistent. The wise programmer is told about Tao and follows it. The average programmer is told about Tao and searches for it. The foolish programmer is told about Tao and laughs at it. If it were not for laughter, there would be no Tao. The highest sounds are hardest to hear. Going forward is a way to retreat. Great talent shows itself late in life. Even a perfect program still has bugs. --- src/Braccio++.cpp | 14 +++++++------- src/Braccio++.h | 6 +----- src/lib/motors/SmartServo.cpp | 7 ++----- src/lib/motors/SmartServo.h | 3 +-- 4 files changed, 11 insertions(+), 19 deletions(-) diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index b53c074..e322681 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -25,7 +25,6 @@ BraccioClass::BraccioClass() , _is_motor_connected{false} , _motors_connected_mtx{} , _motors_connected_thd{} -, runTime{SLOW} , _customMenu{nullptr} { @@ -163,6 +162,7 @@ bool BraccioClass::begin(voidFuncPtr customMenu) } servos.begin(); + servos.setTime(SmartServoClass::BROADCAST, SLOW); servos.setPositionMode(PositionMode::IMMEDIATE); _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc)); @@ -202,12 +202,12 @@ Servo BraccioClass::get(int const id) void BraccioClass::moveTo(float const a1, float const a2, float const a3, float const a4, float const a5, float const 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.setPosition(1, a1); + servos.setPosition(2, a2); + servos.setPosition(3, a3); + servos.setPosition(4, a4); + servos.setPosition(5, a5); + servos.setPosition(6, a6); servos.synchronize(); servos.setPositionMode(PositionMode::IMMEDIATE); } diff --git a/src/Braccio++.h b/src/Braccio++.h index d590a1c..9c50d40 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -114,8 +114,6 @@ class BraccioClass void setMotorConnectionStatus(int const id, bool const is_connected); void motorConnectedThreadFunc(); - speed_grade_t runTime; //ms - voidFuncPtr _customMenu; const int BTN_LEFT = 3; @@ -177,7 +175,7 @@ class Servo inline bool engaged() { return _servos.isEngaged(_id); } inline Servo & move() { return *this; } - inline Servo & to (float const angle) { _servos.setPosition(_id, angle, _speed); return *this; } + inline Servo & to (float const angle) { _servos.setPosition(_id, angle); return *this; } inline Servo & in (std::chrono::milliseconds const len) { _servos.setTime(_id, len.count()); return *this; } inline float position() { return _servos.getPosition(_id); } @@ -191,8 +189,6 @@ class Servo SmartServoClass & _servos; int const _id; - int const _speed = 100; - }; struct __callback__container__ { diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index d6c20ab..efc60b4 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -175,7 +175,7 @@ void SmartServoClass::begin() _positionMode = PositionMode::IMMEDIATE; } -void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed) +void SmartServoClass::setPosition(uint8_t const id, float const angle) { if (!isValidAngle(angle)) return; @@ -184,7 +184,6 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t if (isValidId(id)) { _targetPosition[idToArrayIndex(id)] = angleToPosition(angle); - _targetSpeed[idToArrayIndex(id)] = speed; if (_positionMode==PositionMode::IMMEDIATE) { writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle)); } @@ -212,15 +211,13 @@ void SmartServoClass::synchronize() _txPacket.length = MAX_TX_PAYLOAD_LEN; _txPacket.instruction = CMD(SmartServoOperation::SYNC_WRITE); _txPacket.payload[0] = REG(SmartServoRegister::TARGET_POSITION_H); - _txPacket.payload[1] = 4; + _txPacket.payload[1] = 2; int index = 2; for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++) { _txPacket.payload[index++] = i; _txPacket.payload[index++] = _targetPosition[idToArrayIndex(i)] >>8; _txPacket.payload[index++] = _targetPosition[idToArrayIndex(i)]; - _txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)]>>8; - _txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)]; } sendPacket(); } diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 25cd347..ad87bce 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -26,7 +26,7 @@ class SmartServoClass inline void setPositionMode(PositionMode const mode) { _positionMode = mode; } - void setPosition(uint8_t const id, float const angle, uint16_t const speed); + void setPosition(uint8_t const id, float const angle); float getPosition(uint8_t const id); @@ -120,7 +120,6 @@ class SmartServoClass uint8_t _rxBuf[MAX_RX_PAYLOAD_LEN]; uint8_t _rxLen; uint16_t _targetPosition[NUM_MOTORS]; - uint16_t _targetSpeed[NUM_MOTORS]; PositionMode _positionMode; };