Diff: SpeedControl.cpp

Revision:

11:05d5539141c8

Parent:

8:351b0b7b05b2

--- a/SpeedControl.cpp	Mon May 29 13:03:28 2017 +0000
+++ b/SpeedControl.cpp	Thu Jun 01 08:34:27 2017 +0000
@@ -4,15 +4,19 @@
 #include "EncoderCounter.h"
 #include "LowpassFilter.h"
 
-//Constructor    
+long integralErrorRight=0;
+long integralErrorLeft=0;
+short integralErrorMax=1000;
+
+
+//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
+    // 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
@@ -31,22 +35,8 @@
     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;
+    t2.attach( callback(this, &SpeedControl::speedCtrl), 0.1f);  // SpeedControl:: allows you to access funtion within the class file
 
-    //for testing
-    /*
-        while(1) {
-            my_led = !my_led;
-            wait(0.2);
-
-            printf("encL: %d, encR: %d\n\r", counterLeft.read(), counterRight.read());
-        }
-    */
 }
 
 //Destructor
@@ -54,18 +44,9 @@
 {
 }
 
-
-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]
+    //Calculate Effective speeds of the motor [rpm]
     short valueCounterLeft = counterLeft->read();
     short valueCounterRight = counterRight->read();
     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
@@ -76,10 +57,13 @@
     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;
@@ -93,13 +77,16 @@
     if (integralErrorLeft < -integralErrorMax)
         integralErrorLeft = -integralErrorMax;
     //end of Paul's I control
-
+    */
+    // + KI*integralErrorLeft and KI*integralErrorRight + were removed from below
     // P + I Control!
-    float voltageLeft = KP*eL + KI*integralErrorLeft + desiredSpeedLeft/KN;
-    float voltageRight = KP*eR + KI*integralErrorRight + desiredSpeedRight/KN;
-
+    float voltageLeft = KP*eL  + desiredSpeedLeft/KN;
+    float voltageRight = KP*eR +  desiredSpeedRight/KN;
+    
+    //printf("the voltage on the L and R sides are %f and %f \n\r", voltageLeft, voltageRight);
+    
     // Berechnen, Limitieren und Setzen der Duty-Cycle
-    float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
+    float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;    // Max_voltage is 12.0f
     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
 
@@ -110,8 +97,12 @@
     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
 
     *pwmRight = dutyCycleRight;
+
+   // printf("the pwm L and R speeds should be %f and %f \n\r", dutyCycleLeft, dutyCycleRight);
+
 }
 
+//set desired wheel speed in RPM
 void SpeedControl::setDesiredSpeed( float L, float R)
 {
     desiredSpeedLeft = L;