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