Diff: SpeedControl.cpp

Revision:

8:351b0b7b05b2

Child:

11:05d5539141c8

Child:

15:4efc66de795a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SpeedControl.cpp	Mon May 29 13:03:28 2017 +0000
@@ -0,0 +1,120 @@
+
+#include "mbed.h"
+#include "SpeedControl.h"
+#include "EncoderCounter.h"
+#include "LowpassFilter.h"
+
+//Constructor    
+//directly from mainSpeedControl int main()
+SpeedControl::SpeedControl(PwmOut* pwmLeft, PwmOut* pwmRight, EncoderCounter* counterLeft, EncoderCounter* counterRight)
+{
+    this->pwmLeft = pwmLeft;
+    this->pwmRight = pwmRight;
+    //this->counterLeft =counterLeft;
+
+          // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
+    this->pwmLeft->period(0.00005f); // Setzt die Periode auf 50 μs
+    this->pwmRight->period(0.00005f);
+    this->pwmLeft->write( 0.5f); // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
+    this->pwmRight->write( 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;
+
+    Ticker t2;
+    t2.attach( this, &SpeedControl::speedCtrl, 0.05f);  // SpeedControl:: allows you to access funtion within the class file
+
+    //desiredSpeedLeft = 50.0f; //50 RPM
+    //desiredSpeedRight = -50.0f; //50 RPM
+    // enableMotorDriver = 1;
+
+    //for testing
+    /*
+        while(1) {
+            my_led = !my_led;
+            wait(0.2);
+
+            printf("encL: %d, encR: %d\n\r", counterLeft.read(), counterRight.read());
+        }
+    */
+}
+
+//Destructor
+SpeedControl::~SpeedControl()
+{
+}
+
+
+LowpassFilter speedLeftFilter;
+LowpassFilter speedRightFilter;
+
+long integralErrorRight=0;
+long integralErrorLeft=0;
+short integralErrorMax=1000;
+
+
+void SpeedControl::speedCtrl()  //EncoderCounter* counterLeft, EncoderCounter* counterRight
+{
+    // Berechnen die effektiven Drehzahlen der Motoren 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);
+
+    //Error of each wheel from Pauls I code
+    short eL = desiredSpeedLeft-actualSpeedLeft;
+    short eR = desiredSpeedRight-actualSpeedRight;
+
+    // Increment the error integral for each wheel
+    integralErrorRight += eR;
+    integralErrorLeft += eL;
+    // Limit the integral term
+    if (integralErrorRight > integralErrorMax)
+        integralErrorRight = integralErrorMax;
+    if (integralErrorRight < -integralErrorMax)
+        integralErrorRight = -integralErrorMax;
+    if (integralErrorLeft > integralErrorMax)
+        integralErrorLeft = integralErrorMax;
+    if (integralErrorLeft < -integralErrorMax)
+        integralErrorLeft = -integralErrorMax;
+    //end of Paul's I control
+
+    // P + I Control!
+    float voltageLeft = KP*eL + KI*integralErrorLeft + desiredSpeedLeft/KN;
+    float voltageRight = KP*eR + KI*integralErrorRight + desiredSpeedRight/KN;
+
+    // Berechnen, Limitieren und Setzen der Duty-Cycle
+    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 = 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 = dutyCycleRight;
+}
+
+void SpeedControl::setDesiredSpeed( float L, float R)
+{
+    desiredSpeedLeft = L;
+    desiredSpeedRight = R;
+}
+