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::AttachTicker(void)
00057 {
00058     previousValueCounterLeft = counterLeft.read();
00059     previousValueCounterRight = counterRight.read();
00060 
00061     //actualSpeedLeft = 0.0f;
00062     //actualSpeedRight = 0.0f;
00063     ticker.attach(callback(this, &Controller::run), PERIOD);
00064 }
00065 
00066 void Controller::DetachTicker(void)
00067 {
00068     //wait(0.5);
00069     ticker.detach();
00070 }
00071 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
00072 {
00073     this->desiredSpeedLeft = desiredSpeedLeft;
00074 }
00075 
00076 void Controller::setDesiredSpeedRight(float desiredSpeedRight)
00077 {
00078     this->desiredSpeedRight = desiredSpeedRight;
00079 }
00080 
00081 float Controller::getSpeedLeft()
00082 {
00083     return actualSpeedLeft;
00084 }
00085 
00086 float Controller::getSpeedRight()
00087 {
00088     return actualSpeedRight;
00089 }
00090 
00091 float Controller::getIntegralLeft()
00092 {
00093     return iSumLeft;
00094 }
00095 
00096 float Controller::getIntegralRight()
00097 {
00098     return iSumRight;
00099 }
00100 
00101 float Controller::getProportionalLeft()
00102 {
00103     return (desiredSpeedLeft-actualSpeedLeft);
00104 }
00105 
00106 float Controller::getProportionalRight()
00107 {
00108     return (desiredSpeedRight-actualSpeedRight);
00109 }
00110 
00111 void Controller::run() {
00112 
00113     // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
00114     short valueCounterLeft = counterLeft.read();
00115     short valueCounterRight = counterRight.read();
00116 
00117     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00118     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00119 
00120     previousValueCounterLeft = valueCounterLeft;
00121     previousValueCounterRight = valueCounterRight;
00122 
00123     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft
00124                       /COUNTS_PER_TURN/PERIOD*60.0f);
00125     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight
00126                        /COUNTS_PER_TURN/PERIOD*60.0f);
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