Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Controller.cpp@36:99f60052c746, 2018-06-30 (annotated)

Committer:
luethale
Date:
Sat Jun 30 14:13:27 2018 +0000
Revision:
36:99f60052c746
Parent:
1:2b5f79285a3e 
Nacht_der_Technik; Angepasste Werte:; Move & HalfeMove; Ev. Bechleunigung verbessern (aprupte Stops); Ev. Move & HalfeMove weiter anpassen

Who changed what in which revision?

UserRevisionLine numberNew 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 }