Antonia Baumgartner / Mbed 2 deprecated YB_copy

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Versuch21 by Antonia Baumgartner

Classes/Controller.cpp@5:93d3efe46493, 2018-05-09 (annotated)

Committer:
baumgant
Date:
Wed May 09 13:33:59 2018 +0000
Revision:
5:93d3efe46493
Parent:
0:b886f13e4ac6 
PES2;

Who changed what in which revision?

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