Diff: Controller.cpp

Revision:

0:5d4d21d56334

Child:

3:a292bdaf03f6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,129 @@
+#include "Controller.h"
+
+using namespace std;
+
+const float Controller::PERIOD = 0.001f;                    // period of 1 ms
+const float Controller::COUNTS_PER_TURN = 1200.0f;         // encoder resolution
+const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // given in [rad/s]
+const float Controller::KN = 40.0f;                         // speed constant in [rpm/V]
+const float Controller::KP = 0.2f;                          // speed control parameter
+const float Controller::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
+const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum duty-cycle
+const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle
+
+Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
+                        EncoderCounter& counterLeft, EncoderCounter& counterRight) :
+                        pwmLeft(pwmLeft), pwmRight(pwmRight),
+                        counterLeft(counterLeft), counterRight(counterRight) {
+
+    // Initialisieren der PWM Ausgaenge
+    
+    pwmLeft.period(0.00005f); // PWM Periode von 50 us
+    pwmLeft = 0.5f; // Duty-Cycle von 50%
+    pwmRight.period(0.00005f); // PWM Periode von 50 us
+    pwmRight = 0.5f; // Duty-Cycle von 50%
+
+    // Initialisieren von lokalen Variabeln
+
+    previousValueCounterLeft = counterLeft.read();
+    previousValueCounterRight = counterRight.read();
+
+    speedLeftFilter.setPeriod(PERIOD);
+    speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+    speedRightFilter.setPeriod(PERIOD);
+    speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+    desiredSpeedLeft = 0.0f;
+    desiredSpeedRight = 0.0f;
+
+    actualSpeedLeft = 0.0f;
+    actualSpeedRight = 0.0f;
+    
+    actualAngleLeft = 0.0f;
+    actualAngleRight = 0.0f;
+
+    // Starten des periodischen Tasks
+    thread.start(callback(this, &Controller::run));
+    ticker.attach(callback(this, &Controller::sendThreadFlag), PERIOD);
+}
+
+Controller::~Controller()
+{
+    ticker.detach(); // Stoppt den periodischen Task
+}
+
+
+void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
+{
+    this->desiredSpeedLeft = desiredSpeedLeft;
+}
+
+void Controller::setDesiredSpeedRight(float desiredSpeedRight)
+{
+    this->desiredSpeedRight = desiredSpeedRight;
+}
+
+float Controller::getSpeedLeft()
+{
+    return actualSpeedLeft;
+}
+
+float Controller::getSpeedRight()
+{
+    return actualSpeedRight;
+}
+
+/**
+ * This method is called by the ticker timer interrupt service routine.
+ * It sends a flag to the thread to make it run again.
+ */
+void Controller::sendThreadFlag() {
+    
+    thread.flags_set(threadFlag);
+}
+
+void Controller::run() {
+
+    while(true) {
+
+        // wait for the periodic signal
+
+        ThisThread::flags_wait_any(threadFlag);
+
+        // calculate actual speed of motors in [rpm]
+
+        short valueCounterLeft = counterLeft.read();
+        short valueCounterRight = counterRight.read();
+
+        short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
+        short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
+
+        previousValueCounterLeft = valueCounterLeft;
+        previousValueCounterRight = valueCounterRight;
+
+        actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
+        actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
+
+        actualAngleLeft = actualAngleLeft + actualSpeedLeft/60.0f*PERIOD;
+        actualAngleRight = actualAngleRight + actualSpeedRight/60.0f*PERIOD;
+
+        // calculate motor phase voltages
+
+        float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+        float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
+
+        // calculate, limit and set duty cycles
+        
+        float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
+        if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
+        else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
+        pwmLeft.write(dutyCycleLeft);
+
+        float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
+        if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
+        else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
+        pwmRight.write(dutyCycleRight);
+
+    }
+}