sef
Dependencies: mbed
Fork of Bewegungen_mit_Sensor by
main.cpp@1:d40ff07e2fe0, 2017-05-10 (annotated)
- Committer:
- EHess
- Date:
- Wed May 10 09:14:12 2017 +0000
- Revision:
- 1:d40ff07e2fe0
- Parent:
- 0:96f88638114b
- Child:
- 2:365bf16abbf6
Encoder und Sensorwerte
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
EHess | 0:96f88638114b | 1 | #include "mbed.h" |
EHess | 0:96f88638114b | 2 | #include "IRSensor.h" |
EHess | 1:d40ff07e2fe0 | 3 | #include "MotorEncoder.h" |
EHess | 1:d40ff07e2fe0 | 4 | #include "LowpassFilter.h" |
EHess | 0:96f88638114b | 5 | |
EHess | 1:d40ff07e2fe0 | 6 | DigitalOut led(LED1); //Zustands-LED: Grüne LED für Benutzer |
EHess | 0:96f88638114b | 7 | |
EHess | 1:d40ff07e2fe0 | 8 | AnalogIn distance(PB_1); //Input der Distanzsensoren |
EHess | 1:d40ff07e2fe0 | 9 | DigitalOut enableSensor(PC_1); //Aktivierung der IRSensoren |
EHess | 1:d40ff07e2fe0 | 10 | DigitalOut bit0(PH_1); //Ansteuerung der Sensoren 0-5 mit 3 Bits |
EHess | 0:96f88638114b | 11 | DigitalOut bit1(PC_2); |
EHess | 0:96f88638114b | 12 | DigitalOut bit2(PC_3); |
EHess | 1:d40ff07e2fe0 | 13 | IRSensor sensors[6]; //Erstellt 6 IRSensor-Objekte als Array |
EHess | 0:96f88638114b | 14 | |
EHess | 1:d40ff07e2fe0 | 15 | DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; //LED-Outputs der Sensoren |
EHess | 0:96f88638114b | 16 | |
EHess | 0:96f88638114b | 17 | |
EHess | 1:d40ff07e2fe0 | 18 | //Titel printf() |
EHess | 1:d40ff07e2fe0 | 19 | void title() { |
EHess | 1:d40ff07e2fe0 | 20 | printf("\f < \t\t - \t\t >\n\r"); //"\f" Setzt den Cursor an den Anfang der nächsten Seite |
EHess | 0:96f88638114b | 21 | } |
EHess | 0:96f88638114b | 22 | |
EHess | 1:d40ff07e2fe0 | 23 | |
EHess | 1:d40ff07e2fe0 | 24 | const float PERIOD = 0.002f; // period of control task, given in [s] |
EHess | 1:d40ff07e2fe0 | 25 | const float COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter |
EHess | 1:d40ff07e2fe0 | 26 | const float LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s] |
EHess | 1:d40ff07e2fe0 | 27 | const float KN = 40.0f; // speed constant of motor, given in [rpm/V] |
EHess | 1:d40ff07e2fe0 | 28 | const float KP = 0.2f; // speed controller gain, given in [V/rpm] |
EHess | 1:d40ff07e2fe0 | 29 | const float MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V] |
EHess | 1:d40ff07e2fe0 | 30 | const float MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%) |
EHess | 1:d40ff07e2fe0 | 31 | const float MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%) |
EHess | 1:d40ff07e2fe0 | 32 | |
EHess | 1:d40ff07e2fe0 | 33 | MotorEncoder counterLeft(PB_6, PB_7); |
EHess | 1:d40ff07e2fe0 | 34 | MotorEncoder counterRight(PA_6, PC_7); |
EHess | 1:d40ff07e2fe0 | 35 | |
EHess | 1:d40ff07e2fe0 | 36 | LowpassFilter speedLeftFilter; |
EHess | 1:d40ff07e2fe0 | 37 | LowpassFilter speedRightFilter; |
EHess | 1:d40ff07e2fe0 | 38 | |
EHess | 1:d40ff07e2fe0 | 39 | DigitalOut enableMotorDriver(PB_2); |
EHess | 1:d40ff07e2fe0 | 40 | PwmOut pwmLeft(PA_8); |
EHess | 1:d40ff07e2fe0 | 41 | PwmOut pwmRight(PA_9); |
EHess | 1:d40ff07e2fe0 | 42 | |
EHess | 1:d40ff07e2fe0 | 43 | DigitalOut my_led(LED1); |
EHess | 1:d40ff07e2fe0 | 44 | |
EHess | 1:d40ff07e2fe0 | 45 | short previousValueCounterRight = 0; |
EHess | 1:d40ff07e2fe0 | 46 | short previousValueCounterLeft = 0; |
EHess | 1:d40ff07e2fe0 | 47 | |
EHess | 1:d40ff07e2fe0 | 48 | float desiredSpeedLeft; |
EHess | 1:d40ff07e2fe0 | 49 | float desiredSpeedRight; |
EHess | 0:96f88638114b | 50 | |
EHess | 1:d40ff07e2fe0 | 51 | float actualSpeedLeft; |
EHess | 1:d40ff07e2fe0 | 52 | float actualSpeedRight; |
EHess | 1:d40ff07e2fe0 | 53 | |
EHess | 1:d40ff07e2fe0 | 54 | void speedCtrl() |
EHess | 1:d40ff07e2fe0 | 55 | { |
EHess | 1:d40ff07e2fe0 | 56 | // Berechnen die effektiven Drehzahlen der Motoren in [rpm] |
EHess | 1:d40ff07e2fe0 | 57 | short valueCounterLeft = counterLeft.read(); |
EHess | 1:d40ff07e2fe0 | 58 | short valueCounterRight = counterRight.read(); |
EHess | 1:d40ff07e2fe0 | 59 | short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft; |
EHess | 1:d40ff07e2fe0 | 60 | short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight; |
EHess | 1:d40ff07e2fe0 | 61 | |
EHess | 1:d40ff07e2fe0 | 62 | previousValueCounterLeft = valueCounterLeft; |
EHess | 1:d40ff07e2fe0 | 63 | previousValueCounterRight = valueCounterRight; |
EHess | 1:d40ff07e2fe0 | 64 | actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f); |
EHess | 1:d40ff07e2fe0 | 65 | actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f); |
EHess | 1:d40ff07e2fe0 | 66 | |
EHess | 1:d40ff07e2fe0 | 67 | // Berechnen der Motorspannungen Uout |
EHess | 1:d40ff07e2fe0 | 68 | float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN; |
EHess | 1:d40ff07e2fe0 | 69 | float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN; |
EHess | 1:d40ff07e2fe0 | 70 | |
EHess | 1:d40ff07e2fe0 | 71 | // Berechnen, Limitieren und Setzen der Duty-Cycle |
EHess | 1:d40ff07e2fe0 | 72 | float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; |
EHess | 1:d40ff07e2fe0 | 73 | if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE; |
EHess | 1:d40ff07e2fe0 | 74 | else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE; |
EHess | 1:d40ff07e2fe0 | 75 | |
EHess | 1:d40ff07e2fe0 | 76 | pwmLeft = dutyCycleLeft; |
EHess | 1:d40ff07e2fe0 | 77 | float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE; |
EHess | 1:d40ff07e2fe0 | 78 | if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE; |
EHess | 1:d40ff07e2fe0 | 79 | else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE; |
EHess | 1:d40ff07e2fe0 | 80 | |
EHess | 1:d40ff07e2fe0 | 81 | pwmRight = dutyCycleRight; |
EHess | 0:96f88638114b | 82 | } |
EHess | 0:96f88638114b | 83 | |
EHess | 1:d40ff07e2fe0 | 84 | int main(){ |
EHess | 1:d40ff07e2fe0 | 85 | |
EHess | 1:d40ff07e2fe0 | 86 | // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us |
EHess | 1:d40ff07e2fe0 | 87 | pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs |
EHess | 1:d40ff07e2fe0 | 88 | pwmRight.period(0.00005f); |
EHess | 1:d40ff07e2fe0 | 89 | pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us |
EHess | 1:d40ff07e2fe0 | 90 | pwmRight = 0.5f; // Duty-Cycle von 50% |
EHess | 1:d40ff07e2fe0 | 91 | |
EHess | 1:d40ff07e2fe0 | 92 | // Initialisieren von lokalen Variabeln |
EHess | 1:d40ff07e2fe0 | 93 | previousValueCounterLeft = counterLeft.read(); |
EHess | 1:d40ff07e2fe0 | 94 | previousValueCounterRight = counterRight.read(); |
EHess | 1:d40ff07e2fe0 | 95 | speedLeftFilter.setPeriod(PERIOD); |
EHess | 1:d40ff07e2fe0 | 96 | speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
EHess | 1:d40ff07e2fe0 | 97 | speedRightFilter.setPeriod(PERIOD); |
EHess | 1:d40ff07e2fe0 | 98 | speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
EHess | 0:96f88638114b | 99 | |
EHess | 1:d40ff07e2fe0 | 100 | desiredSpeedLeft = 0.0f; |
EHess | 1:d40ff07e2fe0 | 101 | desiredSpeedRight = 0.0f; |
EHess | 1:d40ff07e2fe0 | 102 | actualSpeedLeft = 0.0f; |
EHess | 1:d40ff07e2fe0 | 103 | actualSpeedRight = 0.0f; |
EHess | 1:d40ff07e2fe0 | 104 | |
EHess | 1:d40ff07e2fe0 | 105 | Ticker t1; |
EHess | 1:d40ff07e2fe0 | 106 | t1.attach( &speedCtrl, PERIOD); |
EHess | 0:96f88638114b | 107 | |
EHess | 1:d40ff07e2fe0 | 108 | desiredSpeedLeft = 15.0f; //50 RPM |
EHess | 1:d40ff07e2fe0 | 109 | desiredSpeedRight = 15.0f; //50 RPM |
EHess | 1:d40ff07e2fe0 | 110 | enableMotorDriver = 1; |
EHess | 1:d40ff07e2fe0 | 111 | |
EHess | 1:d40ff07e2fe0 | 112 | float sensorMittelwert[6]; //Array der 6 Sensorenwerte |
EHess | 1:d40ff07e2fe0 | 113 | float sensorTiefbass[6]; |
EHess | 1:d40ff07e2fe0 | 114 | int zaehler = 0; |
EHess | 0:96f88638114b | 115 | |
EHess | 1:d40ff07e2fe0 | 116 | //Initialisiert Distanzsensoren und setzt sensorValue und sensorTiefbass auf NULL |
EHess | 1:d40ff07e2fe0 | 117 | for( int i = 0; i < 6; i++) { |
EHess | 1:d40ff07e2fe0 | 118 | sensors[i].init(&distance, &bit0, &bit1, &bit2, i); |
EHess | 1:d40ff07e2fe0 | 119 | sensorMittelwert[i] = 0.0f; |
EHess | 1:d40ff07e2fe0 | 120 | sensorTiefbass[i] = 0.0f; |
EHess | 1:d40ff07e2fe0 | 121 | } |
EHess | 1:d40ff07e2fe0 | 122 | enableSensor = 1; //Aktiviert die IRSensoren |
EHess | 0:96f88638114b | 123 | |
EHess | 0:96f88638114b | 124 | while(1) { |
EHess | 1:d40ff07e2fe0 | 125 | for(int j = 0; j < 25; j++){ //Zählt 25 Sensorwerten pro Sensor zusammen |
EHess | 1:d40ff07e2fe0 | 126 | for(int i = 0; i < 6; i++){ |
EHess | 1:d40ff07e2fe0 | 127 | sensorMittelwert[i] += sensors[i].read(); |
EHess | 1:d40ff07e2fe0 | 128 | } |
EHess | 1:d40ff07e2fe0 | 129 | wait( 0.001f ); |
EHess | 1:d40ff07e2fe0 | 130 | } |
EHess | 1:d40ff07e2fe0 | 131 | for(int i = 0; i < 6; i++){ |
EHess | 1:d40ff07e2fe0 | 132 | sensorTiefbass[i] = sensorTiefbass[i]*0.75f + sensorMittelwert[i]*0.25f; //Verrechnet den neuen Wert mit dem alten |
EHess | 1:d40ff07e2fe0 | 133 | sensorMittelwert[i] = 0.0f; //Setzt die Sensorwerte auf NULL |
EHess | 0:96f88638114b | 134 | } |
EHess | 1:d40ff07e2fe0 | 135 | |
EHess | 1:d40ff07e2fe0 | 136 | printf("%f\t%f\t%f\n\r", sensorTiefbass[5], sensorTiefbass[0], sensorTiefbass[1]); //Plottet die unteren Sensoren + Mitte-Oben |
EHess | 1:d40ff07e2fe0 | 137 | |
EHess | 1:d40ff07e2fe0 | 138 | zaehler++; |
EHess | 1:d40ff07e2fe0 | 139 | if(zaehler % 120 == 0){ |
EHess | 1:d40ff07e2fe0 | 140 | wait(3.5f); |
EHess | 0:96f88638114b | 141 | } |
EHess | 1:d40ff07e2fe0 | 142 | if(zaehler % 40 == 0) title(); //Erstellt nach 40 Zeilen eine neue Seite |
EHess | 0:96f88638114b | 143 | } |
EHess | 1:d40ff07e2fe0 | 144 | } |