kings / Mbed 2 deprecated Bewegungen_mit_Sensor

Encoder und Sensorwerte

Dependencies:   mbed

Fork of Bewegungen by kings

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 
Encoder und Sensorwerte

Who changed what in which revision?

UserRevisionLine numberNew 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 }