Diff: main.cpp

Revision:

1:e454e6f5d81a

Parent:

0:96f88638114b

--- a/main.cpp	Tue Mar 21 14:57:54 2017 +0000
+++ b/main.cpp	Wed May 10 09:15:42 2017 +0000
@@ -1,75 +1,108 @@
 #include "mbed.h"
-#include "IRSensor.h"
+#include "MotorEncoder.h"
+#include "LowpassFilter.h"
 
-//E. Hess
-//Robotterbewegungen
-
-DigitalOut led(LED1);
+const float PERIOD = 0.002f;                    // period of control task, given in [s]
+const float COUNTS_PER_TURN = 1200.0f;          // resolution of encoder counter
+const float LOWPASS_FILTER_FREQUENCY = 300.0f;  // frequency of lowpass filter for actual speed values, given in [rad/s]
+const float KN = 40.0f;                         // speed constant of motor, given in [rpm/V]
+const float KP = 0.2f;                          // speed controller gain, given in [V/rpm]
+const float MAX_VOLTAGE = 12.0f;                // supply voltage for power stage in [V]
+const float MIN_DUTY_CYCLE = 0.02f;             // minimum allowed value for duty cycle (2%)
+const float MAX_DUTY_CYCLE = 0.98f;             // maximum allowed value for duty cycle (98%)
 
-//Erstellt In- / Outputs
-AnalogIn distance(PB_1); 
-DigitalOut enable(PC_1);
-DigitalOut bit0(PH_1);
-DigitalOut bit1(PC_2);
-DigitalOut bit2(PC_3);
-IRSensor sensors[6];        //-> Was
+MotorEncoder counterLeft(PB_6, PB_7);
+MotorEncoder counterRight(PA_6, PC_7);
+
+LowpassFilter speedLeftFilter;
+LowpassFilter speedRightFilter;
+
+DigitalOut enableMotorDriver(PB_2);
+PwmOut pwmLeft(PA_8);
+PwmOut pwmRight(PA_9);
+
+DigitalOut my_led(LED1);
+
+short previousValueCounterRight = 0;
+short previousValueCounterLeft = 0;
+
+float desiredSpeedLeft;
+float desiredSpeedRight;
 
-//LED Indikatoren rund um Roboter
-DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
+float actualSpeedLeft;
+float actualSpeedRight;
 
-//Timer-Objekt für LED- und Distanzsensor
-Ticker t1;
+void speedCtrl()
+{
+    // 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);
 
-//Motoren
-DigitalOut enableMotorDriver(PB_2); //Erstellt das Objekt
-PwmOut pwmL(PA_8);
-PwmOut pwmR(PA_9);
+    // Berechnen der Motorspannungen Uout
+    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
 
-//DistanzLEDs ->Was
-void ledDistance(){
-    for( int ii = 0; ii<6; ++ii)
-        sensors[ii]< 0.1f ? leds[ii] = 1 : leds[ii] = 0;
+    // 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;
 }
 
-//Blinkt beim Start und startet die DistanzLEDs
-void ledShow(){
-    static int timer = 0;
-    for( int ii = 0; ii<6; ++ii)
-        leds[ii] = !leds[ii];
+int main(){
+    
+    // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
+    pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs
+    pwmRight.period(0.00005f);
+    pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
+    pwmRight = 0.5f; // Duty-Cycle von 50%
 
-    //Beendet den Ticker und startet die DistanzLED-Show
-    if( ++timer > 10) {
-        t1.detach();
-        t1.attach( &ledDistance, 0.01f );
-    }
-}
+    // 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);
 
-int main()
-{
-    pwmL.period(0.00005f); // Setzt die Periode auf 50 μs -> f speichert float anstatt double (schneller)
-    pwmR.period(0.00005f);
-    pwmL = 0.5f; // Setzt die Duty-Cycle auf 50%
-    pwmR = 0.5f;
+    desiredSpeedLeft = 0.0f;
+    desiredSpeedRight = 0.0f;
+    actualSpeedLeft = 0.0f;
+    actualSpeedRight = 0.0f;
+
+    Ticker t1;
+    t1.attach( &speedCtrl, PERIOD);
+
+    desiredSpeedLeft = 10.0f; //50 RPM
+    desiredSpeedRight = 10.0f; //50 RPM
     enableMotorDriver = 1;
 
-    t1.attach( &ledShow, 0.05f ); //->Was
-
-    //Initialisiert Distanzsensoren
-    for( int ii = 0; ii<6; ++ii)
-        sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
-
-    enable = 1;
-
     while(1) {
-        wait( 0.2f );
-        if(sensors[0].read() < 0.2f){
-            pwmL = 0.55f;
-            pwmR = 0.55f;
-        }
-        else{
-            pwmL = 0.6f;
-            pwmR = 0.4f;
-        }
-//float x = sensor[0];
+        my_led = !my_led;
+        wait(0.5);
+        
+        
+        
+        
+        
+//        printf( "enc A: %d, enc B:%d \n\r", counterRight.read(), counterLeft.read());
+        
+//        if(i%10 == 0){
+//            desiredSpeedLeft -= 1.0f;
+//            desiredSpeedRight += 1.0f;
+//        }
     }
-}
+}
\ No newline at end of file