File content as of revision 1:d9e840c48b1e:

#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.25f; // KP Regler-Parameter
const float Controller::KI = 4.0f; // KI Regler-Parameter
const float Controller::I_MAX = 10000.0f; // KI Regler-Parameter Saettigung
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

int ii =0;

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()
{
    return actualSpeedLeft;
}

float Controller::getSpeedRight()
{
    return actualSpeedRight;
}

float Controller::getIntegralLeft()
{
    return iSumLeft;
}

float Controller::getIntegralRight()
{
    return iSumRight;
}

float Controller::getProportionalLeft()
{
    return (desiredSpeedLeft-actualSpeedLeft);
}

float Controller::getProportionalRight()
{
    return (desiredSpeedRight-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);


    //Berechnung I - Anteil


    iSumLeft += (desiredSpeedLeft-actualSpeedLeft);
    if (iSumLeft > I_MAX) iSumLeft = I_MAX;  //Max Saettigung I - Anteil
    if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil

    iSumRight += (desiredSpeedRight-actualSpeedRight);
    if (iSumRight > I_MAX) iSumRight = I_MAX;  //Max Saettigung I - Anteil
    if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil

    // Berechnen der Motorspannungen Uout

    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD
                        +desiredSpeedLeft/KN;
    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD
                         +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;

}