File content as of revision 8:351b0b7b05b2:

#include "mbed.h"
#include "SpeedControl.h"
#include "EncoderCounter.h"
#include "LowpassFilter.h"

//Constructor    
//directly from mainSpeedControl int main()
SpeedControl::SpeedControl(PwmOut* pwmLeft, PwmOut* pwmRight, EncoderCounter* counterLeft, EncoderCounter* counterRight)
{
    this->pwmLeft = pwmLeft;
    this->pwmRight = pwmRight;
    //this->counterLeft =counterLeft;

          // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
    this->pwmLeft->period(0.00005f); // Setzt die Periode auf 50 μs
    this->pwmRight->period(0.00005f);
    this->pwmLeft->write( 0.5f); // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
    this->pwmRight->write( 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;

    Ticker t2;
    t2.attach( this, &SpeedControl::speedCtrl, 0.05f);  // SpeedControl:: allows you to access funtion within the class file

    //desiredSpeedLeft = 50.0f; //50 RPM
    //desiredSpeedRight = -50.0f; //50 RPM
    // enableMotorDriver = 1;

    //for testing
    /*
        while(1) {
            my_led = !my_led;
            wait(0.2);

            printf("encL: %d, encR: %d\n\r", counterLeft.read(), counterRight.read());
        }
    */
}

//Destructor
SpeedControl::~SpeedControl()
{
}


LowpassFilter speedLeftFilter;
LowpassFilter speedRightFilter;

long integralErrorRight=0;
long integralErrorLeft=0;
short integralErrorMax=1000;


void SpeedControl::speedCtrl()  //EncoderCounter* counterLeft, EncoderCounter* counterRight
{
    // 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);

    //Error of each wheel from Pauls I code
    short eL = desiredSpeedLeft-actualSpeedLeft;
    short eR = desiredSpeedRight-actualSpeedRight;

    // Increment the error integral for each wheel
    integralErrorRight += eR;
    integralErrorLeft += eL;
    // Limit the integral term
    if (integralErrorRight > integralErrorMax)
        integralErrorRight = integralErrorMax;
    if (integralErrorRight < -integralErrorMax)
        integralErrorRight = -integralErrorMax;
    if (integralErrorLeft > integralErrorMax)
        integralErrorLeft = integralErrorMax;
    if (integralErrorLeft < -integralErrorMax)
        integralErrorLeft = -integralErrorMax;
    //end of Paul's I control

    // P + I Control!
    float voltageLeft = KP*eL + KI*integralErrorLeft + desiredSpeedLeft/KN;
    float voltageRight = KP*eR + KI*integralErrorRight + 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;
}

void SpeedControl::setDesiredSpeed( float L, float R)
{
    desiredSpeedLeft = L;
    desiredSpeedRight = R;
}