Alexander Lüthard
/
Micromouse
Alle Prinf's auskommentiert
Fork of Micromouse by
Controller.cpp@19:e2cb5b8a7a29, 2018-05-10 (annotated)
- Committer:
- luethale
- Date:
- Thu May 10 16:33:32 2018 +0000
- Revision:
- 19:e2cb5b8a7a29
- Parent:
- 1:2b5f79285a3e
Alle Printf's auskommentiert
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Helvis | 1:2b5f79285a3e | 1 | #include "Controller.h" |
Helvis | 1:2b5f79285a3e | 2 | |
Helvis | 1:2b5f79285a3e | 3 | using namespace std; |
Helvis | 1:2b5f79285a3e | 4 | |
Helvis | 1:2b5f79285a3e | 5 | const float Controller::PERIOD = 0.001f; // Periode von 1 ms |
Helvis | 1:2b5f79285a3e | 6 | const float Controller::COUNTS_PER_TURN = 1562.6f; // Encoder-Aufloesung |
Helvis | 1:2b5f79285a3e | 7 | const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s] |
Helvis | 1:2b5f79285a3e | 8 | const float Controller::KN = 15.0f; // Drehzahlkonstante in [rpm/V] |
Helvis | 1:2b5f79285a3e | 9 | const float Controller::KP = 0.25f; // KP Regler-Parameter |
Helvis | 1:2b5f79285a3e | 10 | const float Controller::KI = 4.0f; // KI Regler-Parameter |
Helvis | 1:2b5f79285a3e | 11 | const float Controller::I_MAX = 10000.0f; // KI Regler-Parameter Saettigung |
Helvis | 1:2b5f79285a3e | 12 | const float Controller::MAX_VOLTAGE = 12.0f; // Batteriespannung in [V] |
Helvis | 1:2b5f79285a3e | 13 | const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimale Duty-Cycle |
Helvis | 1:2b5f79285a3e | 14 | const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximale Duty-Cycle |
Helvis | 1:2b5f79285a3e | 15 | |
Helvis | 1:2b5f79285a3e | 16 | int ii =0; |
Helvis | 1:2b5f79285a3e | 17 | |
Helvis | 1:2b5f79285a3e | 18 | Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, |
Helvis | 1:2b5f79285a3e | 19 | EncoderCounter& counterLeft, EncoderCounter& counterRight) : |
Helvis | 1:2b5f79285a3e | 20 | pwmLeft(pwmLeft), pwmRight(pwmRight), |
Helvis | 1:2b5f79285a3e | 21 | counterLeft(counterLeft), counterRight(counterRight) { |
Helvis | 1:2b5f79285a3e | 22 | |
Helvis | 1:2b5f79285a3e | 23 | // Initialisieren der PWM Ausgaenge |
Helvis | 1:2b5f79285a3e | 24 | |
Helvis | 1:2b5f79285a3e | 25 | pwmLeft.period(0.00005f); // PWM Periode von 50 us |
Helvis | 1:2b5f79285a3e | 26 | pwmLeft = 0.5f; // Duty-Cycle von 50% |
Helvis | 1:2b5f79285a3e | 27 | pwmRight.period(0.00005f); // PWM Periode von 50 us |
Helvis | 1:2b5f79285a3e | 28 | pwmRight = 0.5f; // Duty-Cycle von 50% |
Helvis | 1:2b5f79285a3e | 29 | |
Helvis | 1:2b5f79285a3e | 30 | // Initialisieren von lokalen Variabeln |
Helvis | 1:2b5f79285a3e | 31 | |
Helvis | 1:2b5f79285a3e | 32 | previousValueCounterLeft = counterLeft.read(); |
Helvis | 1:2b5f79285a3e | 33 | previousValueCounterRight = counterRight.read(); |
Helvis | 1:2b5f79285a3e | 34 | |
Helvis | 1:2b5f79285a3e | 35 | speedLeftFilter.setPeriod(PERIOD); |
Helvis | 1:2b5f79285a3e | 36 | speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
Helvis | 1:2b5f79285a3e | 37 | |
Helvis | 1:2b5f79285a3e | 38 | speedRightFilter.setPeriod(PERIOD); |
Helvis | 1:2b5f79285a3e | 39 | speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
Helvis | 1:2b5f79285a3e | 40 | |
Helvis | 1:2b5f79285a3e | 41 | desiredSpeedLeft = 0.0f; |
Helvis | 1:2b5f79285a3e | 42 | desiredSpeedRight = 0.0f; |
Helvis | 1:2b5f79285a3e | 43 | |
Helvis | 1:2b5f79285a3e | 44 | actualSpeedLeft = 0.0f; |
Helvis | 1:2b5f79285a3e | 45 | actualSpeedRight = 0.0f; |
Helvis | 1:2b5f79285a3e | 46 | |
Helvis | 1:2b5f79285a3e | 47 | // Starten des periodischen Tasks |
Helvis | 1:2b5f79285a3e | 48 | ticker.attach(callback(this, &Controller::run), PERIOD); |
Helvis | 1:2b5f79285a3e | 49 | } |
Helvis | 1:2b5f79285a3e | 50 | |
Helvis | 1:2b5f79285a3e | 51 | Controller::~Controller() |
Helvis | 1:2b5f79285a3e | 52 | { |
Helvis | 1:2b5f79285a3e | 53 | ticker.detach(); // Stoppt den periodischen Task |
Helvis | 1:2b5f79285a3e | 54 | } |
Helvis | 1:2b5f79285a3e | 55 | |
Helvis | 1:2b5f79285a3e | 56 | |
Helvis | 1:2b5f79285a3e | 57 | void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) |
Helvis | 1:2b5f79285a3e | 58 | { |
Helvis | 1:2b5f79285a3e | 59 | this->desiredSpeedLeft = desiredSpeedLeft; |
Helvis | 1:2b5f79285a3e | 60 | } |
Helvis | 1:2b5f79285a3e | 61 | |
Helvis | 1:2b5f79285a3e | 62 | void Controller::setDesiredSpeedRight(float desiredSpeedRight) |
Helvis | 1:2b5f79285a3e | 63 | { |
Helvis | 1:2b5f79285a3e | 64 | this->desiredSpeedRight = desiredSpeedRight; |
Helvis | 1:2b5f79285a3e | 65 | } |
Helvis | 1:2b5f79285a3e | 66 | |
Helvis | 1:2b5f79285a3e | 67 | float Controller::getSpeedLeft() |
Helvis | 1:2b5f79285a3e | 68 | { |
Helvis | 1:2b5f79285a3e | 69 | return actualSpeedLeft; |
Helvis | 1:2b5f79285a3e | 70 | } |
Helvis | 1:2b5f79285a3e | 71 | |
Helvis | 1:2b5f79285a3e | 72 | float Controller::getSpeedRight() |
Helvis | 1:2b5f79285a3e | 73 | { |
Helvis | 1:2b5f79285a3e | 74 | return actualSpeedRight; |
Helvis | 1:2b5f79285a3e | 75 | } |
Helvis | 1:2b5f79285a3e | 76 | |
Helvis | 1:2b5f79285a3e | 77 | float Controller::getIntegralLeft() |
Helvis | 1:2b5f79285a3e | 78 | { |
Helvis | 1:2b5f79285a3e | 79 | return iSumLeft; |
Helvis | 1:2b5f79285a3e | 80 | } |
Helvis | 1:2b5f79285a3e | 81 | |
Helvis | 1:2b5f79285a3e | 82 | float Controller::getIntegralRight() |
Helvis | 1:2b5f79285a3e | 83 | { |
Helvis | 1:2b5f79285a3e | 84 | return iSumRight; |
Helvis | 1:2b5f79285a3e | 85 | } |
Helvis | 1:2b5f79285a3e | 86 | |
Helvis | 1:2b5f79285a3e | 87 | float Controller::getProportionalLeft() |
Helvis | 1:2b5f79285a3e | 88 | { |
Helvis | 1:2b5f79285a3e | 89 | return (desiredSpeedLeft-actualSpeedLeft); |
Helvis | 1:2b5f79285a3e | 90 | } |
Helvis | 1:2b5f79285a3e | 91 | |
Helvis | 1:2b5f79285a3e | 92 | float Controller::getProportionalRight() |
Helvis | 1:2b5f79285a3e | 93 | { |
Helvis | 1:2b5f79285a3e | 94 | return (desiredSpeedRight-actualSpeedRight); |
Helvis | 1:2b5f79285a3e | 95 | } |
Helvis | 1:2b5f79285a3e | 96 | |
Helvis | 1:2b5f79285a3e | 97 | void Controller::run() { |
Helvis | 1:2b5f79285a3e | 98 | |
Helvis | 1:2b5f79285a3e | 99 | // Berechnen die effektiven Drehzahlen der Motoren in [rpm] |
Helvis | 1:2b5f79285a3e | 100 | |
Helvis | 1:2b5f79285a3e | 101 | short valueCounterLeft = counterLeft.read(); |
Helvis | 1:2b5f79285a3e | 102 | short valueCounterRight = counterRight.read(); |
Helvis | 1:2b5f79285a3e | 103 | |
Helvis | 1:2b5f79285a3e | 104 | short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft; |
Helvis | 1:2b5f79285a3e | 105 | short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight; |
Helvis | 1:2b5f79285a3e | 106 | |
Helvis | 1:2b5f79285a3e | 107 | previousValueCounterLeft = valueCounterLeft; |
Helvis | 1:2b5f79285a3e | 108 | previousValueCounterRight = valueCounterRight; |
Helvis | 1:2b5f79285a3e | 109 | |
Helvis | 1:2b5f79285a3e | 110 | actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft |
Helvis | 1:2b5f79285a3e | 111 | /COUNTS_PER_TURN/PERIOD*60.0f); |
Helvis | 1:2b5f79285a3e | 112 | actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight |
Helvis | 1:2b5f79285a3e | 113 | /COUNTS_PER_TURN/PERIOD*60.0f); |
Helvis | 1:2b5f79285a3e | 114 | |
Helvis | 1:2b5f79285a3e | 115 | |
Helvis | 1:2b5f79285a3e | 116 | //Berechnung I - Anteil |
Helvis | 1:2b5f79285a3e | 117 | |
Helvis | 1:2b5f79285a3e | 118 | |
Helvis | 1:2b5f79285a3e | 119 | iSumLeft += (desiredSpeedLeft-actualSpeedLeft); |
Helvis | 1:2b5f79285a3e | 120 | if (iSumLeft > I_MAX) iSumLeft = I_MAX; //Max Saettigung I - Anteil |
Helvis | 1:2b5f79285a3e | 121 | if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil |
Helvis | 1:2b5f79285a3e | 122 | |
Helvis | 1:2b5f79285a3e | 123 | iSumRight += (desiredSpeedRight-actualSpeedRight); |
Helvis | 1:2b5f79285a3e | 124 | if (iSumRight > I_MAX) iSumRight = I_MAX; //Max Saettigung I - Anteil |
Helvis | 1:2b5f79285a3e | 125 | if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil |
Helvis | 1:2b5f79285a3e | 126 | |
Helvis | 1:2b5f79285a3e | 127 | // Berechnen der Motorspannungen Uout |
Helvis | 1:2b5f79285a3e | 128 | |
Helvis | 1:2b5f79285a3e | 129 | float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD |
Helvis | 1:2b5f79285a3e | 130 | +desiredSpeedLeft/KN; |
Helvis | 1:2b5f79285a3e | 131 | float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD |
Helvis | 1:2b5f79285a3e | 132 | +desiredSpeedRight/KN; |
Helvis | 1:2b5f79285a3e | 133 | |
Helvis | 1:2b5f79285a3e | 134 | // Berechnen, Limitieren und Setzen der Duty-Cycle |
Helvis | 1:2b5f79285a3e | 135 | |
Helvis | 1:2b5f79285a3e | 136 | float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; |
Helvis | 1:2b5f79285a3e | 137 | if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE; |
Helvis | 1:2b5f79285a3e | 138 | else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE; |
Helvis | 1:2b5f79285a3e | 139 | pwmLeft = dutyCycleLeft; |
Helvis | 1:2b5f79285a3e | 140 | |
Helvis | 1:2b5f79285a3e | 141 | float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE; |
Helvis | 1:2b5f79285a3e | 142 | if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE; |
Helvis | 1:2b5f79285a3e | 143 | else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE; |
Helvis | 1:2b5f79285a3e | 144 | pwmRight = dutyCycleRight; |
Helvis | 1:2b5f79285a3e | 145 | |
Helvis | 1:2b5f79285a3e | 146 | } |
Helvis | 1:2b5f79285a3e | 147 | |
Helvis | 1:2b5f79285a3e | 148 | void Controller::counterReset() { |
Helvis | 1:2b5f79285a3e | 149 | |
Helvis | 1:2b5f79285a3e | 150 | ticker.detach(); |
Helvis | 1:2b5f79285a3e | 151 | counterLeft.reset(); |
Helvis | 1:2b5f79285a3e | 152 | counterRight.reset(); |
Helvis | 1:2b5f79285a3e | 153 | |
Helvis | 1:2b5f79285a3e | 154 | previousValueCounterLeft = counterLeft.read(); |
Helvis | 1:2b5f79285a3e | 155 | previousValueCounterRight = counterRight.read(); |
Helvis | 1:2b5f79285a3e | 156 | |
Helvis | 1:2b5f79285a3e | 157 | ticker.attach(callback(this, &Controller::run), PERIOD); |
Helvis | 1:2b5f79285a3e | 158 | |
Helvis | 1:2b5f79285a3e | 159 | |
Helvis | 1:2b5f79285a3e | 160 | |
Helvis | 1:2b5f79285a3e | 161 | } |