File content as of revision 17:ec52258b9472:

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

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


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

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

    t2.attach( callback(this, &SpeedControl::speedCtrl), PERIOD);  // SpeedControl:: allows you to access funtion within the class file
}

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

void SpeedControl::speedCtrl()  //EncoderCounter* counterLeft, EncoderCounter* counterRight
{
    //Calculate Effective speeds of the motor [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;

   // printf("the pwm L and R speeds should be %f and %f \n\r", dutyCycleLeft, dutyCycleRight);

}

//set desired wheel speed in RPM
void SpeedControl::setDesiredSpeed( float L, float R)
{
    desiredSpeedLeft = L * 1.18f ;
    desiredSpeedRight = R;
}