PM2_Lib
Dependencies: LSM9DS1 RangeFinder FastPWM
SpeedController.cpp@6:41dd03654c44, 2021-04-07 (annotated)
- Committer:
- pmic
- Date:
- Wed Apr 07 12:13:45 2021 +0000
- Revision:
- 6:41dd03654c44
- Parent:
- 5:6cd242a61e4c
- Child:
- 9:583dbd17e0ba
Last commit before first workshop 2.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pmic | 3:8b42e643b294 | 1 | #include "SpeedController.h" |
pmic | 3:8b42e643b294 | 2 | |
pmic | 3:8b42e643b294 | 3 | using namespace std; |
pmic | 3:8b42e643b294 | 4 | |
pmic | 6:41dd03654c44 | 5 | const float SpeedController::TS = 0.001f; // period of 1 ms |
pmic | 6:41dd03654c44 | 6 | const float SpeedController::LOWPASS_FILTER_FREQUENCY = 100.0f; // given in [rad/s] |
pmic | 6:41dd03654c44 | 7 | const float SpeedController::MIN_DUTY_CYCLE = 0.0f; // minimum duty-cycle |
pmic | 6:41dd03654c44 | 8 | const float SpeedController::MAX_DUTY_CYCLE = 1.0f; // maximum duty-cycle |
pmic | 3:8b42e643b294 | 9 | |
pmic | 6:41dd03654c44 | 10 | SpeedController::SpeedController(float counts_per_turn, float kn, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096) |
pmic | 3:8b42e643b294 | 11 | { |
pmic | 6:41dd03654c44 | 12 | this->counts_per_turn = counts_per_turn; |
pmic | 6:41dd03654c44 | 13 | this->kn = kn; |
pmic | 6:41dd03654c44 | 14 | this->kp = 0.1f; |
pmic | 6:41dd03654c44 | 15 | this->max_voltage = max_voltage; |
pmic | 3:8b42e643b294 | 16 | |
pmic | 3:8b42e643b294 | 17 | // Initialisieren der PWM Ausgaenge |
pmic | 5:6cd242a61e4c | 18 | pwm.period(0.00005); // PWM Periode von 50 us |
pmic | 5:6cd242a61e4c | 19 | pwm.write(0.5); // Duty-Cycle von 50% |
pmic | 3:8b42e643b294 | 20 | |
pmic | 3:8b42e643b294 | 21 | // Initialisieren von lokalen Variabeln |
pmic | 3:8b42e643b294 | 22 | previousValueCounter = encoderCounter.read(); |
pmic | 6:41dd03654c44 | 23 | speedFilter.setPeriod(TS); |
pmic | 3:8b42e643b294 | 24 | speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
pmic | 3:8b42e643b294 | 25 | desiredSpeed = 0.0f; |
pmic | 3:8b42e643b294 | 26 | actualSpeed = 0.0f; |
pmic | 4:9c003c402033 | 27 | // actualAngle = 0.0f; |
pmic | 3:8b42e643b294 | 28 | |
pmic | 3:8b42e643b294 | 29 | // Starten des periodischen Tasks |
pmic | 3:8b42e643b294 | 30 | thread.start(callback(this, &SpeedController::run)); |
pmic | 6:41dd03654c44 | 31 | ticker.attach(callback(this, &SpeedController::sendThreadFlag), TS); |
pmic | 6:41dd03654c44 | 32 | } |
pmic | 6:41dd03654c44 | 33 | |
pmic | 6:41dd03654c44 | 34 | SpeedController::SpeedController(float counts_per_turn, float kn, float kp, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096) |
pmic | 6:41dd03654c44 | 35 | { |
pmic | 6:41dd03654c44 | 36 | this->counts_per_turn = counts_per_turn; |
pmic | 6:41dd03654c44 | 37 | this->kn = kn; |
pmic | 6:41dd03654c44 | 38 | this->kp = kp; |
pmic | 6:41dd03654c44 | 39 | this->max_voltage = max_voltage; |
pmic | 6:41dd03654c44 | 40 | |
pmic | 6:41dd03654c44 | 41 | // Initialisieren der PWM Ausgaenge |
pmic | 6:41dd03654c44 | 42 | pwm.period(0.00005); // PWM Periode von 50 us |
pmic | 6:41dd03654c44 | 43 | pwm.write(0.5); // Duty-Cycle von 50% |
pmic | 6:41dd03654c44 | 44 | |
pmic | 6:41dd03654c44 | 45 | // Initialisieren von lokalen Variabeln |
pmic | 6:41dd03654c44 | 46 | previousValueCounter = encoderCounter.read(); |
pmic | 6:41dd03654c44 | 47 | speedFilter.setPeriod(TS); |
pmic | 6:41dd03654c44 | 48 | speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
pmic | 6:41dd03654c44 | 49 | desiredSpeed = 0.0f; |
pmic | 6:41dd03654c44 | 50 | actualSpeed = 0.0f; |
pmic | 6:41dd03654c44 | 51 | // actualAngle = 0.0f; |
pmic | 6:41dd03654c44 | 52 | |
pmic | 6:41dd03654c44 | 53 | // Starten des periodischen Tasks |
pmic | 6:41dd03654c44 | 54 | thread.start(callback(this, &SpeedController::run)); |
pmic | 6:41dd03654c44 | 55 | ticker.attach(callback(this, &SpeedController::sendThreadFlag), TS); |
pmic | 3:8b42e643b294 | 56 | } |
pmic | 3:8b42e643b294 | 57 | |
pmic | 3:8b42e643b294 | 58 | SpeedController::~SpeedController() |
pmic | 3:8b42e643b294 | 59 | { |
pmic | 3:8b42e643b294 | 60 | ticker.detach(); // Stoppt den periodischen Task |
pmic | 3:8b42e643b294 | 61 | } |
pmic | 3:8b42e643b294 | 62 | |
pmic | 4:9c003c402033 | 63 | void SpeedController::setDesiredSpeedRPM(float desiredSpeed) |
pmic | 3:8b42e643b294 | 64 | { |
pmic | 3:8b42e643b294 | 65 | this->desiredSpeed = desiredSpeed; |
pmic | 3:8b42e643b294 | 66 | } |
pmic | 3:8b42e643b294 | 67 | |
pmic | 4:9c003c402033 | 68 | float SpeedController::getSpeedRPM() |
pmic | 3:8b42e643b294 | 69 | { |
pmic | 3:8b42e643b294 | 70 | return actualSpeed; |
pmic | 3:8b42e643b294 | 71 | } |
pmic | 3:8b42e643b294 | 72 | |
pmic | 4:9c003c402033 | 73 | void SpeedController::setDesiredSpeedRPS(float desiredSpeed) |
pmic | 4:9c003c402033 | 74 | { |
pmic | 4:9c003c402033 | 75 | this->desiredSpeed = desiredSpeed*60.0f; |
pmic | 4:9c003c402033 | 76 | } |
pmic | 4:9c003c402033 | 77 | |
pmic | 4:9c003c402033 | 78 | float SpeedController::getSpeedRPS() |
pmic | 4:9c003c402033 | 79 | { |
pmic | 4:9c003c402033 | 80 | return actualSpeed/60.0f; |
pmic | 4:9c003c402033 | 81 | } |
pmic | 4:9c003c402033 | 82 | |
pmic | 3:8b42e643b294 | 83 | void SpeedController::run() |
pmic | 3:8b42e643b294 | 84 | { |
pmic | 3:8b42e643b294 | 85 | while(true) { |
pmic | 3:8b42e643b294 | 86 | |
pmic | 3:8b42e643b294 | 87 | // wait for the periodic signal |
pmic | 3:8b42e643b294 | 88 | ThisThread::flags_wait_any(threadFlag); |
pmic | 3:8b42e643b294 | 89 | |
pmic | 3:8b42e643b294 | 90 | // calculate actual speed of motors in [rpm] |
pmic | 3:8b42e643b294 | 91 | short valueCounter = encoderCounter.read(); |
pmic | 4:9c003c402033 | 92 | short countsInPastPeriod = valueCounter - previousValueCounter; |
pmic | 3:8b42e643b294 | 93 | previousValueCounter = valueCounter; |
pmic | 6:41dd03654c44 | 94 | actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f); |
pmic | 6:41dd03654c44 | 95 | // actualAngle = actualAngle + actualSpeed/60.0f*TS; |
pmic | 3:8b42e643b294 | 96 | |
pmic | 3:8b42e643b294 | 97 | // calculate motor phase voltages |
pmic | 6:41dd03654c44 | 98 | float voltage = kp*(desiredSpeed - actualSpeed) + desiredSpeed/kn; |
pmic | 3:8b42e643b294 | 99 | |
pmic | 3:8b42e643b294 | 100 | // calculate, limit and set duty cycles |
pmic | 6:41dd03654c44 | 101 | float dutyCycle = 0.5f + 0.5f*voltage/max_voltage; |
pmic | 3:8b42e643b294 | 102 | if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE; |
pmic | 3:8b42e643b294 | 103 | else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE; |
pmic | 5:6cd242a61e4c | 104 | pwm.write(static_cast<double>(dutyCycle)); |
pmic | 3:8b42e643b294 | 105 | |
pmic | 3:8b42e643b294 | 106 | } |
pmic | 3:8b42e643b294 | 107 | } |
pmic | 3:8b42e643b294 | 108 | |
pmic | 3:8b42e643b294 | 109 | void SpeedController::sendThreadFlag() |
pmic | 3:8b42e643b294 | 110 | { |
pmic | 3:8b42e643b294 | 111 | thread.flags_set(threadFlag); |
pmic | 3:8b42e643b294 | 112 | } |