Sebastian Grünwald / PM2_Libary

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?

UserRevisionLine numberNew 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 }