File content as of revision 4:9c003c402033:

#include "SpeedController.h"

using namespace std;

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

SpeedController::SpeedController(float COUNTS_PER_TURN, float KN, float KP, float MAX_VOLTAGE, PwmOut& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
{
    this->COUNTS_PER_TURN = COUNTS_PER_TURN;
    this->KN = KN;
    this->KP = KP;
    this->MAX_VOLTAGE = MAX_VOLTAGE;

    // Initialisieren der PWM Ausgaenge
    pwm.period(0.00005f); // PWM Periode von 50 us
    pwm = 0.5f;           // Duty-Cycle von 50%

    // Initialisieren von lokalen Variabeln
    previousValueCounter = encoderCounter.read();
    speedFilter.setPeriod(PERIOD);
    speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
    desiredSpeed = 0.0f;
    actualSpeed = 0.0f;
    // actualAngle = 0.0f;

    // Starten des periodischen Tasks
    thread.start(callback(this, &SpeedController::run));
    ticker.attach(callback(this, &SpeedController::sendThreadFlag), PERIOD);
}

SpeedController::~SpeedController()
{
    ticker.detach(); // Stoppt den periodischen Task
}

void SpeedController::setDesiredSpeedRPM(float desiredSpeed)
{
    this->desiredSpeed = desiredSpeed;
}

float SpeedController::getSpeedRPM()
{
    return actualSpeed;
}

void SpeedController::setDesiredSpeedRPS(float desiredSpeed)
{
    this->desiredSpeed = desiredSpeed*60.0f;
}

float SpeedController::getSpeedRPS()
{
    return actualSpeed/60.0f;
}

void SpeedController::run()
{
    while(true) {

        // wait for the periodic signal
        ThisThread::flags_wait_any(threadFlag);

        // calculate actual speed of motors in [rpm]
        short valueCounter = encoderCounter.read();
        short countsInPastPeriod = valueCounter - previousValueCounter;
        previousValueCounter = valueCounter;
        actualSpeed = speedFilter.filter((float)countsInPastPeriod/COUNTS_PER_TURN/PERIOD*60.0f);
        // actualAngle = actualAngle + actualSpeed/60.0f*PERIOD;

        // calculate motor phase voltages
        float voltage = KP*(desiredSpeed - actualSpeed) + desiredSpeed/KN;

        // calculate, limit and set duty cycles
        float dutyCycle = 0.5f + 0.5f*voltage/MAX_VOLTAGE;
        if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
        else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
        pwm.write(dutyCycle);

    }
}

void SpeedController::sendThreadFlag()
{
    thread.flags_set(threadFlag);
}