Totale Testversion
Dependencies: mbed
Fork of DrehungMitStopp by
main.cpp
- Committer:
- EHess
- Date:
- 2017-05-10
- Revision:
- 1:d40ff07e2fe0
- Parent:
- 0:96f88638114b
- Child:
- 2:365bf16abbf6
File content as of revision 1:d40ff07e2fe0:
#include "mbed.h" #include "IRSensor.h" #include "MotorEncoder.h" #include "LowpassFilter.h" DigitalOut led(LED1); //Zustands-LED: Grüne LED für Benutzer 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]; //Erstellt 6 IRSensor-Objekte als Array DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; //LED-Outputs der Sensoren //Titel printf() void title() { printf("\f < \t\t - \t\t >\n\r"); //"\f" Setzt den Cursor an den Anfang der nächsten Seite } 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; 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 = 15.0f; //50 RPM desiredSpeedRight = 15.0f; //50 RPM enableMotorDriver = 1; float sensorMittelwert[6]; //Array der 6 Sensorenwerte float sensorTiefbass[6]; int zaehler = 0; //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) { 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 } 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); } if(zaehler % 40 == 0) title(); //Erstellt nach 40 Zeilen eine neue Seite } }