Controller.cpp

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