File content as of revision 0:a9fe4ef404bf:

#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.02f; // 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;
}

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