Diff: SpeedControl.cpp

Revision:

17:ec52258b9472

Parent:

15:4efc66de795a

--- a/SpeedControl.cpp	Mon Jun 05 07:57:07 2017 +0000
+++ b/SpeedControl.cpp	Wed Jun 07 11:35:59 2017 +0000
@@ -35,8 +35,7 @@
     actualSpeedLeft = 0.0f;
     actualSpeedRight = 0.0f;
 
-    t2.attach( callback(this, &SpeedControl::speedCtrl), 0.05f);  // SpeedControl:: allows you to access funtion within the class file
-
+    t2.attach( callback(this, &SpeedControl::speedCtrl), PERIOD);  // SpeedControl:: allows you to access funtion within the class file
 }
 
 //Destructor
@@ -51,47 +50,22 @@
     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;
+    // Berechnen der Motorspannungen Uout
+    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
 
-    /*
-    // 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
-    */
-    // + KI*integralErrorLeft and KI*integralErrorRight + were removed from below
-    // P + I Control!
-    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;    // Max_voltage is 12.0f
+    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;
@@ -105,7 +79,7 @@
 //set desired wheel speed in RPM
 void SpeedControl::setDesiredSpeed( float L, float R)
 {
-    desiredSpeedLeft = L * 1.1f ;
+    desiredSpeedLeft = L * 1.18f ;
     desiredSpeedRight = R;
 }