PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FIVE

ENDLÖSUNG:)

Dependencies:   mbed

Fork of MicroMouse_MASTER_FOUR by PES2_R2D2.0

Controller.cpp@1:d9e840c48b1e, 2018-03-31 (annotated)

Committer:
ruesipat
Date:
Sat Mar 31 16:45:57 2018 +0000
Revision:
1:d9e840c48b1e
Parent:
0:a9fe4ef404bf
Child:
2:592f01278db4
j

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 0:a9fe4ef404bf 5 const float Controller::PERIOD = 0.001f; // Periode von 1 ms
ruesipat 0:a9fe4ef404bf 6 const float Controller::COUNTS_PER_TURN = 1200.0f; // Encoder-Aufloesung
ruesipat 0:a9fe4ef404bf 7 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s]
ruesipat 0:a9fe4ef404bf 8 const float Controller::KN = 40.0f; // Drehzahlkonstante in [rpm/V]
ruesipat 1:d9e840c48b1e 9 const float Controller::KP = 0.25f; // KP Regler-Parameter
ruesipat 1:d9e840c48b1e 10 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 1:d9e840c48b1e 16 int ii =0;
ruesipat 1:d9e840c48b1e 17
ruesipat 0:a9fe4ef404bf 18 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
ruesipat 1:d9e840c48b1e 19 EncoderCounter& counterLeft, EncoderCounter& counterRight) :
ruesipat 1:d9e840c48b1e 20 pwmLeft(pwmLeft), pwmRight(pwmRight),
ruesipat 1:d9e840c48b1e 21 counterLeft(counterLeft), counterRight(counterRight)
ruesipat 1:d9e840c48b1e 22 {
ruesipat 0:a9fe4ef404bf 23
ruesipat 0:a9fe4ef404bf 24 // Initialisieren der PWM Ausgaenge
ruesipat 1:d9e840c48b1e 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 1:d9e840c48b1e 98 void Controller::run()
ruesipat 1:d9e840c48b1e 99 {
ruesipat 0:a9fe4ef404bf 100
ruesipat 0:a9fe4ef404bf 101 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
ruesipat 1:d9e840c48b1e 102
ruesipat 0:a9fe4ef404bf 103 short valueCounterLeft = counterLeft.read();
ruesipat 0:a9fe4ef404bf 104 short valueCounterRight = counterRight.read();
ruesipat 0:a9fe4ef404bf 105
ruesipat 0:a9fe4ef404bf 106 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
ruesipat 0:a9fe4ef404bf 107 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
ruesipat 0:a9fe4ef404bf 108
ruesipat 0:a9fe4ef404bf 109 previousValueCounterLeft = valueCounterLeft;
ruesipat 0:a9fe4ef404bf 110 previousValueCounterRight = valueCounterRight;
ruesipat 0:a9fe4ef404bf 111
ruesipat 0:a9fe4ef404bf 112 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft
ruesipat 0:a9fe4ef404bf 113 /COUNTS_PER_TURN/PERIOD*60.0f);
ruesipat 0:a9fe4ef404bf 114 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight
ruesipat 0:a9fe4ef404bf 115 /COUNTS_PER_TURN/PERIOD*60.0f);
ruesipat 0:a9fe4ef404bf 116
ruesipat 1:d9e840c48b1e 117
ruesipat 1:d9e840c48b1e 118 //Berechnung I - Anteil
ruesipat 1:d9e840c48b1e 119
ruesipat 1:d9e840c48b1e 120
ruesipat 1:d9e840c48b1e 121 iSumLeft += (desiredSpeedLeft-actualSpeedLeft);
ruesipat 1:d9e840c48b1e 122 if (iSumLeft > I_MAX) iSumLeft = I_MAX; //Max Saettigung I - Anteil
ruesipat 1:d9e840c48b1e 123 if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil
ruesipat 1:d9e840c48b1e 124
ruesipat 1:d9e840c48b1e 125 iSumRight += (desiredSpeedRight-actualSpeedRight);
ruesipat 1:d9e840c48b1e 126 if (iSumRight > I_MAX) iSumRight = I_MAX; //Max Saettigung I - Anteil
ruesipat 1:d9e840c48b1e 127 if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil
ruesipat 1:d9e840c48b1e 128
ruesipat 0:a9fe4ef404bf 129 // Berechnen der Motorspannungen Uout
ruesipat 1:d9e840c48b1e 130
ruesipat 1:d9e840c48b1e 131 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD
ruesipat 1:d9e840c48b1e 132 +desiredSpeedLeft/KN;
ruesipat 1:d9e840c48b1e 133 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD
ruesipat 0:a9fe4ef404bf 134 +desiredSpeedRight/KN;
ruesipat 1:d9e840c48b1e 135
ruesipat 0:a9fe4ef404bf 136 // Berechnen, Limitieren und Setzen der Duty-Cycle
ruesipat 1:d9e840c48b1e 137
ruesipat 0:a9fe4ef404bf 138 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
ruesipat 0:a9fe4ef404bf 139 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
ruesipat 0:a9fe4ef404bf 140 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
ruesipat 0:a9fe4ef404bf 141 pwmLeft = dutyCycleLeft;
ruesipat 1:d9e840c48b1e 142
ruesipat 0:a9fe4ef404bf 143 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
ruesipat 0:a9fe4ef404bf 144 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
ruesipat 0:a9fe4ef404bf 145 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
ruesipat 0:a9fe4ef404bf 146 pwmRight = dutyCycleRight;
ruesipat 1:d9e840c48b1e 147
ruesipat 0:a9fe4ef404bf 148 }