Motor otrok

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,14 +43,16 @@ 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;
         stat.power = m_actualPower;
         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;
 };

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/2a47451ed98dccde6b72d0e34c01cdda91a9dc88.zip
 
 build_flags =
     -Iinclude

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,18 @@ 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& currentMotor = motor[m];
+                auto& targetMotor
+                    = currentMotor.mode() != MotorMode_COUPLE_POWER
+                    ? currentMotor
+                    : motor[currentMotor.coupleAxis()];
+
+                auto power = targetMotor.actualPower();
+                bool brake = targetMotor.mode() == MotorMode_BRAKE;
+                setMotorPower(m, power, brake);
+            }
         }
         vTaskDelayUntil(&now, pdMS_TO_TICKS(1000 / motorLoopFreq));
     }
@@ -160,6 +171,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;
     }
 }