File content as of revision 3:86f7471eaa79:

#include "Controller.h"
using namespace std;
const float Controller::PERIOD = 0.001f; // Periode von 1 ms
const float Controller::COUNTS_PER_TURN = 1200.0f; // Encoder-Aufloesung
const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s]
const float Controller::KN = 40.0f; // Drehzahlkonstante in [rpm/V]
const float Controller::KP = 0.1f; // Regler-Parameter
const float Controller::MAX_VOLTAGE = 12.0f; // Batteriespannung in [V]
const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimale Duty-Cycle
const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximale Duty-Cycle


Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
                       EncoderCounter& counterLeft, EncoderCounter& counterRight) :
    pwmLeft(pwmLeft), pwmRight(pwmRight),
    counterLeft(counterLeft), counterRight(counterRight)
{
// Initialisieren der PWM Ausgaenge
    pwmLeft.period(0.00005f); // PWM Periode von 50 us
    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;
// Starten des periodischen Tasks
    ticker.attach(callback(this, &Controller::run), PERIOD);
}
Controller::~Controller()
{
    ticker.detach(); // Stoppt den periodischen Task
}
void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
{
    this->desiredSpeedLeft = desiredSpeedLeft;
}
void Controller::setDesiredSpeedRight(float desiredSpeedRight)
{
    this->desiredSpeedRight = desiredSpeedRight;
}
float Controller::getSpeedLeft(void)
{
    return actualSpeedLeft;
}
float Controller::getSpeedRight(void)
{
    return actualSpeedRight;
}
    

void Controller::run()
{
// 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;
}