Controller.cpp

00001 #include "Controller.h"
00002 
00003 using namespace std;
00004 
00005 const float Controller::PERIOD = 0.001f;                    // period of 1 ms
00006 const float Controller::COUNTS_PER_TURN = 1200.0f;         // encoder resolution
00007 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // given in [rad/s]
00008 const float Controller::KN = 40.0f;                         // speed constant in [rpm/V]
00009 const float Controller::KP = 0.15f;                          // speed control parameter
00010 const float Controller::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
00011 const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum duty-cycle
00012 const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle
00013 
00014 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
00015                         EncoderCounter& counterLeft, EncoderCounter& counterRight) :
00016                         pwmLeft(pwmLeft), pwmRight(pwmRight),
00017                         counterLeft(counterLeft), counterRight(counterRight), thread(osPriorityHigh, STACK_SIZE) {
00018 
00019     // Initialisieren der PWM Ausgaenge
00020     
00021     pwmLeft.period(0.00005f); // PWM Periode von 50 us
00022     pwmLeft = 0.5f; // Duty-Cycle von 50%
00023     pwmRight.period(0.00005f); // PWM Periode von 50 us
00024     pwmRight = 0.5f; // Duty-Cycle von 50%
00025 
00026     // Initialisieren von lokalen Variabeln
00027 
00028     previousValueCounterLeft = counterLeft.read();
00029     previousValueCounterRight = counterRight.read();
00030 
00031     speedLeftFilter.setPeriod(PERIOD);
00032     speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00033 
00034     speedRightFilter.setPeriod(PERIOD);
00035     speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00036 
00037     desiredSpeedLeft = 0.0f;
00038     desiredSpeedRight = 0.0f;
00039 
00040     actualSpeedLeft = 0.0f;
00041     actualSpeedRight = 0.0f;
00042     
00043     actualAngleLeft = 0.0f;
00044     actualAngleRight = 0.0f;
00045 
00046     // Starten des periodischen Tasks
00047     thread.start(callback(this, &Controller::run));
00048     ticker.attach(callback(this, &Controller::sendThreadFlag), 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 /**
00078  * This method is called by the ticker timer interrupt service routine.
00079  * It sends a flag to the thread to make it run again.
00080  */
00081 void Controller::sendThreadFlag() {
00082     
00083     thread.flags_set(threadFlag);
00084 }
00085 
00086 void Controller::run() {
00087 
00088     while(true) {
00089 
00090         // wait for the periodic signal
00091 
00092         ThisThread::flags_wait_any(threadFlag);
00093 
00094         // calculate actual speed of motors in [rpm]
00095 
00096         short valueCounterLeft = counterLeft.read();
00097         short valueCounterRight = counterRight.read();
00098 
00099         short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00100         short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00101 
00102         previousValueCounterLeft = valueCounterLeft;
00103         previousValueCounterRight = valueCounterRight;
00104 
00105         actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
00106         actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
00107 
00108         actualAngleLeft = actualAngleLeft + actualSpeedLeft/60.0f*PERIOD;
00109         actualAngleRight = actualAngleRight + actualSpeedRight/60.0f*PERIOD;
00110 
00111         // calculate motor phase voltages
00112 
00113         float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
00114         float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
00115 
00116         // calculate, limit and set duty cycles
00117         
00118         float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
00119         if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
00120         else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
00121         pwmLeft.write(dutyCycleLeft);
00122 
00123         float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
00124         if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
00125         else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
00126         pwmRight.write(dutyCycleRight);
00127 
00128     }
00129 }