PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FOUR

Pathfinding nach rechts funktioniert noch nicht...der rest schon

Dependencies:   mbed

Fork of MicroMouse_MASTER_THREE by PES2_R2D2.0

Controller.cpp@2:592f01278db4, 2018-04-04 (annotated)

Committer:
ruesipat
Date:
Wed Apr 04 15:24:28 2018 +0000
Revision:
2:592f01278db4
Parent:
1:d9e840c48b1e
Child:
5:b8b1a979b0d5
p

Who changed what in which revision?

UserRevisionLine numberNew 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 2:592f01278db4 19 EncoderCounter& counterLeft, EncoderCounter& counterRight) :
ruesipat 2:592f01278db4 20 pwmLeft(pwmLeft), pwmRight(pwmRight),
ruesipat 2:592f01278db4 21 counterLeft(counterLeft), counterRight(counterRight) {
ruesipat 0:a9fe4ef404bf 22
ruesipat 0:a9fe4ef404bf 23 // Initialisieren der PWM Ausgaenge
ruesipat 2:592f01278db4 24
ruesipat 0:a9fe4ef404bf 25 pwmLeft.period(0.00005f); // PWM Periode von 50 us
ruesipat 0:a9fe4ef404bf 26 pwmLeft = 0.5f; // Duty-Cycle von 50%
ruesipat 0:a9fe4ef404bf 27 pwmRight.period(0.00005f); // PWM Periode von 50 us
ruesipat 0:a9fe4ef404bf 28 pwmRight = 0.5f; // Duty-Cycle von 50%
ruesipat 0:a9fe4ef404bf 29
ruesipat 0:a9fe4ef404bf 30 // Initialisieren von lokalen Variabeln
ruesipat 0:a9fe4ef404bf 31
ruesipat 0:a9fe4ef404bf 32 previousValueCounterLeft = counterLeft.read();
ruesipat 0:a9fe4ef404bf 33 previousValueCounterRight = counterRight.read();
ruesipat 0:a9fe4ef404bf 34
ruesipat 0:a9fe4ef404bf 35 speedLeftFilter.setPeriod(PERIOD);
ruesipat 0:a9fe4ef404bf 36 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
ruesipat 0:a9fe4ef404bf 37
ruesipat 0:a9fe4ef404bf 38 speedRightFilter.setPeriod(PERIOD);
ruesipat 0:a9fe4ef404bf 39 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
ruesipat 0:a9fe4ef404bf 40
ruesipat 0:a9fe4ef404bf 41 desiredSpeedLeft = 0.0f;
ruesipat 0:a9fe4ef404bf 42 desiredSpeedRight = 0.0f;
ruesipat 0:a9fe4ef404bf 43
ruesipat 0:a9fe4ef404bf 44 actualSpeedLeft = 0.0f;
ruesipat 0:a9fe4ef404bf 45 actualSpeedRight = 0.0f;
ruesipat 0:a9fe4ef404bf 46
ruesipat 0:a9fe4ef404bf 47 // Starten des periodischen Tasks
ruesipat 0:a9fe4ef404bf 48 ticker.attach(callback(this, &Controller::run), PERIOD);
ruesipat 0:a9fe4ef404bf 49 }
ruesipat 0:a9fe4ef404bf 50
ruesipat 0:a9fe4ef404bf 51 Controller::~Controller()
ruesipat 0:a9fe4ef404bf 52 {
ruesipat 0:a9fe4ef404bf 53 ticker.detach(); // Stoppt den periodischen Task
ruesipat 0:a9fe4ef404bf 54 }
ruesipat 0:a9fe4ef404bf 55
ruesipat 0:a9fe4ef404bf 56
ruesipat 0:a9fe4ef404bf 57 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
ruesipat 0:a9fe4ef404bf 58 {
ruesipat 0:a9fe4ef404bf 59 this->desiredSpeedLeft = desiredSpeedLeft;
ruesipat 0:a9fe4ef404bf 60 }
ruesipat 0:a9fe4ef404bf 61
ruesipat 0:a9fe4ef404bf 62 void Controller::setDesiredSpeedRight(float desiredSpeedRight)
ruesipat 0:a9fe4ef404bf 63 {
ruesipat 0:a9fe4ef404bf 64 this->desiredSpeedRight = desiredSpeedRight;
ruesipat 0:a9fe4ef404bf 65 }
ruesipat 0:a9fe4ef404bf 66
ruesipat 1:d9e840c48b1e 67 float Controller::getSpeedLeft()
ruesipat 1:d9e840c48b1e 68 {
ruesipat 1:d9e840c48b1e 69 return actualSpeedLeft;
ruesipat 1:d9e840c48b1e 70 }
ruesipat 1:d9e840c48b1e 71
ruesipat 1:d9e840c48b1e 72 float Controller::getSpeedRight()
ruesipat 1:d9e840c48b1e 73 {
ruesipat 1:d9e840c48b1e 74 return actualSpeedRight;
ruesipat 1:d9e840c48b1e 75 }
ruesipat 1:d9e840c48b1e 76
ruesipat 1:d9e840c48b1e 77 float Controller::getIntegralLeft()
ruesipat 1:d9e840c48b1e 78 {
ruesipat 1:d9e840c48b1e 79 return iSumLeft;
ruesipat 1:d9e840c48b1e 80 }
ruesipat 1:d9e840c48b1e 81
ruesipat 1:d9e840c48b1e 82 float Controller::getIntegralRight()
ruesipat 1:d9e840c48b1e 83 {
ruesipat 1:d9e840c48b1e 84 return iSumRight;
ruesipat 1:d9e840c48b1e 85 }
ruesipat 1:d9e840c48b1e 86
ruesipat 1:d9e840c48b1e 87 float Controller::getProportionalLeft()
ruesipat 1:d9e840c48b1e 88 {
ruesipat 1:d9e840c48b1e 89 return (desiredSpeedLeft-actualSpeedLeft);
ruesipat 1:d9e840c48b1e 90 }
ruesipat 1:d9e840c48b1e 91
ruesipat 1:d9e840c48b1e 92 float Controller::getProportionalRight()
ruesipat 1:d9e840c48b1e 93 {
ruesipat 1:d9e840c48b1e 94 return (desiredSpeedRight-actualSpeedRight);
ruesipat 1:d9e840c48b1e 95 }
ruesipat 1:d9e840c48b1e 96
ruesipat 2:592f01278db4 97 void Controller::run() {
ruesipat 0:a9fe4ef404bf 98
ruesipat 0:a9fe4ef404bf 99 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
ruesipat 2:592f01278db4 100
ruesipat 0:a9fe4ef404bf 101 short valueCounterLeft = counterLeft.read();
ruesipat 0:a9fe4ef404bf 102 short valueCounterRight = counterRight.read();
ruesipat 0:a9fe4ef404bf 103
ruesipat 0:a9fe4ef404bf 104 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
ruesipat 0:a9fe4ef404bf 105 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
ruesipat 0:a9fe4ef404bf 106
ruesipat 0:a9fe4ef404bf 107 previousValueCounterLeft = valueCounterLeft;
ruesipat 0:a9fe4ef404bf 108 previousValueCounterRight = valueCounterRight;
ruesipat 0:a9fe4ef404bf 109
ruesipat 0:a9fe4ef404bf 110 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft
ruesipat 0:a9fe4ef404bf 111 /COUNTS_PER_TURN/PERIOD*60.0f);
ruesipat 0:a9fe4ef404bf 112 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight
ruesipat 0:a9fe4ef404bf 113 /COUNTS_PER_TURN/PERIOD*60.0f);
ruesipat 0:a9fe4ef404bf 114
ruesipat 1:d9e840c48b1e 115
ruesipat 2:592f01278db4 116
ruesipat 1:d9e840c48b1e 117 //Berechnung I - Anteil
ruesipat 1:d9e840c48b1e 118
ruesipat 2:592f01278db4 119
ruesipat 2:592f01278db4 120 iSumLeft += (desiredSpeedLeft-actualSpeedLeft);
ruesipat 2:592f01278db4 121 if (iSumLeft > I_MAX) iSumLeft = I_MAX; //Max Saettigung I - Anteil
ruesipat 1:d9e840c48b1e 122 if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil
ruesipat 1:d9e840c48b1e 123
ruesipat 2:592f01278db4 124 iSumRight += (desiredSpeedRight-actualSpeedRight);
ruesipat 2:592f01278db4 125 if (iSumRight > I_MAX) iSumRight = I_MAX; //Max Saettigung I - Anteil
ruesipat 1:d9e840c48b1e 126 if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil
ruesipat 2:592f01278db4 127
ruesipat 0:a9fe4ef404bf 128 // Berechnen der Motorspannungen Uout
ruesipat 2:592f01278db4 129
ruesipat 1:d9e840c48b1e 130 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD
ruesipat 1:d9e840c48b1e 131 +desiredSpeedLeft/KN;
ruesipat 1:d9e840c48b1e 132 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD
ruesipat 2:592f01278db4 133 +desiredSpeedRight/KN;
ruesipat 2:592f01278db4 134
ruesipat 0:a9fe4ef404bf 135 // Berechnen, Limitieren und Setzen der Duty-Cycle
ruesipat 2:592f01278db4 136
ruesipat 0:a9fe4ef404bf 137 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
ruesipat 0:a9fe4ef404bf 138 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
ruesipat 0:a9fe4ef404bf 139 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
ruesipat 0:a9fe4ef404bf 140 pwmLeft = dutyCycleLeft;
ruesipat 2:592f01278db4 141
ruesipat 0:a9fe4ef404bf 142 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
ruesipat 0:a9fe4ef404bf 143 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
ruesipat 0:a9fe4ef404bf 144 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
ruesipat 0:a9fe4ef404bf 145 pwmRight = dutyCycleRight;
ruesipat 2:592f01278db4 146
ruesipat 2:592f01278db4 147
ruesipat 0:a9fe4ef404bf 148 }