Diff: SpeedController.cpp

Revision:

3:8b42e643b294

Child:

4:9c003c402033

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SpeedController.cpp	Thu Apr 01 14:31:43 2021 +0000
@@ -0,0 +1,79 @@
+#include "SpeedController.h"
+
+using namespace std;
+
+const float SpeedController::PERIOD = 0.001f;                    // period of 1 ms
+const float SpeedController::COUNTS_PER_TURN = 1560.0f;          // encoder resolution
+const float SpeedController::LOWPASS_FILTER_FREQUENCY = 300.0f;  // given in [rad/s]
+const float SpeedController::KN = 15.0f;                         // speed constant in [rpm/V]
+const float SpeedController::KP = 0.15f;                         // speed control parameter
+const float SpeedController::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
+const float SpeedController::MIN_DUTY_CYCLE = 0.02f;             // minimum duty-cycle
+const float SpeedController::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle
+
+SpeedController::SpeedController(PwmOut& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
+{
+
+    // Initialisieren der PWM Ausgaenge
+    pwm.period(0.00005f); // PWM Periode von 50 us
+    pwm = 0.5f;           // Duty-Cycle von 50%
+
+    // Initialisieren von lokalen Variabeln
+    previousValueCounter = encoderCounter.read();
+    speedFilter.setPeriod(PERIOD);
+    speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+    desiredSpeed = 0.0f;
+    actualSpeed = 0.0f;
+    actualAngle = 0.0f;
+
+    // Starten des periodischen Tasks
+    thread.start(callback(this, &SpeedController::run));
+    ticker.attach(callback(this, &SpeedController::sendThreadFlag), PERIOD);
+}
+
+SpeedController::~SpeedController()
+{
+    ticker.detach(); // Stoppt den periodischen Task
+}
+
+
+void SpeedController::setDesiredSpeed(float desiredSpeed)
+{
+    this->desiredSpeed = desiredSpeed;
+}
+
+float SpeedController::getSpeed()
+{
+    return actualSpeed;
+}
+
+void SpeedController::run()
+{
+    while(true) {
+
+        // wait for the periodic signal
+        ThisThread::flags_wait_any(threadFlag);
+
+        // calculate actual speed of motors in [rpm]
+        short valueCounter = encoderCounter.read();
+        short countsInPastPeriod = valueCounter-previousValueCounter;
+        previousValueCounter = valueCounter;
+        actualSpeed = speedFilter.filter((float)countsInPastPeriod/COUNTS_PER_TURN/PERIOD*60.0f);
+        actualAngle = actualAngle + actualSpeed/60.0f*PERIOD;
+
+        // calculate motor phase voltages
+        float voltage = KP*(desiredSpeed-actualSpeed)+desiredSpeed/KN;
+
+        // calculate, limit and set duty cycles
+        float dutyCycle = 0.5f+0.5f*voltage/MAX_VOLTAGE;
+        if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
+        else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
+        pwm.write(dutyCycle);
+
+    }
+}
+
+void SpeedController::sendThreadFlag()
+{
+    thread.flags_set(threadFlag);
+}
\ No newline at end of file