Fabian Meissner / PM2_Libary

Libary for PM2.

Dependencies:   RangeFinder FastPWM

SpeedController.cpp@3:8b42e643b294, 2021-04-01 (annotated)

Committer:
pmic
Date:
Thu Apr 01 14:31:43 2021 +0000
Revision:
3:8b42e643b294
Child:
4:9c003c402033
Changed Controller to SpeedController to only speedcontrol one motor per instance.

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 3:8b42e643b294 6 const float SpeedController::COUNTS_PER_TURN = 1560.0f; // encoder resolution
pmic 3:8b42e643b294 7 const float SpeedController::LOWPASS_FILTER_FREQUENCY = 300.0f; // given in [rad/s]
pmic 3:8b42e643b294 8 const float SpeedController::KN = 15.0f; // speed constant in [rpm/V]
pmic 3:8b42e643b294 9 const float SpeedController::KP = 0.15f; // speed control parameter
pmic 3:8b42e643b294 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 3:8b42e643b294 14 SpeedController::SpeedController(PwmOut& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
pmic 3:8b42e643b294 15 {
pmic 3:8b42e643b294 16
pmic 3:8b42e643b294 17 // Initialisieren der PWM Ausgaenge
pmic 3:8b42e643b294 18 pwm.period(0.00005f); // PWM Periode von 50 us
pmic 3:8b42e643b294 19 pwm = 0.5f; // Duty-Cycle von 50%
pmic 3:8b42e643b294 20
pmic 3:8b42e643b294 21 // Initialisieren von lokalen Variabeln
pmic 3:8b42e643b294 22 previousValueCounter = encoderCounter.read();
pmic 3:8b42e643b294 23 speedFilter.setPeriod(PERIOD);
pmic 3:8b42e643b294 24 speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 3:8b42e643b294 25 desiredSpeed = 0.0f;
pmic 3:8b42e643b294 26 actualSpeed = 0.0f;
pmic 3:8b42e643b294 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 3:8b42e643b294 31 ticker.attach(callback(this, &SpeedController::sendThreadFlag), PERIOD);
pmic 3:8b42e643b294 32 }
pmic 3:8b42e643b294 33
pmic 3:8b42e643b294 34 SpeedController::~SpeedController()
pmic 3:8b42e643b294 35 {
pmic 3:8b42e643b294 36 ticker.detach(); // Stoppt den periodischen Task
pmic 3:8b42e643b294 37 }
pmic 3:8b42e643b294 38
pmic 3:8b42e643b294 39
pmic 3:8b42e643b294 40 void SpeedController::setDesiredSpeed(float desiredSpeed)
pmic 3:8b42e643b294 41 {
pmic 3:8b42e643b294 42 this->desiredSpeed = desiredSpeed;
pmic 3:8b42e643b294 43 }
pmic 3:8b42e643b294 44
pmic 3:8b42e643b294 45 float SpeedController::getSpeed()
pmic 3:8b42e643b294 46 {
pmic 3:8b42e643b294 47 return actualSpeed;
pmic 3:8b42e643b294 48 }
pmic 3:8b42e643b294 49
pmic 3:8b42e643b294 50 void SpeedController::run()
pmic 3:8b42e643b294 51 {
pmic 3:8b42e643b294 52 while(true) {
pmic 3:8b42e643b294 53
pmic 3:8b42e643b294 54 // wait for the periodic signal
pmic 3:8b42e643b294 55 ThisThread::flags_wait_any(threadFlag);
pmic 3:8b42e643b294 56
pmic 3:8b42e643b294 57 // calculate actual speed of motors in [rpm]
pmic 3:8b42e643b294 58 short valueCounter = encoderCounter.read();
pmic 3:8b42e643b294 59 short countsInPastPeriod = valueCounter-previousValueCounter;
pmic 3:8b42e643b294 60 previousValueCounter = valueCounter;
pmic 3:8b42e643b294 61 actualSpeed = speedFilter.filter((float)countsInPastPeriod/COUNTS_PER_TURN/PERIOD*60.0f);
pmic 3:8b42e643b294 62 actualAngle = actualAngle + actualSpeed/60.0f*PERIOD;
pmic 3:8b42e643b294 63
pmic 3:8b42e643b294 64 // calculate motor phase voltages
pmic 3:8b42e643b294 65 float voltage = KP*(desiredSpeed-actualSpeed)+desiredSpeed/KN;
pmic 3:8b42e643b294 66
pmic 3:8b42e643b294 67 // calculate, limit and set duty cycles
pmic 3:8b42e643b294 68 float dutyCycle = 0.5f+0.5f*voltage/MAX_VOLTAGE;
pmic 3:8b42e643b294 69 if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
pmic 3:8b42e643b294 70 else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
pmic 3:8b42e643b294 71 pwm.write(dutyCycle);
pmic 3:8b42e643b294 72
pmic 3:8b42e643b294 73 }
pmic 3:8b42e643b294 74 }
pmic 3:8b42e643b294 75
pmic 3:8b42e643b294 76 void SpeedController::sendThreadFlag()
pmic 3:8b42e643b294 77 {
pmic 3:8b42e643b294 78 thread.flags_set(threadFlag);
pmic 3:8b42e643b294 79 }