Antonia Baumgartner / Mbed 2 deprecated Versuch21

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 Versuch20 by Alexander Wyss

Classes/Controller.cpp@0:b886f13e4ac6, 2018-04-22 (annotated)

Committer:
baumgant
Date:
Sun Apr 22 16:14:54 2018 +0000
Revision:
0:b886f13e4ac6
Child:
5:64d7b4b69fdf
Roebi

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 0:b886f13e4ac6 6 const float Controller::COUNTS_PER_TURN = 1260.0f; // Encoder-Aufloesung
baumgant 0:b886f13e4ac6 7 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s]
baumgant 0:b886f13e4ac6 8 const float Controller::KN = 18.75f; // Drehzahlkonstante in [rpm/V]
baumgant 0:b886f13e4ac6 9 const float Controller::KP = 0.02f; // Regler-Parameter
baumgant 0:b886f13e4ac6 10 const float Controller::MAX_VOLTAGE = 12.0f; // Batteriespannung in [V]
baumgant 0:b886f13e4ac6 11 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimale Duty-Cycle
baumgant 0:b886f13e4ac6 12 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximale Duty-Cycle
baumgant 0:b886f13e4ac6 13
baumgant 0:b886f13e4ac6 14 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
baumgant 0:b886f13e4ac6 15 EncoderCounter& counterLeft, EncoderCounter& counterRight) :
baumgant 0:b886f13e4ac6 16 pwmLeft(pwmLeft), pwmRight(pwmRight),
baumgant 0:b886f13e4ac6 17 counterLeft(counterLeft), counterRight(counterRight) {
baumgant 0:b886f13e4ac6 18
baumgant 0:b886f13e4ac6 19 // Initialisieren der PWM Ausgaenge
baumgant 0:b886f13e4ac6 20
baumgant 0:b886f13e4ac6 21 pwmLeft.period(0.00005f); // PWM Periode von 50 us
baumgant 0:b886f13e4ac6 22 pwmLeft = 0.5f; // Duty-Cycle von 50%
baumgant 0:b886f13e4ac6 23 pwmRight.period(0.00005f); // PWM Periode von 50 us
baumgant 0:b886f13e4ac6 24 pwmRight = 0.5f; // Duty-Cycle von 50%
baumgant 0:b886f13e4ac6 25
baumgant 0:b886f13e4ac6 26 // Initialisieren von lokalen Variabeln
baumgant 0:b886f13e4ac6 27
baumgant 0:b886f13e4ac6 28 previousValueCounterLeft = counterLeft.read();
baumgant 0:b886f13e4ac6 29 previousValueCounterRight = counterRight.read();
baumgant 0:b886f13e4ac6 30
baumgant 0:b886f13e4ac6 31 speedLeftFilter.setPeriod(PERIOD);
baumgant 0:b886f13e4ac6 32 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
baumgant 0:b886f13e4ac6 33
baumgant 0:b886f13e4ac6 34 speedRightFilter.setPeriod(PERIOD);
baumgant 0:b886f13e4ac6 35 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
baumgant 0:b886f13e4ac6 36
baumgant 0:b886f13e4ac6 37 desiredSpeedLeft = 0.0f;
baumgant 0:b886f13e4ac6 38 desiredSpeedRight = 0.0f;
baumgant 0:b886f13e4ac6 39
baumgant 0:b886f13e4ac6 40 actualSpeedLeft = 0.0f;
baumgant 0:b886f13e4ac6 41 actualSpeedRight = 0.0f;
baumgant 0:b886f13e4ac6 42
baumgant 0:b886f13e4ac6 43 // Starten des periodischen Tasks
baumgant 0:b886f13e4ac6 44 ticker.attach(callback(this, &Controller::run), PERIOD);
baumgant 0:b886f13e4ac6 45 }
baumgant 0:b886f13e4ac6 46
baumgant 0:b886f13e4ac6 47 Controller::~Controller()
baumgant 0:b886f13e4ac6 48 {
baumgant 0:b886f13e4ac6 49 ticker.detach(); // Stoppt den periodischen Task
baumgant 0:b886f13e4ac6 50 }
baumgant 0:b886f13e4ac6 51
baumgant 0:b886f13e4ac6 52
baumgant 0:b886f13e4ac6 53 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
baumgant 0:b886f13e4ac6 54 {
baumgant 0:b886f13e4ac6 55 this->desiredSpeedLeft = desiredSpeedLeft;
baumgant 0:b886f13e4ac6 56 }
baumgant 0:b886f13e4ac6 57 void Controller::resetCounter()
baumgant 0:b886f13e4ac6 58 {
baumgant 0:b886f13e4ac6 59 ticker.detach();
baumgant 0:b886f13e4ac6 60 counterLeft.reset();
baumgant 0:b886f13e4ac6 61 counterRight.reset();
baumgant 0:b886f13e4ac6 62 previousValueCounterLeft = counterLeft.read();
baumgant 0:b886f13e4ac6 63 previousValueCounterRight = counterRight.read();
baumgant 0:b886f13e4ac6 64 ticker.attach(callback(this, &Controller::run), PERIOD);
baumgant 0:b886f13e4ac6 65 }
baumgant 0:b886f13e4ac6 66
baumgant 0:b886f13e4ac6 67 void Controller::setDesiredSpeedRight(float desiredSpeedRight)
baumgant 0:b886f13e4ac6 68 {
baumgant 0:b886f13e4ac6 69 this->desiredSpeedRight = desiredSpeedRight;
baumgant 0:b886f13e4ac6 70 }
baumgant 0:b886f13e4ac6 71
baumgant 0:b886f13e4ac6 72 void Controller::run() {
baumgant 0:b886f13e4ac6 73
baumgant 0:b886f13e4ac6 74 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
baumgant 0:b886f13e4ac6 75
baumgant 0:b886f13e4ac6 76 short valueCounterLeft = counterLeft.read();
baumgant 0:b886f13e4ac6 77 short valueCounterRight = counterRight.read();
baumgant 0:b886f13e4ac6 78
baumgant 0:b886f13e4ac6 79 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
baumgant 0:b886f13e4ac6 80 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
baumgant 0:b886f13e4ac6 81
baumgant 0:b886f13e4ac6 82 previousValueCounterLeft = valueCounterLeft;
baumgant 0:b886f13e4ac6 83 previousValueCounterRight = valueCounterRight;
baumgant 0:b886f13e4ac6 84
baumgant 0:b886f13e4ac6 85 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft
baumgant 0:b886f13e4ac6 86 /COUNTS_PER_TURN/PERIOD*60.0f);
baumgant 0:b886f13e4ac6 87 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight
baumgant 0:b886f13e4ac6 88 /COUNTS_PER_TURN/PERIOD*60.0f);
baumgant 0:b886f13e4ac6 89
baumgant 0:b886f13e4ac6 90 // Berechnen der Motorspannungen Uout
baumgant 0:b886f13e4ac6 91
baumgant 0:b886f13e4ac6 92 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
baumgant 0:b886f13e4ac6 93 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)
baumgant 0:b886f13e4ac6 94 +desiredSpeedRight/KN;
baumgant 0:b886f13e4ac6 95
baumgant 0:b886f13e4ac6 96 // Berechnen, Limitieren und Setzen der Duty-Cycle
baumgant 0:b886f13e4ac6 97
baumgant 0:b886f13e4ac6 98 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
baumgant 0:b886f13e4ac6 99 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
baumgant 0:b886f13e4ac6 100 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
baumgant 0:b886f13e4ac6 101 pwmLeft = dutyCycleLeft;
baumgant 0:b886f13e4ac6 102
baumgant 0:b886f13e4ac6 103 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
baumgant 0:b886f13e4ac6 104 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
baumgant 0:b886f13e4ac6 105 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
baumgant 0:b886f13e4ac6 106 pwmRight = dutyCycleRight;
baumgant 0:b886f13e4ac6 107
baumgant 0:b886f13e4ac6 108 }