Nicolas Borla
/
PES2_mbed_os
PES2_mbed_os_6
Controller.cpp
- Committer:
- boro
- Date:
- 2021-03-12
- Revision:
- 0:5d4d21d56334
- Child:
- 3:a292bdaf03f6
File content as of revision 0:5d4d21d56334:
#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); } }