Diff: main.cpp

Revision:

1:d40ff07e2fe0

Parent:

0:96f88638114b

Child:

2:365bf16abbf6

diff -r 96f88638114b -r d40ff07e2fe0 main.cpp
--- a/main.cpp	Tue Mar 21 14:57:54 2017 +0000
+++ b/main.cpp	Wed May 10 09:14:12 2017 +0000
@@ -1,75 +1,144 @@
 #include "mbed.h"
 #include "IRSensor.h"
-
-//E. Hess
-//Robotterbewegungen
+#include "MotorEncoder.h"
+#include "LowpassFilter.h"
 
-DigitalOut led(LED1);
+DigitalOut led(LED1);           //Zustands-LED: Grüne LED für Benutzer
 
-//Erstellt In- / Outputs
-AnalogIn distance(PB_1); 
-DigitalOut enable(PC_1);
-DigitalOut bit0(PH_1);
+AnalogIn distance(PB_1);        //Input der Distanzsensoren
+DigitalOut enableSensor(PC_1);  //Aktivierung der IRSensoren
+DigitalOut bit0(PH_1);          //Ansteuerung der Sensoren 0-5 mit 3 Bits
 DigitalOut bit1(PC_2);
 DigitalOut bit2(PC_3);
-IRSensor sensors[6];        //-> Was
+IRSensor sensors[6];            //Erstellt 6 IRSensor-Objekte als Array
 
-//LED Indikatoren rund um Roboter
-DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
-
-//Timer-Objekt für LED- und Distanzsensor
-Ticker t1;
+DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };    //LED-Outputs der Sensoren
 
-//Motoren
-DigitalOut enableMotorDriver(PB_2); //Erstellt das Objekt
-PwmOut pwmL(PA_8);
-PwmOut pwmR(PA_9);
 
-//DistanzLEDs ->Was
-void ledDistance(){
-    for( int ii = 0; ii<6; ++ii)
-        sensors[ii]< 0.1f ? leds[ii] = 1 : leds[ii] = 0;
+//Titel printf()
+void title() {
+    printf("\f  <    \t\t   -   \t\t    >\n\r"); //"\f" Setzt den Cursor an den Anfang der nächsten Seite
 }
 
-//Blinkt beim Start und startet die DistanzLEDs
-void ledShow(){
-    static int timer = 0;
-    for( int ii = 0; ii<6; ++ii)
-        leds[ii] = !leds[ii];
+
+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%)
+
+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;
 
-    //Beendet den Ticker und startet die DistanzLED-Show
-    if( ++timer > 10) {
-        t1.detach();
-        t1.attach( &ledDistance, 0.01f );
-    }
+float actualSpeedLeft;
+float actualSpeedRight;
+
+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);
+
+    // Berechnen der Motorspannungen Uout
+    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+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;
 }
 
-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;
-    enableMotorDriver = 1;
+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%
+
+    // 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);
 
-    t1.attach( &ledShow, 0.05f ); //->Was
+    desiredSpeedLeft = 0.0f;
+    desiredSpeedRight = 0.0f;
+    actualSpeedLeft = 0.0f;
+    actualSpeedRight = 0.0f;
+
+    Ticker t1;
+    t1.attach( &speedCtrl, PERIOD);
 
-    //Initialisiert Distanzsensoren
-    for( int ii = 0; ii<6; ++ii)
-        sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
+    desiredSpeedLeft = 15.0f; //50 RPM
+    desiredSpeedRight = 15.0f; //50 RPM
+    enableMotorDriver = 1;
+    
+    float sensorMittelwert[6];   //Array der 6 Sensorenwerte
+    float sensorTiefbass[6];
+    int zaehler = 0;
 
-    enable = 1;
+    //Initialisiert Distanzsensoren und setzt sensorValue und sensorTiefbass auf NULL
+    for( int i = 0; i < 6; i++) {
+        sensors[i].init(&distance, &bit0, &bit1, &bit2, i);
+        sensorMittelwert[i] = 0.0f;
+        sensorTiefbass[i] = 0.0f;
+    }
+    enableSensor = 1;   //Aktiviert die IRSensoren
 
     while(1) {
-        wait( 0.2f );
-        if(sensors[0].read() < 0.2f){
-            pwmL = 0.55f;
-            pwmR = 0.55f;
+        for(int j = 0; j < 25; j++){                 //Zählt 25 Sensorwerten pro Sensor zusammen
+            for(int i = 0; i < 6; i++){
+                sensorMittelwert[i] += sensors[i].read();
+            }
+            wait( 0.001f );
+        }
+        for(int i = 0; i < 6; i++){
+            sensorTiefbass[i] = sensorTiefbass[i]*0.75f + sensorMittelwert[i]*0.25f;    //Verrechnet den neuen Wert mit dem alten
+            sensorMittelwert[i] = 0.0f;                                                 //Setzt die Sensorwerte auf NULL
         }
-        else{
-            pwmL = 0.6f;
-            pwmR = 0.4f;
+        
+        printf("%f\t%f\t%f\n\r", sensorTiefbass[5], sensorTiefbass[0], sensorTiefbass[1]);   //Plottet die unteren Sensoren + Mitte-Oben
+        
+        zaehler++;
+        if(zaehler % 120 == 0){
+            wait(3.5f);
         }
-//float x = sensor[0];
+        if(zaehler % 40 == 0) title();  //Erstellt nach 40 Zeilen eine neue Seite
     }
-}
+}
\ No newline at end of file