Helvijs Kiselis / Mbed 2 deprecated MicromousePES2

main.cpp: Sensoren einlesen und Motoren ansteuern

Dependencies:   mbed

Controller.cpp@5:47262622a9bf, 2018-04-16 (annotated)

Committer:
Helvis
Date:
Mon Apr 16 12:44:48 2018 +0000
Revision:
5:47262622a9bf
Parent:
0:9a3e7847a4be 
testtestest;

Who changed what in which revision?

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