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 = 1562.6f; // Encoder-Aufloesung
00007 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s]
00008 const float Controller::KN = 15.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 
00057 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
00058 {
00059     this->desiredSpeedLeft = desiredSpeedLeft;
00060 }
00061 
00062 void Controller::setDesiredSpeedRight(float desiredSpeedRight)
00063 {
00064     this->desiredSpeedRight = desiredSpeedRight;
00065 }
00066 
00067 float Controller::getSpeedLeft()
00068 {
00069     return actualSpeedLeft;
00070 }
00071 
00072 float Controller::getSpeedRight()
00073 {
00074     return actualSpeedRight;
00075 }
00076 
00077 float Controller::getIntegralLeft()
00078 {
00079     return iSumLeft;
00080 }
00081 
00082 float Controller::getIntegralRight()
00083 {
00084     return iSumRight;
00085 }
00086 
00087 float Controller::getProportionalLeft()
00088 {
00089     return (desiredSpeedLeft-actualSpeedLeft);
00090 }
00091 
00092 float Controller::getProportionalRight()
00093 {
00094     return (desiredSpeedRight-actualSpeedRight);
00095 }
00096 
00097 void Controller::run() {
00098 
00099     // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
00100     
00101     short valueCounterLeft = counterLeft.read();
00102     short valueCounterRight = counterRight.read();
00103 
00104     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00105     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00106 
00107     previousValueCounterLeft = valueCounterLeft;
00108     previousValueCounterRight = valueCounterRight;
00109 
00110     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft
00111                       /COUNTS_PER_TURN/PERIOD*60.0f);
00112     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight
00113                        /COUNTS_PER_TURN/PERIOD*60.0f);
00114 
00115 
00116     //Berechnung I - Anteil
00117 
00118     
00119     iSumLeft += (desiredSpeedLeft-actualSpeedLeft); 
00120     if (iSumLeft > I_MAX) iSumLeft = I_MAX;  //Max Saettigung I - Anteil       
00121     if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil
00122 
00123     iSumRight += (desiredSpeedRight-actualSpeedRight); 
00124     if (iSumRight > I_MAX) iSumRight = I_MAX;  //Max Saettigung I - Anteil       
00125     if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil
00126        
00127     // Berechnen der Motorspannungen Uout
00128         
00129     float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD
00130                         +desiredSpeedLeft/KN;
00131     float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD
00132                          +desiredSpeedRight/KN;                                   
00133                          
00134     // Berechnen, Limitieren und Setzen der Duty-Cycle
00135     
00136     float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
00137     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
00138     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
00139     pwmLeft = dutyCycleLeft;
00140     
00141     float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
00142     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
00143     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
00144     pwmRight = dutyCycleRight;
00145     
00146 }
00147 
00148 void Controller::counterReset() {
00149     
00150     ticker.detach();
00151     counterLeft.reset();
00152     counterRight.reset();
00153     
00154     previousValueCounterLeft = counterLeft.read();
00155     previousValueCounterRight = counterRight.read();
00156     
00157     ticker.attach(callback(this, &Controller::run), PERIOD);
00158     
00159     
00160         
00161 }