Fabian Wenger / Mbed 2 deprecated Rainbow

Grundfunktionen für Micromouse

Dependencies:   AutomationElements mbed

MotorDriver/Controller.cpp@0:e38b500d6e74, 2018-04-19 (annotated)

Committer:
wengefa1
Date:
Thu Apr 19 11:31:49 2018 +0000
Revision:
0:e38b500d6e74
keine Regelung vorhanden

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wengefa1 0:e38b500d6e74 1 #include "Controller.h"
wengefa1 0:e38b500d6e74 2
wengefa1 0:e38b500d6e74 3 using namespace std;
wengefa1 0:e38b500d6e74 4
wengefa1 0:e38b500d6e74 5 const float Controller::PERIOD = 0.001f; // Periode von 1 ms
wengefa1 0:e38b500d6e74 6 const float Controller::COUNTS_PER_TURN = 1200.0f; // Encoder-Aufloesung
wengefa1 0:e38b500d6e74 7 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s]
wengefa1 0:e38b500d6e74 8 const float Controller::KN = 40.0f; // Drehzahlkonstante in [rpm/V]
wengefa1 0:e38b500d6e74 9 const float Controller::KP = 0.25f; // KP Regler-Parameter
wengefa1 0:e38b500d6e74 10 const float Controller::KI = 4.0f; // KI Regler-Parameter
wengefa1 0:e38b500d6e74 11 const float Controller::I_MAX = 10000.0f; // KI Regler-Parameter Saettigung
wengefa1 0:e38b500d6e74 12 const float Controller::MAX_VOLTAGE = 12.0f; // Batteriespannung in [V]
wengefa1 0:e38b500d6e74 13 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimale Duty-Cycle
wengefa1 0:e38b500d6e74 14 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximale Duty-Cycle
wengefa1 0:e38b500d6e74 15
wengefa1 0:e38b500d6e74 16 int ii =0;
wengefa1 0:e38b500d6e74 17
wengefa1 0:e38b500d6e74 18 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
wengefa1 0:e38b500d6e74 19 EncoderCounter& counterLeft, EncoderCounter& counterRight) :
wengefa1 0:e38b500d6e74 20 pwmLeft(pwmLeft), pwmRight(pwmRight),
wengefa1 0:e38b500d6e74 21 counterLeft(counterLeft), counterRight(counterRight) {
wengefa1 0:e38b500d6e74 22
wengefa1 0:e38b500d6e74 23 // Initialisieren der PWM Ausgaenge
wengefa1 0:e38b500d6e74 24
wengefa1 0:e38b500d6e74 25 pwmLeft.period(0.00005f); // PWM Periode von 50 us
wengefa1 0:e38b500d6e74 26 pwmLeft = 0.5f; // Duty-Cycle von 50%
wengefa1 0:e38b500d6e74 27 pwmRight.period(0.00005f); // PWM Periode von 50 us
wengefa1 0:e38b500d6e74 28 pwmRight = 0.5f; // Duty-Cycle von 50%
wengefa1 0:e38b500d6e74 29
wengefa1 0:e38b500d6e74 30 // Initialisieren von lokalen Variabeln
wengefa1 0:e38b500d6e74 31
wengefa1 0:e38b500d6e74 32 previousValueCounterLeft = counterLeft.read();
wengefa1 0:e38b500d6e74 33 previousValueCounterRight = counterRight.read();
wengefa1 0:e38b500d6e74 34
wengefa1 0:e38b500d6e74 35 speedLeftFilter.setPeriod(PERIOD);
wengefa1 0:e38b500d6e74 36 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
wengefa1 0:e38b500d6e74 37
wengefa1 0:e38b500d6e74 38 speedRightFilter.setPeriod(PERIOD);
wengefa1 0:e38b500d6e74 39 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
wengefa1 0:e38b500d6e74 40
wengefa1 0:e38b500d6e74 41 desiredSpeedLeft = 0.0f;
wengefa1 0:e38b500d6e74 42 desiredSpeedRight = 0.0f;
wengefa1 0:e38b500d6e74 43
wengefa1 0:e38b500d6e74 44 actualSpeedLeft = 0.0f;
wengefa1 0:e38b500d6e74 45 actualSpeedRight = 0.0f;
wengefa1 0:e38b500d6e74 46
wengefa1 0:e38b500d6e74 47 // Starten des periodischen Tasks
wengefa1 0:e38b500d6e74 48 ticker.attach(callback(this, &Controller::run), PERIOD);
wengefa1 0:e38b500d6e74 49 }
wengefa1 0:e38b500d6e74 50
wengefa1 0:e38b500d6e74 51 Controller::~Controller()
wengefa1 0:e38b500d6e74 52 {
wengefa1 0:e38b500d6e74 53 ticker.detach(); // Stoppt den periodischen Task
wengefa1 0:e38b500d6e74 54 }
wengefa1 0:e38b500d6e74 55
wengefa1 0:e38b500d6e74 56 void Controller::AttachTicker(void)
wengefa1 0:e38b500d6e74 57 {
wengefa1 0:e38b500d6e74 58 previousValueCounterLeft = counterLeft.read();
wengefa1 0:e38b500d6e74 59 previousValueCounterRight = counterRight.read();
wengefa1 0:e38b500d6e74 60
wengefa1 0:e38b500d6e74 61 //actualSpeedLeft = 0.0f;
wengefa1 0:e38b500d6e74 62 //actualSpeedRight = 0.0f;
wengefa1 0:e38b500d6e74 63 ticker.attach(callback(this, &Controller::run), PERIOD);
wengefa1 0:e38b500d6e74 64 }
wengefa1 0:e38b500d6e74 65
wengefa1 0:e38b500d6e74 66 void Controller::DetachTicker(void)
wengefa1 0:e38b500d6e74 67 {
wengefa1 0:e38b500d6e74 68 wait(0.5);
wengefa1 0:e38b500d6e74 69 ticker.detach();
wengefa1 0:e38b500d6e74 70 }
wengefa1 0:e38b500d6e74 71 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
wengefa1 0:e38b500d6e74 72 {
wengefa1 0:e38b500d6e74 73 this->desiredSpeedLeft = desiredSpeedLeft;
wengefa1 0:e38b500d6e74 74 }
wengefa1 0:e38b500d6e74 75
wengefa1 0:e38b500d6e74 76 void Controller::setDesiredSpeedRight(float desiredSpeedRight)
wengefa1 0:e38b500d6e74 77 {
wengefa1 0:e38b500d6e74 78 this->desiredSpeedRight = desiredSpeedRight;
wengefa1 0:e38b500d6e74 79 }
wengefa1 0:e38b500d6e74 80
wengefa1 0:e38b500d6e74 81 float Controller::getSpeedLeft()
wengefa1 0:e38b500d6e74 82 {
wengefa1 0:e38b500d6e74 83 return actualSpeedLeft;
wengefa1 0:e38b500d6e74 84 }
wengefa1 0:e38b500d6e74 85
wengefa1 0:e38b500d6e74 86 float Controller::getSpeedRight()
wengefa1 0:e38b500d6e74 87 {
wengefa1 0:e38b500d6e74 88 return actualSpeedRight;
wengefa1 0:e38b500d6e74 89 }
wengefa1 0:e38b500d6e74 90
wengefa1 0:e38b500d6e74 91 float Controller::getIntegralLeft()
wengefa1 0:e38b500d6e74 92 {
wengefa1 0:e38b500d6e74 93 return iSumLeft;
wengefa1 0:e38b500d6e74 94 }
wengefa1 0:e38b500d6e74 95
wengefa1 0:e38b500d6e74 96 float Controller::getIntegralRight()
wengefa1 0:e38b500d6e74 97 {
wengefa1 0:e38b500d6e74 98 return iSumRight;
wengefa1 0:e38b500d6e74 99 }
wengefa1 0:e38b500d6e74 100
wengefa1 0:e38b500d6e74 101 float Controller::getProportionalLeft()
wengefa1 0:e38b500d6e74 102 {
wengefa1 0:e38b500d6e74 103 return (desiredSpeedLeft-actualSpeedLeft);
wengefa1 0:e38b500d6e74 104 }
wengefa1 0:e38b500d6e74 105
wengefa1 0:e38b500d6e74 106 float Controller::getProportionalRight()
wengefa1 0:e38b500d6e74 107 {
wengefa1 0:e38b500d6e74 108 return (desiredSpeedRight-actualSpeedRight);
wengefa1 0:e38b500d6e74 109 }
wengefa1 0:e38b500d6e74 110
wengefa1 0:e38b500d6e74 111 void Controller::run() {
wengefa1 0:e38b500d6e74 112
wengefa1 0:e38b500d6e74 113 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
wengefa1 0:e38b500d6e74 114 short valueCounterLeft = counterLeft.read();
wengefa1 0:e38b500d6e74 115 short valueCounterRight = counterRight.read();
wengefa1 0:e38b500d6e74 116
wengefa1 0:e38b500d6e74 117 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
wengefa1 0:e38b500d6e74 118 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
wengefa1 0:e38b500d6e74 119
wengefa1 0:e38b500d6e74 120 previousValueCounterLeft = valueCounterLeft;
wengefa1 0:e38b500d6e74 121 previousValueCounterRight = valueCounterRight;
wengefa1 0:e38b500d6e74 122
wengefa1 0:e38b500d6e74 123 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft
wengefa1 0:e38b500d6e74 124 /COUNTS_PER_TURN/PERIOD*60.0f);
wengefa1 0:e38b500d6e74 125 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight
wengefa1 0:e38b500d6e74 126 /COUNTS_PER_TURN/PERIOD*60.0f);
wengefa1 0:e38b500d6e74 127
wengefa1 0:e38b500d6e74 128
wengefa1 0:e38b500d6e74 129 //Berechnung I - Anteil
wengefa1 0:e38b500d6e74 130
wengefa1 0:e38b500d6e74 131
wengefa1 0:e38b500d6e74 132 iSumLeft += (desiredSpeedLeft-actualSpeedLeft);
wengefa1 0:e38b500d6e74 133 if (iSumLeft > I_MAX) iSumLeft = I_MAX; //Max Saettigung I - Anteil
wengefa1 0:e38b500d6e74 134 if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil
wengefa1 0:e38b500d6e74 135
wengefa1 0:e38b500d6e74 136 iSumRight += (desiredSpeedRight-actualSpeedRight);
wengefa1 0:e38b500d6e74 137 if (iSumRight > I_MAX) iSumRight = I_MAX; //Max Saettigung I - Anteil
wengefa1 0:e38b500d6e74 138 if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil
wengefa1 0:e38b500d6e74 139
wengefa1 0:e38b500d6e74 140 // Berechnen der Motorspannungen Uout
wengefa1 0:e38b500d6e74 141
wengefa1 0:e38b500d6e74 142 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD
wengefa1 0:e38b500d6e74 143 +desiredSpeedLeft/KN;
wengefa1 0:e38b500d6e74 144 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD
wengefa1 0:e38b500d6e74 145 +desiredSpeedRight/KN;
wengefa1 0:e38b500d6e74 146
wengefa1 0:e38b500d6e74 147 // Berechnen, Limitieren und Setzen der Duty-Cycle
wengefa1 0:e38b500d6e74 148
wengefa1 0:e38b500d6e74 149 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
wengefa1 0:e38b500d6e74 150 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
wengefa1 0:e38b500d6e74 151 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
wengefa1 0:e38b500d6e74 152 pwmLeft = dutyCycleLeft;
wengefa1 0:e38b500d6e74 153
wengefa1 0:e38b500d6e74 154 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
wengefa1 0:e38b500d6e74 155 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
wengefa1 0:e38b500d6e74 156 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
wengefa1 0:e38b500d6e74 157 pwmRight = dutyCycleRight;
wengefa1 0:e38b500d6e74 158
wengefa1 0:e38b500d6e74 159 }
wengefa1 0:e38b500d6e74 160