![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Fertige Version mit geschwindigkeit 100
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Controller.cpp
- Committer:
- ruesipat
- Date:
- 2018-04-04
- Revision:
- 2:592f01278db4
- Parent:
- 1:d9e840c48b1e
- Child:
- 5:b8b1a979b0d5
File content as of revision 2:592f01278db4:
#include "Controller.h" using namespace std; const float Controller::PERIOD = 0.001f; // Periode von 1 ms //const float Controller::PERIOD = 0.001f; // Periode von 1 ms const float Controller::COUNTS_PER_TURN = 1560.0f; // Encoder-Aufloesung //const float Controller::COUNTS_PER_TURN = 1200.0f; // Encoder-Aufloesung (20Counts rp Umdrehung und Getriebe 78:1) => 1560 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s] const float Controller::KN = 15.0f; // Drehzahlkonstante in [rpm/V] //const float Controller::KN = 40.0f; // Drehzahlkonstante in [rpm/V] Nenndrehzahl durch max Spannung = 180Rpm/12V const float Controller::KP = 0.25f; // KP Regler-Parameter //const float Controller::KP = 0.25f; // KP Regler-Parameter const float Controller::KI = 4.0f; // KI Regler-Parameter //const float Controller::KI = 4.0f; // KI Regler-Parameter const float Controller::I_MAX = 10000.0f; // KI Regler-Parameter Saettigung const float Controller::MAX_VOLTAGE = 12.0f; // Batteriespannung in [V] const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimale Duty-Cycle const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximale 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; // Starten des periodischen Tasks ticker.attach(callback(this, &Controller::run), 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; } float Controller::getIntegralLeft() { return iSumLeft; } float Controller::getIntegralRight() { return iSumRight; } float Controller::getProportionalLeft() { return (desiredSpeedLeft-actualSpeedLeft); } float Controller::getProportionalRight() { return (desiredSpeedRight-actualSpeedRight); } void Controller::run() { // Berechnen die effektiven Drehzahlen der Motoren 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); //Berechnung I - Anteil iSumLeft += (desiredSpeedLeft-actualSpeedLeft); if (iSumLeft > I_MAX) iSumLeft = I_MAX; //Max Saettigung I - Anteil if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil iSumRight += (desiredSpeedRight-actualSpeedRight); if (iSumRight > I_MAX) iSumRight = I_MAX; //Max Saettigung I - Anteil if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil // Berechnen der Motorspannungen Uout float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD +desiredSpeedLeft/KN; float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD +desiredSpeedRight/KN; // Berechnen, Limitieren und Setzen der Duty-Cycle 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 = 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 = dutyCycleRight; }