From cf5c803aeafd7bc9f1e5b27653293c59473e7183 Mon Sep 17 00:00:00 2001 From: Jaroslav Malec Date: Thu, 13 Oct 2022 16:01:01 +0200 Subject: [PATCH 1/3] Bump coproc comm for power coupling --- fw/rbcx-coprocessor/platformio.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fw/rbcx-coprocessor/platformio.ini b/fw/rbcx-coprocessor/platformio.ini index fc22b2a7..c564ae87 100644 --- a/fw/rbcx-coprocessor/platformio.ini +++ b/fw/rbcx-coprocessor/platformio.ini @@ -16,7 +16,7 @@ platform = ststm32@~7.0.0 board = genericSTM32F103VC framework = stm32cube lib_deps = - https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/26267ccf1d3eda5c6aa973c3b0be9aaceb43c61f.zip + https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/dc09065d38029b0ab91579e04c70839469f58e82.zip build_flags = -Iinclude From cf290c97a74c56673e81bbe260f9cc75558b6be0 Mon Sep 17 00:00:00 2001 From: Jaroslav Malec Date: Sun, 16 Oct 2022 22:55:26 +0200 Subject: [PATCH 2/3] Add motor power coupling --- fw/rbcx-coprocessor/include/Motor.hpp | 15 +++++++++++-- fw/rbcx-coprocessor/platformio.ini | 2 +- fw/rbcx-coprocessor/src/MotorController.cpp | 25 +++++++++++++++++++-- 3 files changed, 37 insertions(+), 5 deletions(-) diff --git a/fw/rbcx-coprocessor/include/Motor.hpp b/fw/rbcx-coprocessor/include/Motor.hpp index 0cdfc366..317f5bdb 100644 --- a/fw/rbcx-coprocessor/include/Motor.hpp +++ b/fw/rbcx-coprocessor/include/Motor.hpp @@ -29,6 +29,8 @@ class Motor { m_mode = MotorMode_POWER; } + int16_t actualPower() const { return m_actualPower; } + bool atTargetPosition() const { return uint32_t(abs(m_actualPosition - m_targetPosition)) <= m_posEpsilon; @@ -41,6 +43,8 @@ class Motor { MotorMode mode() const { return m_mode; } void modeChange(MotorMode newMode) { m_mode = newMode; } + uint32_t coupleAxis() const { return m_coupleAxis; } + void reportStat(CoprocStat_MotorStat& stat) { stat.mode = m_mode; stat.position = m_actualPosition; @@ -48,7 +52,7 @@ class Motor { stat.velocity = m_actualTicksPerLoop * motorLoopFreq; }; - int16_t poll(uint16_t encTicks) { + void poll(uint16_t encTicks) { m_actualTicksPerLoop = encTicks - m_lastEncTicks; m_actualPosition += m_actualTicksPerLoop; m_lastEncTicks = encTicks; @@ -93,7 +97,6 @@ class Motor { default: break; } - return m_actualPower; } void setTargetPower(int16_t power) { @@ -157,6 +160,13 @@ class Motor { m_maxAccel = config.maxAccel / motorLoopFreq; } + void setCoupling(const CoprocReq_SetMotorCoupling& coupling) { + if (m_mode != MotorMode_COUPLE_POWER) { + modeChange(MotorMode_COUPLE_POWER); + } + m_coupleAxis = coupling.coupleAxis; + } + private: Regulator m_velocityReg; Regulator m_positionReg; @@ -171,4 +181,5 @@ class Motor { uint16_t m_velEpsilon; uint16_t m_maxAccel; MotorMode m_mode; + int32_t m_coupleAxis; }; diff --git a/fw/rbcx-coprocessor/platformio.ini b/fw/rbcx-coprocessor/platformio.ini index c564ae87..97ff94d3 100644 --- a/fw/rbcx-coprocessor/platformio.ini +++ b/fw/rbcx-coprocessor/platformio.ini @@ -16,7 +16,7 @@ platform = ststm32@~7.0.0 board = genericSTM32F103VC framework = stm32cube lib_deps = - https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/dc09065d38029b0ab91579e04c70839469f58e82.zip + https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/2a47451ed98dccde6b72d0e34c01cdda91a9dc88.zip build_flags = -Iinclude diff --git a/fw/rbcx-coprocessor/src/MotorController.cpp b/fw/rbcx-coprocessor/src/MotorController.cpp index 76fb02fa..ea05d769 100644 --- a/fw/rbcx-coprocessor/src/MotorController.cpp +++ b/fw/rbcx-coprocessor/src/MotorController.cpp @@ -90,9 +90,8 @@ static void taskFunc() { uint16_t encTicks = LL_TIM_GetCounter(encoderTimer[m]); auto& targetMotor = motor[m]; auto modeBefore = targetMotor.mode(); - auto action = targetMotor.poll(encTicks); + targetMotor.poll(encTicks); auto modeAfter = targetMotor.mode(); - setMotorPower(m, action, modeAfter == MotorMode_BRAKE); if (modeBefore == MotorMode_POSITION && modeAfter == MotorMode_POSITION_IDLE) { @@ -104,6 +103,20 @@ static void taskFunc() { dispatcherEnqueueStatus(stat); } } + // Loop split due to possible dependency when coupling one motor's output to another: + for (int m : { 0, 1, 2, 3 }) { + auto& targetMotor = motor[m]; + auto mode = targetMotor.mode(); + auto power = targetMotor.actualPower(); + bool brake = mode == MotorMode_BRAKE; + if (mode == MotorMode_COUPLE_POWER) { + // Inherit power/brake state of the coupled motor + auto& coupleMotor = motor[targetMotor.coupleAxis()]; + power = coupleMotor.actualPower(); + brake = coupleMotor.mode() == MotorMode_BRAKE; + } + setMotorPower(m, power, brake); + } } vTaskDelayUntil(&now, pdMS_TO_TICKS(1000 / motorLoopFreq)); } @@ -160,6 +173,14 @@ void motorDispatch(const CoprocReq_MotorReq& request) { case CoprocReq_MotorReq_setConfig_tag: targetMotor.setConfig(request.motorCmd.setConfig); break; + case CoprocReq_MotorReq_setCoupling_tag: + if (request.motorCmd.setCoupling.coupleAxis > 3) { + DEBUG("Motor coupling axis %u invalid", + request.motorCmd.setCoupling.coupleAxis); + return; + } + targetMotor.setCoupling(request.motorCmd.setCoupling); + break; } } From 3af16ea3002924a8282a1702d48aa05bdbf80478 Mon Sep 17 00:00:00 2001 From: Jaroslav Malec Date: Mon, 17 Oct 2022 15:01:01 +0200 Subject: [PATCH 3/3] Simplify power coupling logic --- fw/rbcx-coprocessor/src/MotorController.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/fw/rbcx-coprocessor/src/MotorController.cpp b/fw/rbcx-coprocessor/src/MotorController.cpp index ea05d769..5ffe1594 100644 --- a/fw/rbcx-coprocessor/src/MotorController.cpp +++ b/fw/rbcx-coprocessor/src/MotorController.cpp @@ -105,16 +105,14 @@ static void taskFunc() { } // Loop split due to possible dependency when coupling one motor's output to another: for (int m : { 0, 1, 2, 3 }) { - auto& targetMotor = motor[m]; - auto mode = targetMotor.mode(); + auto& currentMotor = motor[m]; + auto& targetMotor + = currentMotor.mode() != MotorMode_COUPLE_POWER + ? currentMotor + : motor[currentMotor.coupleAxis()]; + auto power = targetMotor.actualPower(); - bool brake = mode == MotorMode_BRAKE; - if (mode == MotorMode_COUPLE_POWER) { - // Inherit power/brake state of the coupled motor - auto& coupleMotor = motor[targetMotor.coupleAxis()]; - power = coupleMotor.actualPower(); - brake = coupleMotor.mode() == MotorMode_BRAKE; - } + bool brake = targetMotor.mode() == MotorMode_BRAKE; setMotorPower(m, power, brake); } }