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