Diff: SpeedController.cpp

Revision:

6:41dd03654c44

Parent:

5:6cd242a61e4c

--- a/SpeedController.cpp	Tue Apr 06 12:19:29 2021 +0000
+++ b/SpeedController.cpp	Wed Apr 07 12:13:45 2021 +0000
@@ -2,21 +2,17 @@
 
 using namespace std;
 
-const float SpeedController::PERIOD = 0.001f;                    // period of 1 ms
-// const float SpeedController::COUNTS_PER_TURN = 1562.5f;          // encoder resolution
-const float SpeedController::LOWPASS_FILTER_FREQUENCY = 100.0f;  // given in [rad/s]
-// const float SpeedController::KN = 15.0f;                         // speed constant in [rpm/V]
-// const float SpeedController::KP = 0.15f;                         // speed control parameter
-// const float SpeedController::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
-const float SpeedController::MIN_DUTY_CYCLE = 0.02f;             // minimum duty-cycle
-const float SpeedController::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle
+const float SpeedController::TS = 0.001f;                       // period of 1 ms
+const float SpeedController::LOWPASS_FILTER_FREQUENCY = 100.0f; // given in [rad/s]
+const float SpeedController::MIN_DUTY_CYCLE = 0.0f;             // minimum duty-cycle
+const float SpeedController::MAX_DUTY_CYCLE = 1.0f;             // maximum duty-cycle
 
-SpeedController::SpeedController(float COUNTS_PER_TURN, float KN, float KP, float MAX_VOLTAGE, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
+SpeedController::SpeedController(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 = KP;
-    this->MAX_VOLTAGE = MAX_VOLTAGE;
+    this->counts_per_turn = counts_per_turn;
+    this->kn = kn;
+    this->kp = 0.1f;
+    this->max_voltage = max_voltage;
 
     // Initialisieren der PWM Ausgaenge
     pwm.period(0.00005); // PWM Periode von 50 us
@@ -24,7 +20,7 @@
 
     // Initialisieren von lokalen Variabeln
     previousValueCounter = encoderCounter.read();
-    speedFilter.setPeriod(PERIOD);
+    speedFilter.setPeriod(TS);
     speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
     desiredSpeed = 0.0f;
     actualSpeed = 0.0f;
@@ -32,7 +28,31 @@
 
     // Starten des periodischen Tasks
     thread.start(callback(this, &SpeedController::run));
-    ticker.attach(callback(this, &SpeedController::sendThreadFlag), PERIOD);
+    ticker.attach(callback(this, &SpeedController::sendThreadFlag), TS);
+}
+
+SpeedController::SpeedController(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;
+    this->kn = kn;
+    this->kp = kp;
+    this->max_voltage = max_voltage;
+
+    // Initialisieren der PWM Ausgaenge
+    pwm.period(0.00005); // PWM Periode von 50 us
+    pwm.write(0.5);      // Duty-Cycle von 50%
+
+    // Initialisieren von lokalen Variabeln
+    previousValueCounter = encoderCounter.read();
+    speedFilter.setPeriod(TS);
+    speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+    desiredSpeed = 0.0f;
+    actualSpeed = 0.0f;
+    // actualAngle = 0.0f;
+
+    // Starten des periodischen Tasks
+    thread.start(callback(this, &SpeedController::run));
+    ticker.attach(callback(this, &SpeedController::sendThreadFlag), TS);
 }
 
 SpeedController::~SpeedController()
@@ -71,14 +91,14 @@
         short valueCounter = encoderCounter.read();
         short countsInPastPeriod = valueCounter - previousValueCounter;
         previousValueCounter = valueCounter;
-        actualSpeed = speedFilter.filter((float)countsInPastPeriod/COUNTS_PER_TURN/PERIOD*60.0f);
-        // actualAngle = actualAngle + actualSpeed/60.0f*PERIOD;
+        actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f);
+        // actualAngle = actualAngle + actualSpeed/60.0f*TS;
 
         // calculate motor phase voltages
-        float voltage = KP*(desiredSpeed - actualSpeed) + desiredSpeed/KN;
+        float voltage = kp*(desiredSpeed - actualSpeed) + desiredSpeed/kn;
 
         // calculate, limit and set duty cycles
-        float dutyCycle = 0.5f + 0.5f*voltage/MAX_VOLTAGE;
+        float dutyCycle = 0.5f + 0.5f*voltage/max_voltage;
         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));