![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
PES2_mbed_os_6
Diff: Controller.cpp
- Revision:
- 0:5d4d21d56334
- Child:
- 3:a292bdaf03f6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controller.cpp Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,129 @@ +#include "Controller.h" + +using namespace std; + +const float Controller::PERIOD = 0.001f; // period of 1 ms +const float Controller::COUNTS_PER_TURN = 1200.0f; // encoder resolution +const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // given in [rad/s] +const float Controller::KN = 40.0f; // speed constant in [rpm/V] +const float Controller::KP = 0.2f; // speed control parameter +const float Controller::MAX_VOLTAGE = 12.0f; // battery voltage in [V] +const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum duty-cycle +const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum duty-cycle + +Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, + EncoderCounter& counterLeft, EncoderCounter& counterRight) : + pwmLeft(pwmLeft), pwmRight(pwmRight), + counterLeft(counterLeft), counterRight(counterRight) { + + // Initialisieren der PWM Ausgaenge + + pwmLeft.period(0.00005f); // PWM Periode von 50 us + pwmLeft = 0.5f; // Duty-Cycle von 50% + pwmRight.period(0.00005f); // PWM Periode von 50 us + pwmRight = 0.5f; // Duty-Cycle von 50% + + // Initialisieren von lokalen Variabeln + + previousValueCounterLeft = counterLeft.read(); + previousValueCounterRight = counterRight.read(); + + speedLeftFilter.setPeriod(PERIOD); + speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); + + speedRightFilter.setPeriod(PERIOD); + speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); + + desiredSpeedLeft = 0.0f; + desiredSpeedRight = 0.0f; + + actualSpeedLeft = 0.0f; + actualSpeedRight = 0.0f; + + actualAngleLeft = 0.0f; + actualAngleRight = 0.0f; + + // Starten des periodischen Tasks + thread.start(callback(this, &Controller::run)); + ticker.attach(callback(this, &Controller::sendThreadFlag), PERIOD); +} + +Controller::~Controller() +{ + ticker.detach(); // Stoppt den periodischen Task +} + + +void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) +{ + this->desiredSpeedLeft = desiredSpeedLeft; +} + +void Controller::setDesiredSpeedRight(float desiredSpeedRight) +{ + this->desiredSpeedRight = desiredSpeedRight; +} + +float Controller::getSpeedLeft() +{ + return actualSpeedLeft; +} + +float Controller::getSpeedRight() +{ + return actualSpeedRight; +} + +/** + * This method is called by the ticker timer interrupt service routine. + * It sends a flag to the thread to make it run again. + */ +void Controller::sendThreadFlag() { + + thread.flags_set(threadFlag); +} + +void Controller::run() { + + while(true) { + + // wait for the periodic signal + + ThisThread::flags_wait_any(threadFlag); + + // calculate actual speed of motors in [rpm] + + short valueCounterLeft = counterLeft.read(); + short valueCounterRight = counterRight.read(); + + short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft; + short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight; + + previousValueCounterLeft = valueCounterLeft; + previousValueCounterRight = valueCounterRight; + + actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f); + actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f); + + actualAngleLeft = actualAngleLeft + actualSpeedLeft/60.0f*PERIOD; + actualAngleRight = actualAngleRight + actualSpeedRight/60.0f*PERIOD; + + // calculate motor phase voltages + + float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN; + float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN; + + // calculate, limit and set duty cycles + + float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; + if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE; + else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE; + pwmLeft.write(dutyCycleLeft); + + float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE; + if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE; + else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE; + pwmRight.write(dutyCycleRight); + + } +}