Diff: SpeedController.cpp

Revision:

4:9c003c402033

Parent:

3:8b42e643b294

Child:

5:6cd242a61e4c

--- a/SpeedController.cpp	Thu Apr 01 14:31:43 2021 +0000
+++ b/SpeedController.cpp	Tue Apr 06 11:21:54 2021 +0000
@@ -3,16 +3,20 @@
 using namespace std;
 
 const float SpeedController::PERIOD = 0.001f;                    // period of 1 ms
-const float SpeedController::COUNTS_PER_TURN = 1560.0f;          // encoder resolution
-const float SpeedController::LOWPASS_FILTER_FREQUENCY = 300.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::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
 
-SpeedController::SpeedController(PwmOut& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
+SpeedController::SpeedController(float COUNTS_PER_TURN, float KN, float KP, float MAX_VOLTAGE, PwmOut& 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.00005f); // PWM Periode von 50 us
@@ -24,7 +28,7 @@
     speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
     desiredSpeed = 0.0f;
     actualSpeed = 0.0f;
-    actualAngle = 0.0f;
+    // actualAngle = 0.0f;
 
     // Starten des periodischen Tasks
     thread.start(callback(this, &SpeedController::run));
@@ -36,17 +40,26 @@
     ticker.detach(); // Stoppt den periodischen Task
 }
 
-
-void SpeedController::setDesiredSpeed(float desiredSpeed)
+void SpeedController::setDesiredSpeedRPM(float desiredSpeed)
 {
     this->desiredSpeed = desiredSpeed;
 }
 
-float SpeedController::getSpeed()
+float SpeedController::getSpeedRPM()
 {
     return actualSpeed;
 }
 
+void SpeedController::setDesiredSpeedRPS(float desiredSpeed)
+{
+    this->desiredSpeed = desiredSpeed*60.0f;
+}
+
+float SpeedController::getSpeedRPS()
+{
+    return actualSpeed/60.0f;
+}
+
 void SpeedController::run()
 {
     while(true) {
@@ -56,16 +69,16 @@
 
         // calculate actual speed of motors in [rpm]
         short valueCounter = encoderCounter.read();
-        short countsInPastPeriod = valueCounter-previousValueCounter;
+        short countsInPastPeriod = valueCounter - previousValueCounter;
         previousValueCounter = valueCounter;
         actualSpeed = speedFilter.filter((float)countsInPastPeriod/COUNTS_PER_TURN/PERIOD*60.0f);
-        actualAngle = actualAngle + actualSpeed/60.0f*PERIOD;
+        // actualAngle = actualAngle + actualSpeed/60.0f*PERIOD;
 
         // 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(dutyCycle);