PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FOUR

Pathfinding nach rechts funktioniert noch nicht...der rest schon

Dependencies:   mbed

Fork of MicroMouse_MASTER_THREE by PES2_R2D2.0

Controller.cpp@0:a9fe4ef404bf, 2018-03-07 (annotated)

Committer:
ruesipat
Date:
Wed Mar 07 14:06:19 2018 +0000
Revision:
0:a9fe4ef404bf
Child:
1:d9e840c48b1e
hallo

Who changed what in which revision?

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