SpeedControl.cpp

00001 
00002 #include "mbed.h"
00003 #include "SpeedControl.h"
00004 #include "EncoderCounter.h"
00005 #include "LowpassFilter.h"
00006 
00007 long integralErrorRight=0;
00008 long integralErrorLeft=0;
00009 short integralErrorMax=1000;
00010 
00011 
00012 //Constructor
00013 //directly from mainSpeedControl int main()
00014 SpeedControl::SpeedControl(PwmOut* pwmLeft, PwmOut* pwmRight, EncoderCounter* counterLeft, EncoderCounter* counterRight)
00015 {
00016     this->pwmLeft = pwmLeft;
00017     this->pwmRight = pwmRight;
00018 
00019     // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
00020     this->pwmLeft->period(0.00005f); // Setzt die Periode auf 50 μs
00021     this->pwmRight->period(0.00005f);
00022     this->pwmLeft->write( 0.5f); // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
00023     this->pwmRight->write( 0.5f); // Duty-Cycle von 50%
00024 
00025     // Initialisieren von lokalen Variabeln
00026     previousValueCounterLeft = counterLeft->read();
00027     previousValueCounterRight = counterRight->read();
00028     speedLeftFilter.setPeriod(PERIOD);
00029     speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00030     speedRightFilter.setPeriod(PERIOD);
00031     speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00032 
00033     desiredSpeedLeft = 0.0f;
00034     desiredSpeedRight = 0.0f;
00035     actualSpeedLeft = 0.0f;
00036     actualSpeedRight = 0.0f;
00037 
00038     t2.attach( callback(this, &SpeedControl::speedCtrl), PERIOD);  // SpeedControl:: allows you to access funtion within the class file
00039 }
00040 
00041 //Destructor
00042 SpeedControl::~SpeedControl()
00043 {
00044 }
00045 
00046 void SpeedControl::speedCtrl()  //EncoderCounter* counterLeft, EncoderCounter* counterRight
00047 {
00048     //Calculate Effective speeds of the motor [rpm]
00049     short valueCounterLeft = counterLeft->read();
00050     short valueCounterRight = counterRight->read();
00051     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00052     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00053     
00054     previousValueCounterLeft = valueCounterLeft;
00055     previousValueCounterRight = valueCounterRight;
00056     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
00057     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
00058 
00059     // Berechnen der Motorspannungen Uout
00060     float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
00061     float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
00062 
00063     // Berechnen, Limitieren und Setzen der Duty-Cycle
00064     float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
00065     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
00066     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
00067 
00068     *pwmLeft = dutyCycleLeft;
00069     float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
00070     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
00071     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
00072 
00073     *pwmRight = dutyCycleRight;
00074 
00075    // printf("the pwm L and R speeds should be %f and %f \n\r", dutyCycleLeft, dutyCycleRight);
00076 
00077 }
00078 
00079 //set desired wheel speed in RPM
00080 void SpeedControl::setDesiredSpeed( float L, float R)
00081 {
00082     desiredSpeedLeft = L * 1.18f ;
00083     desiredSpeedRight = R;
00084 }
00085