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