Sebastian Grünwald / PM2_Libary

PM2_Lib

Dependencies:   LSM9DS1 RangeFinder FastPWM

SpeedController.cpp@5:6cd242a61e4c, 2021-04-06 (annotated)

Committer:
pmic
Date:
Tue Apr 06 12:19:29 2021 +0000
Revision:
5:6cd242a61e4c
Parent:
4:9c003c402033
Child:
6:41dd03654c44
Introduce FastPWM Libary.

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 3:8b42e643b294 5 const float SpeedController::PERIOD = 0.001f; // period of 1 ms
pmic 4:9c003c402033 6 // const float SpeedController::COUNTS_PER_TURN = 1562.5f; // encoder resolution
pmic 4:9c003c402033 7 const float SpeedController::LOWPASS_FILTER_FREQUENCY = 100.0f; // given in [rad/s]
pmic 4:9c003c402033 8 // const float SpeedController::KN = 15.0f; // speed constant in [rpm/V]
pmic 4:9c003c402033 9 // const float SpeedController::KP = 0.15f; // speed control parameter
pmic 4:9c003c402033 10 // const float SpeedController::MAX_VOLTAGE = 12.0f; // battery voltage in [V]
pmic 3:8b42e643b294 11 const float SpeedController::MIN_DUTY_CYCLE = 0.02f; // minimum duty-cycle
pmic 3:8b42e643b294 12 const float SpeedController::MAX_DUTY_CYCLE = 0.98f; // maximum duty-cycle
pmic 3:8b42e643b294 13
pmic 5:6cd242a61e4c 14 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 3:8b42e643b294 15 {
pmic 4:9c003c402033 16 this->COUNTS_PER_TURN = COUNTS_PER_TURN;
pmic 4:9c003c402033 17 this->KN = KN;
pmic 4:9c003c402033 18 this->KP = KP;
pmic 4:9c003c402033 19 this->MAX_VOLTAGE = MAX_VOLTAGE;
pmic 3:8b42e643b294 20
pmic 3:8b42e643b294 21 // Initialisieren der PWM Ausgaenge
pmic 5:6cd242a61e4c 22 pwm.period(0.00005); // PWM Periode von 50 us
pmic 5:6cd242a61e4c 23 pwm.write(0.5); // Duty-Cycle von 50%
pmic 3:8b42e643b294 24
pmic 3:8b42e643b294 25 // Initialisieren von lokalen Variabeln
pmic 3:8b42e643b294 26 previousValueCounter = encoderCounter.read();
pmic 3:8b42e643b294 27 speedFilter.setPeriod(PERIOD);
pmic 3:8b42e643b294 28 speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 3:8b42e643b294 29 desiredSpeed = 0.0f;
pmic 3:8b42e643b294 30 actualSpeed = 0.0f;
pmic 4:9c003c402033 31 // actualAngle = 0.0f;
pmic 3:8b42e643b294 32
pmic 3:8b42e643b294 33 // Starten des periodischen Tasks
pmic 3:8b42e643b294 34 thread.start(callback(this, &SpeedController::run));
pmic 3:8b42e643b294 35 ticker.attach(callback(this, &SpeedController::sendThreadFlag), PERIOD);
pmic 3:8b42e643b294 36 }
pmic 3:8b42e643b294 37
pmic 3:8b42e643b294 38 SpeedController::~SpeedController()
pmic 3:8b42e643b294 39 {
pmic 3:8b42e643b294 40 ticker.detach(); // Stoppt den periodischen Task
pmic 3:8b42e643b294 41 }
pmic 3:8b42e643b294 42
pmic 4:9c003c402033 43 void SpeedController::setDesiredSpeedRPM(float desiredSpeed)
pmic 3:8b42e643b294 44 {
pmic 3:8b42e643b294 45 this->desiredSpeed = desiredSpeed;
pmic 3:8b42e643b294 46 }
pmic 3:8b42e643b294 47
pmic 4:9c003c402033 48 float SpeedController::getSpeedRPM()
pmic 3:8b42e643b294 49 {
pmic 3:8b42e643b294 50 return actualSpeed;
pmic 3:8b42e643b294 51 }
pmic 3:8b42e643b294 52
pmic 4:9c003c402033 53 void SpeedController::setDesiredSpeedRPS(float desiredSpeed)
pmic 4:9c003c402033 54 {
pmic 4:9c003c402033 55 this->desiredSpeed = desiredSpeed*60.0f;
pmic 4:9c003c402033 56 }
pmic 4:9c003c402033 57
pmic 4:9c003c402033 58 float SpeedController::getSpeedRPS()
pmic 4:9c003c402033 59 {
pmic 4:9c003c402033 60 return actualSpeed/60.0f;
pmic 4:9c003c402033 61 }
pmic 4:9c003c402033 62
pmic 3:8b42e643b294 63 void SpeedController::run()
pmic 3:8b42e643b294 64 {
pmic 3:8b42e643b294 65 while(true) {
pmic 3:8b42e643b294 66
pmic 3:8b42e643b294 67 // wait for the periodic signal
pmic 3:8b42e643b294 68 ThisThread::flags_wait_any(threadFlag);
pmic 3:8b42e643b294 69
pmic 3:8b42e643b294 70 // calculate actual speed of motors in [rpm]
pmic 3:8b42e643b294 71 short valueCounter = encoderCounter.read();
pmic 4:9c003c402033 72 short countsInPastPeriod = valueCounter - previousValueCounter;
pmic 3:8b42e643b294 73 previousValueCounter = valueCounter;
pmic 3:8b42e643b294 74 actualSpeed = speedFilter.filter((float)countsInPastPeriod/COUNTS_PER_TURN/PERIOD*60.0f);
pmic 4:9c003c402033 75 // actualAngle = actualAngle + actualSpeed/60.0f*PERIOD;
pmic 3:8b42e643b294 76
pmic 3:8b42e643b294 77 // calculate motor phase voltages
pmic 4:9c003c402033 78 float voltage = KP*(desiredSpeed - actualSpeed) + desiredSpeed/KN;
pmic 3:8b42e643b294 79
pmic 3:8b42e643b294 80 // calculate, limit and set duty cycles
pmic 4:9c003c402033 81 float dutyCycle = 0.5f + 0.5f*voltage/MAX_VOLTAGE;
pmic 3:8b42e643b294 82 if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
pmic 3:8b42e643b294 83 else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
pmic 5:6cd242a61e4c 84 pwm.write(static_cast<double>(dutyCycle));
pmic 3:8b42e643b294 85
pmic 3:8b42e643b294 86 }
pmic 3:8b42e643b294 87 }
pmic 3:8b42e643b294 88
pmic 3:8b42e643b294 89 void SpeedController::sendThreadFlag()
pmic 3:8b42e643b294 90 {
pmic 3:8b42e643b294 91 thread.flags_set(threadFlag);
pmic 3:8b42e643b294 92 }