Roboshark / Mbed 2 deprecated Roboshark_V8

Kommt ins Ziwl

Dependencies:   mbed

Fork of Roboshark_V7 by Roboshark

Controller.cpp@11:474ad54d2595, 2018-05-08 (annotated)

Committer:
ahlervin
Date:
Tue May 08 09:22:32 2018 +0000
Revision:
11:474ad54d2595
Parent:
10:fb2195d0de0f 
Kommt ins Ziel

Who changed what in which revision?

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