Nicolas Borla
/
PES2_mbed_os
PES2_mbed_os_6
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?
User | Revision | Line number | New 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 | } |