Nicolas Borla / Mbed OS PES2_mbed_os

PES2_mbed_os_6

Dependencies:   Servo

Controller.cpp@3:a292bdaf03f6, 2021-03-16 (annotated)

Committer:
boro
Date:
Tue Mar 16 17:28:04 2021 +0100
Revision:
3:a292bdaf03f6
Parent:
0:5d4d21d56334 
controller updated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
boro 0:5d4d21d56334 1 #include "Controller.h"
boro 0:5d4d21d56334 2
boro 0:5d4d21d56334 3 using namespace std;
boro 0:5d4d21d56334 4
boro 0:5d4d21d56334 5 const float Controller::PERIOD = 0.001f; // period of 1 ms
boro 0:5d4d21d56334 6 const float Controller::COUNTS_PER_TURN = 1200.0f; // encoder resolution
boro 0:5d4d21d56334 7 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // given in [rad/s]
boro 0:5d4d21d56334 8 const float Controller::KN = 40.0f; // speed constant in [rpm/V]
boro 3:a292bdaf03f6 9 const float Controller::KP = 0.15f; // speed control parameter
boro 0:5d4d21d56334 10 const float Controller::MAX_VOLTAGE = 12.0f; // battery voltage in [V]
boro 0:5d4d21d56334 11 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum duty-cycle
boro 0:5d4d21d56334 12 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum duty-cycle
boro 0:5d4d21d56334 13
boro 0:5d4d21d56334 14 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
boro 0:5d4d21d56334 15 EncoderCounter& counterLeft, EncoderCounter& counterRight) :
boro 0:5d4d21d56334 16 pwmLeft(pwmLeft), pwmRight(pwmRight),
boro 3:a292bdaf03f6 17 counterLeft(counterLeft), counterRight(counterRight), thread(osPriorityHigh, STACK_SIZE) {
boro 0:5d4d21d56334 18
boro 0:5d4d21d56334 19 // Initialisieren der PWM Ausgaenge
boro 0:5d4d21d56334 20
boro 0:5d4d21d56334 21 pwmLeft.period(0.00005f); // PWM Periode von 50 us
boro 0:5d4d21d56334 22 pwmLeft = 0.5f; // Duty-Cycle von 50%
boro 0:5d4d21d56334 23 pwmRight.period(0.00005f); // PWM Periode von 50 us
boro 0:5d4d21d56334 24 pwmRight = 0.5f; // Duty-Cycle von 50%
boro 0:5d4d21d56334 25
boro 0:5d4d21d56334 26 // Initialisieren von lokalen Variabeln
boro 0:5d4d21d56334 27
boro 0:5d4d21d56334 28 previousValueCounterLeft = counterLeft.read();
boro 0:5d4d21d56334 29 previousValueCounterRight = counterRight.read();
boro 0:5d4d21d56334 30
boro 0:5d4d21d56334 31 speedLeftFilter.setPeriod(PERIOD);
boro 0:5d4d21d56334 32 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
boro 0:5d4d21d56334 33
boro 0:5d4d21d56334 34 speedRightFilter.setPeriod(PERIOD);
boro 0:5d4d21d56334 35 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
boro 0:5d4d21d56334 36
boro 0:5d4d21d56334 37 desiredSpeedLeft = 0.0f;
boro 0:5d4d21d56334 38 desiredSpeedRight = 0.0f;
boro 0:5d4d21d56334 39
boro 0:5d4d21d56334 40 actualSpeedLeft = 0.0f;
boro 0:5d4d21d56334 41 actualSpeedRight = 0.0f;
boro 0:5d4d21d56334 42
boro 0:5d4d21d56334 43 actualAngleLeft = 0.0f;
boro 0:5d4d21d56334 44 actualAngleRight = 0.0f;
boro 0:5d4d21d56334 45
boro 0:5d4d21d56334 46 // Starten des periodischen Tasks
boro 0:5d4d21d56334 47 thread.start(callback(this, &Controller::run));
boro 0:5d4d21d56334 48 ticker.attach(callback(this, &Controller::sendThreadFlag), PERIOD);
boro 0:5d4d21d56334 49 }
boro 0:5d4d21d56334 50
boro 0:5d4d21d56334 51 Controller::~Controller()
boro 0:5d4d21d56334 52 {
boro 0:5d4d21d56334 53 ticker.detach(); // Stoppt den periodischen Task
boro 0:5d4d21d56334 54 }
boro 0:5d4d21d56334 55
boro 0:5d4d21d56334 56
boro 0:5d4d21d56334 57 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
boro 0:5d4d21d56334 58 {
boro 0:5d4d21d56334 59 this->desiredSpeedLeft = desiredSpeedLeft;
boro 0:5d4d21d56334 60 }
boro 0:5d4d21d56334 61
boro 0:5d4d21d56334 62 void Controller::setDesiredSpeedRight(float desiredSpeedRight)
boro 0:5d4d21d56334 63 {
boro 0:5d4d21d56334 64 this->desiredSpeedRight = desiredSpeedRight;
boro 0:5d4d21d56334 65 }
boro 0:5d4d21d56334 66
boro 0:5d4d21d56334 67 float Controller::getSpeedLeft()
boro 0:5d4d21d56334 68 {
boro 0:5d4d21d56334 69 return actualSpeedLeft;
boro 0:5d4d21d56334 70 }
boro 0:5d4d21d56334 71
boro 0:5d4d21d56334 72 float Controller::getSpeedRight()
boro 0:5d4d21d56334 73 {
boro 0:5d4d21d56334 74 return actualSpeedRight;
boro 0:5d4d21d56334 75 }
boro 0:5d4d21d56334 76
boro 0:5d4d21d56334 77 /**
boro 0:5d4d21d56334 78 * This method is called by the ticker timer interrupt service routine.
boro 0:5d4d21d56334 79 * It sends a flag to the thread to make it run again.
boro 0:5d4d21d56334 80 */
boro 0:5d4d21d56334 81 void Controller::sendThreadFlag() {
boro 0:5d4d21d56334 82
boro 0:5d4d21d56334 83 thread.flags_set(threadFlag);
boro 0:5d4d21d56334 84 }
boro 0:5d4d21d56334 85
boro 0:5d4d21d56334 86 void Controller::run() {
boro 0:5d4d21d56334 87
boro 0:5d4d21d56334 88 while(true) {
boro 0:5d4d21d56334 89
boro 0:5d4d21d56334 90 // wait for the periodic signal
boro 0:5d4d21d56334 91
boro 0:5d4d21d56334 92 ThisThread::flags_wait_any(threadFlag);
boro 0:5d4d21d56334 93
boro 0:5d4d21d56334 94 // calculate actual speed of motors in [rpm]
boro 0:5d4d21d56334 95
boro 0:5d4d21d56334 96 short valueCounterLeft = counterLeft.read();
boro 0:5d4d21d56334 97 short valueCounterRight = counterRight.read();
boro 0:5d4d21d56334 98
boro 0:5d4d21d56334 99 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
boro 0:5d4d21d56334 100 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
boro 0:5d4d21d56334 101
boro 0:5d4d21d56334 102 previousValueCounterLeft = valueCounterLeft;
boro 0:5d4d21d56334 103 previousValueCounterRight = valueCounterRight;
boro 0:5d4d21d56334 104
boro 0:5d4d21d56334 105 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
boro 0:5d4d21d56334 106 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
boro 0:5d4d21d56334 107
boro 0:5d4d21d56334 108 actualAngleLeft = actualAngleLeft + actualSpeedLeft/60.0f*PERIOD;
boro 0:5d4d21d56334 109 actualAngleRight = actualAngleRight + actualSpeedRight/60.0f*PERIOD;
boro 0:5d4d21d56334 110
boro 0:5d4d21d56334 111 // calculate motor phase voltages
boro 0:5d4d21d56334 112
boro 0:5d4d21d56334 113 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
boro 0:5d4d21d56334 114 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
boro 0:5d4d21d56334 115
boro 0:5d4d21d56334 116 // calculate, limit and set duty cycles
boro 0:5d4d21d56334 117
boro 0:5d4d21d56334 118 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
boro 0:5d4d21d56334 119 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
boro 0:5d4d21d56334 120 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
boro 0:5d4d21d56334 121 pwmLeft.write(dutyCycleLeft);
boro 0:5d4d21d56334 122
boro 0:5d4d21d56334 123 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
boro 0:5d4d21d56334 124 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
boro 0:5d4d21d56334 125 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
boro 0:5d4d21d56334 126 pwmRight.write(dutyCycleRight);
boro 0:5d4d21d56334 127
boro 0:5d4d21d56334 128 }
boro 0:5d4d21d56334 129 }