Diff: PositionController.cpp

Revision:

8:6b747ad59ff5

Child:

9:583dbd17e0ba

diff -r 9cc27f98645a -r 6b747ad59ff5 PositionController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PositionController.cpp	Mon Apr 19 15:54:15 2021 +0000
@@ -0,0 +1,130 @@
+#include "PositionController.h"
+
+using namespace std;
+
+const float PositionController::TS = 0.001f;                       // period of 1 ms
+const float PositionController::LOWPASS_FILTER_FREQUENCY = 100.0f; // given in [rad/s]
+const float PositionController::MIN_DUTY_CYCLE = 0.0f;             // minimum duty-cycle
+const float PositionController::MAX_DUTY_CYCLE = 1.0f;             // maximum duty-cycle
+
+PositionController::PositionController(float counts_per_turn, float kn, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
+{
+    this->counts_per_turn = counts_per_turn;
+    this->kn = kn;
+    this->kp = 0.1f;
+    this->max_voltage = max_voltage;
+    this->max_speed   = kn*max_voltage;
+    this->p  = 1300.0f;
+
+    // Initialisieren der PWM Ausgaenge
+    pwm.period(0.00005); // PWM Periode von 50 us
+    pwm.write(0.5);      // Duty-Cycle von 50%
+
+    // Initialisieren von lokalen Variabeln
+    previousValueCounter = encoderCounter.read();
+    speedFilter.setPeriod(TS);
+    speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+    desiredSpeed = 0.0f;
+    actualSpeed = 0.0f;
+    float initialRotation = (float)encoderCounter.read()/counts_per_turn;
+    this->initialRotation = initialRotation;
+    actualRotation  = initialRotation;
+    desiredRotation = initialRotation;
+
+    // Starten des periodischen Tasks
+    thread.start(callback(this, &PositionController::run));
+    ticker.attach(callback(this, &PositionController::sendThreadFlag), TS);
+}
+
+PositionController::PositionController(float counts_per_turn, float kn, float kp, float p, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
+{
+    this->counts_per_turn = counts_per_turn;
+    this->kn = kn;
+    this->kp = kp;
+    this->max_voltage = max_voltage;
+    this->max_speed   = kn*max_voltage;
+    this->p  = p;
+
+    // Initialisieren der PWM Ausgaenge
+    pwm.period(0.00005); // PWM Periode von 50 us
+    pwm.write(0.5);      // Duty-Cycle von 50%
+
+    // Initialisieren von lokalen Variabeln
+    previousValueCounter = encoderCounter.read();
+    speedFilter.setPeriod(TS);
+    speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+    desiredSpeed = 0.0f;
+    actualSpeed  = 0.0f;
+    float initialRotation = (float)encoderCounter.read()/counts_per_turn;
+    this->initialRotation = initialRotation;
+    actualRotation  = initialRotation;
+    desiredRotation = initialRotation;
+
+    // Starten des periodischen Tasks
+    thread.start(callback(this, &PositionController::run));
+    ticker.attach(callback(this, &PositionController::sendThreadFlag), TS);
+}
+
+PositionController::~PositionController()
+{
+    ticker.detach(); // Stoppt den periodischen Task
+}
+
+float PositionController::getSpeedRPM()
+{
+    return actualSpeed;
+}
+
+float PositionController::getSpeedRPS()
+{
+    return actualSpeed/60.0f;
+}
+
+void PositionController::setDesiredRotation(float desiredRotation)
+{
+    this->desiredRotation = initialRotation + desiredRotation;
+}
+
+void PositionController::setDesiredRotation(float desiredRotation, float maxSpeedRPS)
+{
+    this->max_speed = maxSpeedRPS*60.0f;
+    this->desiredRotation = initialRotation + desiredRotation;
+}
+
+float PositionController::getRotation()
+{
+    return actualRotation - initialRotation;
+}
+
+void PositionController::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/TS*60.0f);
+        actualRotation = actualRotation + actualSpeed/60.0f*TS;
+
+        // calculate motor phase voltages
+        desiredSpeed  = p*(desiredRotation - actualRotation);
+        if (desiredSpeed < -max_speed) desiredSpeed = -max_speed;
+        else if (desiredSpeed > max_speed) desiredSpeed = max_speed;
+        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(static_cast<double>(dutyCycle));
+
+    }
+}
+
+void PositionController::sendThreadFlag()
+{
+    thread.flags_set(threadFlag);
+}
\ No newline at end of file