Nicolas Borla / Mbed 2 deprecated Example_for_Joel

IMU measurement + Speed controller

Dependencies:   mbed

Controller.cpp@1:17fdd812cb8d, 2019-05-30 (annotated)

Committer:
boro
Date:
Thu May 30 13:21:44 2019 +0000
Revision:
1:17fdd812cb8d
Parent:
0:5a93e4916fb1 
a

Who changed what in which revision?

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