Totale Testversion
Dependencies: mbed
Fork of DrehungMitStopp by
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