File content as of revision 3:a292bdaf03f6:

#include "Controller.h"

using namespace std;

const float Controller::PERIOD = 0.001f;                    // period of 1 ms
const float Controller::COUNTS_PER_TURN = 1200.0f;         // encoder resolution
const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // given in [rad/s]
const float Controller::KN = 40.0f;                         // speed constant in [rpm/V]
const float Controller::KP = 0.15f;                          // speed control parameter
const float Controller::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum duty-cycle
const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle

Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
                        EncoderCounter& counterLeft, EncoderCounter& counterRight) :
                        pwmLeft(pwmLeft), pwmRight(pwmRight),
                        counterLeft(counterLeft), counterRight(counterRight), thread(osPriorityHigh, STACK_SIZE) {

    // 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;
    
    actualAngleLeft = 0.0f;
    actualAngleRight = 0.0f;

    // Starten des periodischen Tasks
    thread.start(callback(this, &Controller::run));
    ticker.attach(callback(this, &Controller::sendThreadFlag), 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;
}

/**
 * This method is called by the ticker timer interrupt service routine.
 * It sends a flag to the thread to make it run again.
 */
void Controller::sendThreadFlag() {
    
    thread.flags_set(threadFlag);
}

void Controller::run() {

    while(true) {

        // wait for the periodic signal

        ThisThread::flags_wait_any(threadFlag);

        // calculate actual speed of motors 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);

        actualAngleLeft = actualAngleLeft + actualSpeedLeft/60.0f*PERIOD;
        actualAngleRight = actualAngleRight + actualSpeedRight/60.0f*PERIOD;

        // calculate motor phase voltages

        float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
        float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;

        // calculate, limit and set duty cycles
        
        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.write(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.write(dutyCycleRight);

    }
}