Michael Fluck / Mbed 2 deprecated MyClass

d

Dependencies:   mbed

Fork of MyClass by Roboshark

Controller.cpp@0:af3f2e5c9cd4, 2018-04-19 (annotated)

Committer:
fluckmi1
Date:
Thu Apr 19 11:53:52 2018 +0000
Revision:
0:af3f2e5c9cd4
peace

Who changed what in which revision?

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