Controller.cpp

00001 #include "Controller.h"
00002 
00003 using namespace std;
00004 
00005 const float Controller::PERIOD = 0.001f; // Periode von 1 ms
00006 const float Controller::COUNTS_PER_TURN = 1200.0f; // Encoder-Aufloesung
00007 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s]
00008 const float Controller::KN = 40.0f; // Drehzahlkonstante in [rpm/V]
00009 const float Controller::KP = 0.25f; // KP Regler-Parameter
00010 const float Controller::KI = 4.0f; // KI Regler-Parameter
00011 const float Controller::I_MAX = 10000.0f; // KI Regler-Parameter Saettigung
00012 const float Controller::MAX_VOLTAGE = 12.0f; // Batteriespannung in [V]
00013 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimale Duty-Cycle
00014 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximale Duty-Cycle
00015 
00016 int ii =0;
00017 
00018 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
00019                         EncoderCounter& counterLeft, EncoderCounter& counterRight) :
00020                         pwmLeft(pwmLeft), pwmRight(pwmRight),
00021                         counterLeft(counterLeft), counterRight(counterRight) {
00022 
00023     // Initialisieren der PWM Ausgaenge
00024     
00025     pwmLeft.period(0.00005f); // PWM Periode von 50 us
00026     pwmLeft = 0.5f; // Duty-Cycle von 50%
00027     pwmRight.period(0.00005f); // PWM Periode von 50 us
00028     pwmRight = 0.5f; // Duty-Cycle von 50%
00029 
00030     // Initialisieren von lokalen Variabeln
00031 
00032     previousValueCounterLeft = counterLeft.read();
00033     previousValueCounterRight = counterRight.read();
00034 
00035     speedLeftFilter.setPeriod(PERIOD);
00036     speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00037 
00038     speedRightFilter.setPeriod(PERIOD);
00039     speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00040 
00041     desiredSpeedLeft = 0.0f;
00042     desiredSpeedRight = 0.0f;
00043 
00044     actualSpeedLeft = 0.0f;
00045     actualSpeedRight = 0.0f;
00046 
00047     // Starten des periodischen Tasks
00048     ticker.attach(callback(this, &Controller::run), PERIOD);
00049 }
00050 
00051 Controller::~Controller()
00052 {
00053     ticker.detach(); // Stoppt den periodischen Task
00054 }
00055 
00056 void Controller::resetEncoder(){
00057 ticker.detach();
00058 counterLeft.reset();
00059 counterRight.reset();
00060 ticker.attach(callback(this, &Controller::run), PERIOD);
00061 }
00062 
00063 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
00064 {
00065     this->desiredSpeedLeft = desiredSpeedLeft;
00066 }
00067 
00068 void Controller::setDesiredSpeedRight(float desiredSpeedRight)
00069 {
00070     this->desiredSpeedRight = desiredSpeedRight;
00071 }
00072 
00073 float Controller::getSpeedLeft()
00074 {
00075     return actualSpeedLeft;
00076 }
00077 
00078 float Controller::getSpeedRight()
00079 {
00080     return actualSpeedRight;
00081 }
00082 
00083 float Controller::getIntegralLeft()
00084 {
00085     return iSumLeft;
00086 }
00087 
00088 float Controller::getIntegralRight()
00089 {
00090     return iSumRight;
00091 }
00092 
00093 float Controller::getProportionalLeft()
00094 {
00095     return (desiredSpeedLeft-actualSpeedLeft);
00096 }
00097 
00098 float Controller::getProportionalRight()
00099 {
00100     return (desiredSpeedRight-actualSpeedRight);
00101 }
00102 
00103 void Controller::run() {
00104 
00105     // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
00106     
00107     short valueCounterLeft = counterLeft.read();
00108     short valueCounterRight = counterRight.read();
00109 
00110     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00111     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00112 
00113     previousValueCounterLeft = valueCounterLeft;
00114     previousValueCounterRight = valueCounterRight;
00115 
00116     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft
00117                       /COUNTS_PER_TURN/PERIOD*60.0f);
00118     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight
00119                        /COUNTS_PER_TURN/PERIOD*60.0f);
00120 
00121 
00122     //Berechnung I - Anteil
00123 
00124     
00125     iSumLeft += (desiredSpeedLeft-actualSpeedLeft); 
00126     if (iSumLeft > I_MAX) iSumLeft = I_MAX;  //Max Saettigung I - Anteil       
00127     if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil
00128 
00129     iSumRight += (desiredSpeedRight-actualSpeedRight); 
00130     if (iSumRight > I_MAX) iSumRight = I_MAX;  //Max Saettigung I - Anteil       
00131     if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil
00132        
00133     // Berechnen der Motorspannungen Uout
00134         
00135     float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD
00136                         +desiredSpeedLeft/KN;
00137     float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD
00138                          +desiredSpeedRight/KN;                                   
00139                          
00140     // Berechnen, Limitieren und Setzen der Duty-Cycle
00141     
00142     float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
00143     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
00144     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
00145     pwmLeft = dutyCycleLeft;
00146     
00147     float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
00148     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
00149     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
00150     pwmRight = dutyCycleRight;
00151     
00152 }
00153