Fabian Meissner / PM2_Libary

Libary for PM2.

Dependencies:   RangeFinder FastPWM

PositionController.cpp@9:97b59d914fd8, 2021-04-24 (annotated)

Committer:
mrford1616
Date:
Sat Apr 24 09:47:52 2021 +0000
Revision:
9:97b59d914fd8
Parent:
8:6b747ad59ff5 
V1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 8:6b747ad59ff5 1 #include "PositionController.h"
pmic 8:6b747ad59ff5 2
pmic 8:6b747ad59ff5 3 using namespace std;
pmic 8:6b747ad59ff5 4
pmic 8:6b747ad59ff5 5 const float PositionController::TS = 0.001f; // period of 1 ms
pmic 8:6b747ad59ff5 6 const float PositionController::LOWPASS_FILTER_FREQUENCY = 100.0f; // given in [rad/s]
pmic 8:6b747ad59ff5 7 const float PositionController::MIN_DUTY_CYCLE = 0.0f; // minimum duty-cycle
pmic 8:6b747ad59ff5 8 const float PositionController::MAX_DUTY_CYCLE = 1.0f; // maximum duty-cycle
pmic 8:6b747ad59ff5 9
pmic 8:6b747ad59ff5 10 PositionController::PositionController(float counts_per_turn, float kn, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
pmic 8:6b747ad59ff5 11 {
pmic 8:6b747ad59ff5 12 this->counts_per_turn = counts_per_turn;
pmic 8:6b747ad59ff5 13 this->kn = kn;
pmic 8:6b747ad59ff5 14 this->kp = 0.1f;
pmic 8:6b747ad59ff5 15 this->max_voltage = max_voltage;
pmic 8:6b747ad59ff5 16 this->max_speed = kn*max_voltage;
pmic 8:6b747ad59ff5 17 this->p = 1300.0f;
pmic 8:6b747ad59ff5 18
pmic 8:6b747ad59ff5 19 // Initialisieren der PWM Ausgaenge
pmic 8:6b747ad59ff5 20 pwm.period(0.00005); // PWM Periode von 50 us
pmic 8:6b747ad59ff5 21 pwm.write(0.5); // Duty-Cycle von 50%
pmic 8:6b747ad59ff5 22
pmic 8:6b747ad59ff5 23 // Initialisieren von lokalen Variabeln
pmic 8:6b747ad59ff5 24 previousValueCounter = encoderCounter.read();
pmic 8:6b747ad59ff5 25 speedFilter.setPeriod(TS);
pmic 8:6b747ad59ff5 26 speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 8:6b747ad59ff5 27 desiredSpeed = 0.0f;
pmic 8:6b747ad59ff5 28 actualSpeed = 0.0f;
pmic 8:6b747ad59ff5 29 float initialRotation = (float)encoderCounter.read()/counts_per_turn;
pmic 8:6b747ad59ff5 30 this->initialRotation = initialRotation;
pmic 8:6b747ad59ff5 31 actualRotation = initialRotation;
pmic 8:6b747ad59ff5 32 desiredRotation = initialRotation;
pmic 8:6b747ad59ff5 33
pmic 8:6b747ad59ff5 34 // Starten des periodischen Tasks
pmic 8:6b747ad59ff5 35 thread.start(callback(this, &PositionController::run));
pmic 8:6b747ad59ff5 36 ticker.attach(callback(this, &PositionController::sendThreadFlag), TS);
pmic 8:6b747ad59ff5 37 }
pmic 8:6b747ad59ff5 38
pmic 8:6b747ad59ff5 39 PositionController::PositionController(float counts_per_turn, float kn, float kp, float p, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
pmic 8:6b747ad59ff5 40 {
pmic 8:6b747ad59ff5 41 this->counts_per_turn = counts_per_turn;
pmic 8:6b747ad59ff5 42 this->kn = kn;
pmic 8:6b747ad59ff5 43 this->kp = kp;
pmic 8:6b747ad59ff5 44 this->max_voltage = max_voltage;
pmic 8:6b747ad59ff5 45 this->max_speed = kn*max_voltage;
pmic 8:6b747ad59ff5 46 this->p = p;
pmic 8:6b747ad59ff5 47
pmic 8:6b747ad59ff5 48 // Initialisieren der PWM Ausgaenge
pmic 8:6b747ad59ff5 49 pwm.period(0.00005); // PWM Periode von 50 us
pmic 8:6b747ad59ff5 50 pwm.write(0.5); // Duty-Cycle von 50%
pmic 8:6b747ad59ff5 51
pmic 8:6b747ad59ff5 52 // Initialisieren von lokalen Variabeln
pmic 8:6b747ad59ff5 53 previousValueCounter = encoderCounter.read();
pmic 8:6b747ad59ff5 54 speedFilter.setPeriod(TS);
pmic 8:6b747ad59ff5 55 speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 8:6b747ad59ff5 56 desiredSpeed = 0.0f;
pmic 8:6b747ad59ff5 57 actualSpeed = 0.0f;
pmic 8:6b747ad59ff5 58 float initialRotation = (float)encoderCounter.read()/counts_per_turn;
pmic 8:6b747ad59ff5 59 this->initialRotation = initialRotation;
pmic 8:6b747ad59ff5 60 actualRotation = initialRotation;
pmic 8:6b747ad59ff5 61 desiredRotation = initialRotation;
pmic 8:6b747ad59ff5 62
pmic 8:6b747ad59ff5 63 // Starten des periodischen Tasks
pmic 8:6b747ad59ff5 64 thread.start(callback(this, &PositionController::run));
pmic 8:6b747ad59ff5 65 ticker.attach(callback(this, &PositionController::sendThreadFlag), TS);
pmic 8:6b747ad59ff5 66 }
pmic 8:6b747ad59ff5 67
pmic 8:6b747ad59ff5 68 PositionController::~PositionController()
pmic 8:6b747ad59ff5 69 {
pmic 8:6b747ad59ff5 70 ticker.detach(); // Stoppt den periodischen Task
pmic 8:6b747ad59ff5 71 }
pmic 8:6b747ad59ff5 72
pmic 8:6b747ad59ff5 73 float PositionController::getSpeedRPM()
pmic 8:6b747ad59ff5 74 {
pmic 8:6b747ad59ff5 75 return actualSpeed;
pmic 8:6b747ad59ff5 76 }
pmic 8:6b747ad59ff5 77
pmic 8:6b747ad59ff5 78 float PositionController::getSpeedRPS()
pmic 8:6b747ad59ff5 79 {
pmic 8:6b747ad59ff5 80 return actualSpeed/60.0f;
pmic 8:6b747ad59ff5 81 }
pmic 8:6b747ad59ff5 82
pmic 8:6b747ad59ff5 83 void PositionController::setDesiredRotation(float desiredRotation)
pmic 8:6b747ad59ff5 84 {
pmic 8:6b747ad59ff5 85 this->desiredRotation = initialRotation + desiredRotation;
pmic 8:6b747ad59ff5 86 }
pmic 8:6b747ad59ff5 87
pmic 8:6b747ad59ff5 88 void PositionController::setDesiredRotation(float desiredRotation, float maxSpeedRPS)
pmic 8:6b747ad59ff5 89 {
pmic 8:6b747ad59ff5 90 this->max_speed = maxSpeedRPS*60.0f;
pmic 8:6b747ad59ff5 91 this->desiredRotation = initialRotation + desiredRotation;
pmic 8:6b747ad59ff5 92 }
pmic 8:6b747ad59ff5 93
pmic 8:6b747ad59ff5 94 float PositionController::getRotation()
pmic 8:6b747ad59ff5 95 {
pmic 8:6b747ad59ff5 96 return actualRotation - initialRotation;
pmic 8:6b747ad59ff5 97 }
pmic 8:6b747ad59ff5 98
pmic 8:6b747ad59ff5 99 void PositionController::run()
pmic 8:6b747ad59ff5 100 {
pmic 8:6b747ad59ff5 101 while(true) {
pmic 8:6b747ad59ff5 102
pmic 8:6b747ad59ff5 103 // wait for the periodic signal
pmic 8:6b747ad59ff5 104 ThisThread::flags_wait_any(threadFlag);
pmic 8:6b747ad59ff5 105
pmic 8:6b747ad59ff5 106 // calculate actual speed of motors in [rpm]
pmic 8:6b747ad59ff5 107 short valueCounter = encoderCounter.read();
pmic 8:6b747ad59ff5 108 short countsInPastPeriod = valueCounter - previousValueCounter;
pmic 8:6b747ad59ff5 109 previousValueCounter = valueCounter;
pmic 8:6b747ad59ff5 110 actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f);
pmic 8:6b747ad59ff5 111 actualRotation = actualRotation + actualSpeed/60.0f*TS;
pmic 8:6b747ad59ff5 112
pmic 8:6b747ad59ff5 113 // calculate motor phase voltages
pmic 8:6b747ad59ff5 114 desiredSpeed = p*(desiredRotation - actualRotation);
pmic 8:6b747ad59ff5 115 if (desiredSpeed < -max_speed) desiredSpeed = -max_speed;
pmic 8:6b747ad59ff5 116 else if (desiredSpeed > max_speed) desiredSpeed = max_speed;
pmic 8:6b747ad59ff5 117 float voltage = kp*(desiredSpeed - actualSpeed) + desiredSpeed/kn;
pmic 8:6b747ad59ff5 118 // calculate, limit and set duty cycles
pmic 8:6b747ad59ff5 119 float dutyCycle = 0.5f + 0.5f*voltage/max_voltage;
pmic 8:6b747ad59ff5 120 if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
pmic 8:6b747ad59ff5 121 else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
pmic 8:6b747ad59ff5 122 pwm.write(static_cast<double>(dutyCycle));
pmic 8:6b747ad59ff5 123
pmic 8:6b747ad59ff5 124 }
pmic 8:6b747ad59ff5 125 }
pmic 8:6b747ad59ff5 126
pmic 8:6b747ad59ff5 127 void PositionController::sendThreadFlag()
pmic 8:6b747ad59ff5 128 {
pmic 8:6b747ad59ff5 129 thread.flags_set(threadFlag);
pmic 8:6b747ad59ff5 130 }