Diff: main.cpp

Revision:

0:dfea4e0e064b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 22 10:45:28 2017 +0000
@@ -0,0 +1,107 @@
+/*
+
+Folgendes Programm zeigt einen einfach P-Geschwindigkeitsregler
+Die DIP-Switch auf der Ruekseite des Roboters muessen dazu of "on" stehen.
+
+ACHTUNG:
+Die Motorencoder koennen nicht simultan mit den R/C-Servos gebraucht werden.
+*/
+
+
+#include "mbed.h"
+#include "EncoderCounter.h"
+#include "LowpassFilter.h"
+
+const float PERIOD = 0.001f;                    // 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%)
+
+EncoderCounter counterLeft(PB_6, PB_7);
+EncoderCounter 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;
+
+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()
+{
+    // 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);
+
+    desiredSpeedLeft = 0.0f;
+    desiredSpeedRight = 0.0f;
+    actualSpeedLeft = 0.0f;
+    actualSpeedRight = 0.0f;
+
+    Ticker t1;
+    t1.attach( &speedCtrl, PERIOD);
+
+    desiredSpeedLeft = 50.0f; //50 RPM
+    desiredSpeedRight = -50.0f; //50 RPM
+    enableMotorDriver = 1;
+
+    while(1) {
+        my_led = !my_led;
+        wait(0.5);
+    }
+}