Diff: PositionController.cpp

Revision:

10:fe74e8909d3f

Parent:

9:583dbd17e0ba

Child:

19:518ed284d98b

--- a/PositionController.cpp	Tue Aug 31 15:38:44 2021 +0000
+++ b/PositionController.cpp	Thu Feb 10 12:04:36 2022 +0000
@@ -1,7 +1,5 @@
 #include "PositionController.h"
 
-using namespace std;
-
 const float PositionController::TS = 0.001f;                       // period of 1 ms
 const float PositionController::LOWPASS_FILTER_FREQUENCY = 100.0f; // given in [rad/s]
 const float PositionController::MIN_DUTY_CYCLE = 0.02f;            // minimum duty-cycle
@@ -10,17 +8,46 @@
 PositionController::PositionController(float counts_per_turn, float kn, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
 {
     this->counts_per_turn = counts_per_turn;
-    this->kn = kn;
-    this->kp = 0.1f;
+    setFeedForwardGain(kn);
+    setSpeedCntrlGain(0.1f);
     this->max_voltage = max_voltage;
     this->max_speed   = kn*max_voltage;
-    this->p  = 1300.0f;
+    setPositionCntrlGain(1300.0f);
+
+    // initialise pwm
+    pwm.period(0.00005); // pwm period of 50 us
+    pwm.write(0.5);      // duty-cycle of 50%
+
+    // initialise
+    previousValueCounter = encoderCounter.read();
+    speedFilter.setPeriod(TS);
+    speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+    desiredSpeed = 0.0f;
+    actualSpeed = 0.0f;
+    float initialRotation = (float)encoderCounter.read()/counts_per_turn;
+    this->initialRotation = initialRotation;
+    actualRotation  = initialRotation;
+    desiredRotation = initialRotation;
 
-    // Initialisieren der PWM Ausgaenge
-    pwm.period(0.00005); // PWM Periode von 50 us
-    pwm.write(0.5);      // Duty-Cycle von 50%
+    // set up thread
+    thread.start(callback(this, &PositionController::run));
+    ticker.attach(callback(this, &PositionController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
+}
 
-    // Initialisieren von lokalen Variabeln
+PositionController::PositionController(float counts_per_turn, float kn, float kp, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
+{
+    this->counts_per_turn = counts_per_turn;
+    setFeedForwardGain(kn);
+    setSpeedCntrlGain(kp);
+    this->max_voltage = max_voltage;
+    this->max_speed   = kn*max_voltage;
+    setPositionCntrlGain(1300.0f);
+
+    // initialise pwm
+    pwm.period(0.00005); // pwm period of 50 us
+    pwm.write(0.5);      // duty-cycle of 50%
+
+    // initialise
     previousValueCounter = encoderCounter.read();
     speedFilter.setPeriod(TS);
     speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
@@ -31,75 +58,120 @@
     actualRotation  = initialRotation;
     desiredRotation = initialRotation;
 
-    // Starten des periodischen Tasks
+    // set up thread
     thread.start(callback(this, &PositionController::run));
-    ticker.attach(callback(this, &PositionController::sendThreadFlag), TS);
+    ticker.attach(callback(this, &PositionController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
 }
 
 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)
 {
     this->counts_per_turn = counts_per_turn;
-    this->kn = kn;
-    this->kp = kp;
+    setFeedForwardGain(kn);
+    setSpeedCntrlGain(kp);
     this->max_voltage = max_voltage;
     this->max_speed   = kn*max_voltage;
-    this->p  = p;
+    setPositionCntrlGain(p);
 
-    // Initialisieren der PWM Ausgaenge
-    pwm.period(0.00005); // PWM Periode von 50 us
-    pwm.write(0.5);      // Duty-Cycle von 50%
+    // initialise pwm
+    pwm.period(0.00005); // pwm period of 50 us
+    pwm.write(0.5);      // duty-cycle of 50%
 
-    // Initialisieren von lokalen Variabeln
+    // initialise
     previousValueCounter = encoderCounter.read();
     speedFilter.setPeriod(TS);
     speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
     desiredSpeed = 0.0f;
-    actualSpeed  = 0.0f;
+    actualSpeed = 0.0f;
     float initialRotation = (float)encoderCounter.read()/counts_per_turn;
     this->initialRotation = initialRotation;
     actualRotation  = initialRotation;
     desiredRotation = initialRotation;
 
-    // Starten des periodischen Tasks
+    // set up thread
     thread.start(callback(this, &PositionController::run));
-    ticker.attach(callback(this, &PositionController::sendThreadFlag), TS);
+    ticker.attach(callback(this, &PositionController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
 }
 
 PositionController::~PositionController()
 {
-    ticker.detach(); // Stoppt den periodischen Task
+    ticker.detach();
 }
 
+/**
+ * Reads the speed in RPM (rotations per minute).
+ * @return actual speed in RPM.
+ */
 float PositionController::getSpeedRPM()
 {
     return actualSpeed;
 }
 
+/**
+ * Reads the speed in RPS (rotations per second).
+ * @return actual speed in RPS.
+ */
 float PositionController::getSpeedRPS()
 {
     return actualSpeed/60.0f;
 }
 
+/**
+ * Sets desired rotation (1 corresponds to 360 deg).
+ */
 void PositionController::setDesiredRotation(float desiredRotation)
 {
     this->desiredRotation = initialRotation + desiredRotation;
 }
 
+/**
+ * Sets desired rotation (1 corresponds to 360 deg) and max. rotational speed in RPS (rotations per second).
+ */
 void PositionController::setDesiredRotation(float desiredRotation, float maxSpeedRPS)
 {
-    this->max_speed = maxSpeedRPS*60.0f;
+    float maxSpeedRPM = fabs(maxSpeedRPS * 60.0f);
+    if(maxSpeedRPM > kn * max_voltage) {
+        maxSpeedRPM = kn * max_voltage;
+    }
+    this->max_speed = maxSpeedRPM;
     this->desiredRotation = initialRotation + desiredRotation;
 }
 
+/**
+ * Reads the number of rotations (1 corresponds to 360 deg).
+ * @return actual rotations.
+ */
 float PositionController::getRotation()
 {
     return actualRotation - initialRotation;
 }
 
+/**
+ * Sets the feed-forward gain.
+ */
+void PositionController::setFeedForwardGain(float kn)
+{
+    this->kn = kn;
+}
+
+/**
+ * Sets the gain of the speed controller (p-controller).
+ */
+void PositionController::setSpeedCntrlGain(float kp)
+{
+    this->kp = kp;
+}
+
+/**
+ * Sets the gain of the position controller (p-controller).
+ */
+void PositionController::setPositionCntrlGain(float p)
+{
+    this->p = p;
+}
+
 void PositionController::run()
 {
     while(true) {
-
         // wait for the periodic signal
         ThisThread::flags_wait_any(threadFlag);
 
@@ -120,7 +192,6 @@
         if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
         else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
         pwm.write(static_cast<double>(dutyCycle));
-
     }
 }