Merge pull request #44 from RoboticsBrno/motor-posreg

dzarda · web-flow · commit 5e370eb9a7ee · 2020-07-14T10:58:55.000+02:00
Add motor position regulation
diff --git a/fw/rbcx-coprocessor/include/Motor.hpp b/fw/rbcx-coprocessor/include/Motor.hpp
@@ -3,27 +3,80 @@
 #include "utils/Debug.hpp"
 #include "utils/Regulator.hpp"
 
-#include <algorithm>
 #include <stdint.h>
 
 inline const uint16_t motorLoopFreq = 100;
 
 class Motor {
 public:
-    Motor()
-        : m_velocityReg(INT16_MAX, 150000, 300000, 20000)
-        , m_positionReg(INT16_MAX, 150000, 300000, 20000)
-        , m_state(PowerCtrl) {}
-
-    std::pair<int16_t, bool> poll(uint16_t encTicks) {
-        int16_t actualTicksPerLoop = encTicks - m_lastEncTicks;
-        m_actualPosition += actualTicksPerLoop;
+    Motor() {
+        m_lastEncTicks = 0;
+        reset();
+    }
+
+    void reset() {
+        m_velocityReg = Regulator(INT16_MAX, 150000, 300000, 20000);
+        m_positionReg = Regulator(500, 1000, 0, 0);
+        m_dither = 0;
+        m_targetVelocity = 0;
+        m_actualPower = 0;
+        m_actualPosition = 0;
+        m_targetPosition = 0;
+        m_actualTicksPerLoop = 0;
+        m_posEpsilon = 3;
+        m_velEpsilon = 3;
+        m_maxAccel = 2000 / motorLoopFreq;
+        m_mode = MotorMode_POWER;
+    }
+
+    bool atTargetPosition() const {
+        return uint32_t(abs(m_actualPosition - m_targetPosition))
+            <= m_posEpsilon;
+    }
+
+    bool atStandstill() const {
+        return uint32_t(abs(m_actualTicksPerLoop)) <= m_velEpsilon;
+    }
+
+    MotorMode mode() const { return m_mode; }
+    void modeChange(MotorMode newMode) { m_mode = newMode; }
+
+    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) {
+        m_actualTicksPerLoop = encTicks - m_lastEncTicks;
+        m_actualPosition += m_actualTicksPerLoop;
         m_lastEncTicks = encTicks;
-        if (m_state == PowerCtrl) {
-            return std::make_pair(m_targetPower, false);
-        } else if (m_state == BrakeCtrl) {
-            return std::make_pair(m_targetBrakingPower, true);
-        } else if (m_state == VelocityCtrl) {
+
+        switch (m_mode) {
+        case MotorMode_POSITION:
+        case MotorMode_POSITION_IDLE: {
+            // DEBUG("i:%ld e:%ld s:%ld -> v:%ld\n", m_positionReg.integrator(),
+            //     m_positionReg.e(), m_actualPosition, m_positionReg.output());
+            if (atTargetPosition() && atStandstill()) {
+                m_positionReg.clear();
+                m_targetVelocity = 0;
+                modeChange(MotorMode_POSITION_IDLE);
+            } else {
+                auto action
+                    = m_positionReg.process(m_targetPosition, m_actualPosition);
+
+                // Limit ramp-up to max acceleration
+                if (action > m_targetVelocity + m_maxAccel) {
+                    m_targetVelocity += m_maxAccel;
+                } else if (action < m_targetVelocity - m_maxAccel) {
+                    m_targetVelocity -= m_maxAccel;
+                } else {
+                    m_targetVelocity = action;
+                }
+            }
+        } // fallthrough
+        case MotorMode_VELOCITY: {
             int16_t targetTicksPerLoop = m_targetVelocity / motorLoopFreq;
             uint16_t targetTicksRem = abs(m_targetVelocity % motorLoopFreq);
             if ((targetTicksRem * 4) / motorLoopFreq > m_dither) {
@@ -33,35 +86,34 @@ class Motor {
                 m_dither = 0;
             }
 
-            auto action
-                = m_velocityReg.process(targetTicksPerLoop, actualTicksPerLoop);
-            return std::make_pair(action, false);
-        } else if (m_state == PositionCtrl) {
-            auto action
-                = m_positionReg.process(m_targetPosition, m_actualPosition);
-            return std::make_pair(action, false);
+            auto action = m_velocityReg.process(
+                targetTicksPerLoop, m_actualTicksPerLoop);
+            m_actualPower = action;
+        } break;
+        default:
+            break;
         }
-        return std::make_pair(0, false);
+        return m_actualPower;
     }
 
     void setTargetPower(int16_t power) {
-        if (m_state != PowerCtrl) {
-            m_state = PowerCtrl;
+        if (m_mode != MotorMode_POWER) {
+            modeChange(MotorMode_POWER);
         }
-        m_targetPower = power;
+        m_actualPower = power;
     }
 
     void setTargetBrakingPower(int16_t brakingPower) {
-        if (m_state != BrakeCtrl) {
-            m_state = BrakeCtrl;
+        if (m_mode != MotorMode_BRAKE) {
+            modeChange(MotorMode_BRAKE);
         }
-        m_targetBrakingPower = brakingPower;
+        m_actualPower = brakingPower;
     }
 
     void setTargetVelocity(int16_t ticksPerSec) {
-        if (m_state != VelocityCtrl) {
+        if (m_mode != MotorMode_VELOCITY) {
             m_velocityReg.clear();
-            m_state = VelocityCtrl;
+            modeChange(MotorMode_VELOCITY);
         }
         m_targetVelocity = ticksPerSec;
     }
@@ -71,50 +123,52 @@ class Motor {
         m_actualPosition = homedTicks;
     }
 
-    void setTargetPosition(int32_t ticks) {
-        if (m_state != PositionCtrl) {
+    void setTargetPosition(
+        const CoprocReq_MotorReq_SetPosition& req, bool additive) {
+        if (m_mode != MotorMode_POSITION) {
             m_positionReg.clear();
-            m_state = PositionCtrl;
+            modeChange(MotorMode_POSITION);
         }
-        m_targetPosition = ticks;
-    }
-
-    void addTargetPosition(int32_t ticksDelta) {
-        if (m_state != PositionCtrl) {
-            m_positionReg.clear();
-            m_state = PositionCtrl;
+        if (additive) {
+            m_targetPosition += req.targetPosition;
+        } else {
+            m_targetPosition = req.targetPosition;
         }
-        m_targetPosition += ticksDelta;
+        m_positionReg.setMaxOutput(req.runningVelocity);
     }
 
-    void setVelocityPid(uint32_t p, uint32_t i, uint32_t d) {
-        m_velocityReg.setP(p);
-        m_velocityReg.setI(i);
-        m_velocityReg.setD(d);
+    void setVelocityPid(const RegCoefs& coefs) {
+        m_velocityReg.setP(coefs.p);
+        m_velocityReg.setI(coefs.i);
+        m_velocityReg.setD(coefs.d);
         m_velocityReg.clear();
     }
 
-    void setPositionPid(uint32_t p, uint32_t i, uint32_t d) {
-        m_positionReg.setP(p);
-        m_positionReg.setI(i);
-        m_positionReg.setD(d);
+    void setPositionPid(const RegCoefs& coefs) {
+        m_positionReg.setP(coefs.p);
+        m_positionReg.setI(coefs.i);
+        m_positionReg.setD(coefs.d);
         m_positionReg.clear();
     }
 
+    void setConfig(const MotorConfig& config) {
+        m_velEpsilon = config.velEpsilon;
+        m_posEpsilon = config.posEpsilon;
+        m_maxAccel = config.maxAccel / motorLoopFreq;
+    }
+
 private:
     Regulator m_velocityReg;
     Regulator m_positionReg;
-    int16_t m_targetPower;
-    int16_t m_targetBrakingPower;
+    int16_t m_actualPower;
     int16_t m_targetVelocity;
     int32_t m_targetPosition;
     int32_t m_actualPosition;
+    int16_t m_actualTicksPerLoop;
     uint16_t m_dither;
     uint16_t m_lastEncTicks;
-    enum MotorState {
-        PowerCtrl,
-        BrakeCtrl,
-        VelocityCtrl,
-        PositionCtrl,
-    } m_state;
+    uint16_t m_posEpsilon;
+    uint16_t m_velEpsilon;
+    uint16_t m_maxAccel;
+    MotorMode m_mode;
 };
diff --git a/fw/rbcx-coprocessor/include/utils/Regulator.hpp b/fw/rbcx-coprocessor/include/utils/Regulator.hpp
@@ -27,6 +27,8 @@ struct Regulator {
     coef_type I() const { return m_i; }
     void setD(const coef_type& v) { m_d = v; }
     coef_type D() const { return m_d; }
+    void setMaxOutput(value_type max_output) { m_max_output = max_output; }
+    value_type maxOutput() { return m_max_output; }
 
     void stop(bool s = true) {
         m_stop = s;
diff --git a/fw/rbcx-coprocessor/platformio.ini b/fw/rbcx-coprocessor/platformio.ini
@@ -17,7 +17,7 @@ platform = ststm32@~7.0.0
 board = genericSTM32F103VC
 framework = stm32cube
 lib_deps =
-    https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/703a8cb92b45c5e50f809c58a6e132458ccadc04.zip
+    https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/6715dde5a9b004860dfa17af17f330ded1fa9715.zip
 
 build_flags =
     -Iinclude
diff --git a/fw/rbcx-coprocessor/src/DebugLink.cpp b/fw/rbcx-coprocessor/src/DebugLink.cpp
@@ -278,6 +278,24 @@ static void debugLinkHandleCommand(const char* cmd) {
             dispatcherEnqueueRequest(req);
             return;
         });
+
+        COMMAND("position", {
+            CoprocReq req = {
+                .which_payload = CoprocReq_motorReq_tag,
+            };
+            auto& c = req.payload.motorReq;
+            c.which_motorCmd = CoprocReq_MotorReq_setPosition_tag;
+            if (sscanf(cmd, "%lu %ld %ld", &c.motorIndex,
+                    &c.motorCmd.setPosition.targetPosition,
+                    &c.motorCmd.setPosition.runningVelocity)
+                != 3) {
+                printf("Invalid parameters!\n");
+                return;
+            }
+
+            dispatcherEnqueueRequest(req);
+            return;
+        });
     });
 
     COMMAND("leds", {
diff --git a/fw/rbcx-coprocessor/src/MotorController.cpp b/fw/rbcx-coprocessor/src/MotorController.cpp
@@ -1,5 +1,7 @@
 #include "MotorController.hpp"
 #include "Bsp.hpp"
+#include "ControlLink.hpp"
+#include "Dispatcher.hpp"
 #include "Motor.hpp"
 #include "utils/Debug.hpp"
 #include "utils/MutexWrapper.hpp"
@@ -13,6 +15,8 @@
 static void setPwmValue(TIM_TypeDef* timer, uint8_t motorIndex, uint16_t value);
 static void setMotorPower(uint8_t motorIndex, int32_t power, bool brake);
 
+// Debounce ENC signals at ~3.5us (72MHz fDTS)
+static constexpr uint32_t encoderFilter = LL_TIM_IC_FILTER_FDIV32_N8;
 static constexpr uint16_t maxPwm = 2000;
 static std::array<Motor, 4> motor;
 static MutexWrapper motorMut;
@@ -62,11 +66,11 @@ void motorInit() {
     encInit.IC1Polarity = LL_TIM_IC_POLARITY_RISING;
     encInit.IC1ActiveInput = LL_TIM_ACTIVEINPUT_DIRECTTI;
     encInit.IC1Prescaler = LL_TIM_ICPSC_DIV1;
-    encInit.IC1Filter = LL_TIM_IC_FILTER_FDIV1;
+    encInit.IC1Filter = encoderFilter;
     encInit.IC2Polarity = LL_TIM_IC_POLARITY_RISING;
     encInit.IC2ActiveInput = LL_TIM_ACTIVEINPUT_DIRECTTI;
     encInit.IC2Prescaler = LL_TIM_ICPSC_DIV1;
-    encInit.IC2Filter = LL_TIM_IC_FILTER_FDIV1;
+    encInit.IC2Filter = encoderFilter;
     for (auto timer : encoderTimer) {
         LL_TIM_ENCODER_Init(timer, &encInit);
         LL_TIM_EnableCounter(timer);
@@ -84,8 +88,21 @@ static void taskFunc() {
 
             for (int m : { 0, 1, 2, 3 }) {
                 uint16_t encTicks = LL_TIM_GetCounter(encoderTimer[m]);
-                auto action = motor[m].poll(encTicks);
-                setMotorPower(m, action.first, action.second);
+                auto& targetMotor = motor[m];
+                auto modeBefore = targetMotor.mode();
+                auto action = targetMotor.poll(encTicks);
+                auto modeAfter = targetMotor.mode();
+                setMotorPower(m, action, modeAfter == MotorMode_BRAKE);
+
+                if (modeBefore == MotorMode_POSITION
+                    && modeAfter == MotorMode_POSITION_IDLE) {
+                    CoprocStat stat = {
+                        .which_payload = CoprocStat_motorStat_tag,
+                    };
+                    targetMotor.reportStat(stat.payload.motorStat);
+                    stat.payload.motorStat.motorIndex = m;
+                    dispatcherEnqueueStatus(stat);
+                }
             }
         }
         vTaskDelayUntil(&now, pdMS_TO_TICKS(1000 / motorLoopFreq));
@@ -101,6 +118,14 @@ void motorDispatch(const CoprocReq_MotorReq& request) {
     std::scoped_lock lock(motorMut);
 
     switch (request.which_motorCmd) {
+    case CoprocReq_MotorReq_getState_tag: {
+        CoprocStat stat = {
+            .which_payload = CoprocStat_motorStat_tag,
+        };
+        targetMotor.reportStat(stat.payload.motorStat);
+        stat.payload.motorStat.motorIndex = request.motorIndex;
+        controlLinkTx(stat);
+    } break;
     case CoprocReq_MotorReq_setPower_tag:
         targetMotor.setTargetPower(request.motorCmd.setPower);
         break;
@@ -117,28 +142,32 @@ void motorDispatch(const CoprocReq_MotorReq& request) {
         }
         targetMotor.setTargetVelocity(ticksPerSec);
     } break;
+    case CoprocReq_MotorReq_homePosition_tag:
+        targetMotor.homePosition(request.motorCmd.homePosition);
+        break;
     case CoprocReq_MotorReq_setPosition_tag:
-        targetMotor.setTargetPosition(request.motorCmd.setPosition);
+        targetMotor.setTargetPosition(request.motorCmd.setPosition, false);
         break;
     case CoprocReq_MotorReq_addPosition_tag:
-        targetMotor.addTargetPosition(request.motorCmd.addPosition);
+        targetMotor.setTargetPosition(request.motorCmd.addPosition, true);
+        break;
+    case CoprocReq_MotorReq_setVelocityRegCoefs_tag:
+        targetMotor.setVelocityPid(request.motorCmd.setVelocityRegCoefs);
+        break;
+    case CoprocReq_MotorReq_setPositionRegCoefs_tag:
+        targetMotor.setPositionPid(request.motorCmd.setPositionRegCoefs);
+        break;
+    case CoprocReq_MotorReq_setConfig_tag:
+        targetMotor.setConfig(request.motorCmd.setConfig);
         break;
-    case CoprocReq_MotorReq_setVelocityRegCoefs_tag: {
-        auto& coefs = request.motorCmd.setVelocityRegCoefs;
-        targetMotor.setVelocityPid(coefs.p, coefs.i, coefs.d);
-    } break;
-    case CoprocReq_MotorReq_setPositionRegCoefs_tag: {
-        auto& coefs = request.motorCmd.setPositionRegCoefs;
-        targetMotor.setPositionPid(coefs.p, coefs.i, coefs.d);
-    } break;
     }
 }
 
 void motorReset() {
     std::scoped_lock lock(motorMut);
 
     for (int idx : { 0, 1, 2, 3 }) {
-        motor[idx].setTargetPower(0);
+        motor[idx].reset();
         setMotorPower(idx, 0, false);
     }
 }
