Diff: Controller.cpp

Revision:

5:b8b1a979b0d5

Parent:

2:592f01278db4

--- a/Controller.cpp	Thu Apr 12 16:14:02 2018 +0000
+++ b/Controller.cpp	Wed Apr 25 12:07:03 2018 +0000
@@ -16,12 +16,13 @@
 
 
 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
-                        EncoderCounter& counterLeft, EncoderCounter& counterRight) :
-                        pwmLeft(pwmLeft), pwmRight(pwmRight),
-                        counterLeft(counterLeft), counterRight(counterRight) {
+                       EncoderCounter& counterLeft, EncoderCounter& counterRight) :
+    pwmLeft(pwmLeft), pwmRight(pwmRight),
+    counterLeft(counterLeft), counterRight(counterRight)
+{
 
     // Initialisieren der PWM Ausgaenge
-    
+
     pwmLeft.period(0.00005f); // PWM Periode von 50 us
     pwmLeft = 0.5f; // Duty-Cycle von 50%
     pwmRight.period(0.00005f); // PWM Periode von 50 us
@@ -94,10 +95,21 @@
     return (desiredSpeedRight-actualSpeedRight);
 }
 
-void Controller::run() {
+void Controller::reset()
+{
+    ticker.detach();
+    counterRight.reset();
+    counterLeft.reset();
+    previousValueCounterLeft = counterLeft.read();
+    previousValueCounterRight = counterRight.read();
+    ticker.attach(callback(this, &Controller::run), PERIOD);
+}
+
+void Controller::run()
+{
 
     // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
-    
+
     short valueCounterLeft = counterLeft.read();
     short valueCounterRight = counterRight.read();
 
@@ -116,33 +128,33 @@
 
     //Berechnung I - Anteil
 
-    
-    iSumLeft += (desiredSpeedLeft-actualSpeedLeft); 
-    if (iSumLeft > I_MAX) iSumLeft = I_MAX;  //Max Saettigung I - Anteil       
+
+    iSumLeft += (desiredSpeedLeft-actualSpeedLeft);
+    if (iSumLeft > I_MAX) iSumLeft = I_MAX;  //Max Saettigung I - Anteil
     if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil
 
-    iSumRight += (desiredSpeedRight-actualSpeedRight); 
-    if (iSumRight > I_MAX) iSumRight = I_MAX;  //Max Saettigung I - Anteil       
+    iSumRight += (desiredSpeedRight-actualSpeedRight);
+    if (iSumRight > I_MAX) iSumRight = I_MAX;  //Max Saettigung I - Anteil
     if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil
-       
+
     // Berechnen der Motorspannungen Uout
-        
+
     float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD
                         +desiredSpeedLeft/KN;
     float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD
-                         +desiredSpeedRight/KN;                                   
-                         
+                         +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;
-    
-    
+
+
 }
\ No newline at end of file