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@9:ab19796bf14a, 2018-05-16 (annotated)

Committer:
ruesipat
Date:
Wed May 16 16:41:44 2018 +0000
Revision:
9:ab19796bf14a
Parent:
5:b8b1a979b0d5 
;

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