Diff: PositionController.cpp

Revision:

29:335fb9b01ca7

Parent:

19:518ed284d98b

Child:

30:05abc1d2a2b9

--- a/PositionController.cpp	Thu May 05 11:10:12 2022 +0000
+++ b/PositionController.cpp	Fri May 13 20:53:33 2022 +0000
@@ -12,7 +12,7 @@
     setSpeedCntrlGain(0.1f);
     this->max_voltage = max_voltage;
     this->max_speed   = kn*max_voltage;
-    setPositionCntrlGain(1300.0f);
+    setPositionCntrlGain(22.0f);
 
     // initialise pwm
     pwm.period(0.00005); // pwm period of 50 us
@@ -28,64 +28,11 @@
     this->initialRotation = initialRotation;
     actualRotation  = initialRotation;
     desiredRotation = initialRotation;
-
-    // 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)});
-}
-
-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);
-    desiredSpeed = 0.0f;
-    actualSpeed = 0.0f;
-    float initialRotation = (float)encoderCounter.read()/counts_per_turn;
-    this->initialRotation = initialRotation;
-    actualRotation  = initialRotation;
-    desiredRotation = initialRotation;
-
-    // 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)});
-}
-
-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;
-    setFeedForwardGain(kn);
-    setSpeedCntrlGain(kp);
-    this->max_voltage = max_voltage;
-    this->max_speed   = kn*max_voltage;
-    setPositionCntrlGain(p);
-
-    // 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;
+    
+    motion.setProfileVelocity(max_voltage * kn);
+    float maxAcceleration = 22.0f * max_voltage * kn * 0.4f; // pmic, 13.05.2022, only 40%, based on simple measurement, max_voltage * kn is gearratio
+    motion.setProfileAcceleration(maxAcceleration);
+    motion.setProfileDeceleration(maxAcceleration);
 
     // set up thread
     thread.start(callback(this, &PositionController::run));
@@ -124,19 +71,6 @@
 }
 
 /**
- * Sets desired rotation (1 corresponds to 360 deg) and max. rotational speed in RPS (rotations per second).
- */
-void PositionController::setDesiredRotation(float desiredRotation, float maxSpeedRPS)
-{
-    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.
  */
@@ -169,6 +103,46 @@
     this->p = p;
 }
 
+/**
+ * Sets the maximum Velocity in RPS.
+ */
+void PositionController::setMaxVelocityRPS(float maxVelocityRPS)
+{
+    if (maxVelocityRPS*60.0f <= max_speed)
+        motion.setProfileVelocity(maxVelocityRPS*60.0f);
+    else
+        motion.setProfileVelocity(max_speed);
+}
+
+/**
+ * Sets the maximum Velocity in RPM.
+ */
+void PositionController::setMaxVelocityRPM(float maxVelocityRPM)
+{
+    if (maxVelocityRPM <= max_speed)
+        motion.setProfileVelocity(maxVelocityRPM);
+    else
+        motion.setProfileVelocity(max_speed);
+}
+
+/**
+ * Sets the maximum Acceleration in RPS/sec.
+ */
+void PositionController::setMaxAccelerationRPS(float maxAccelerationRPS)
+{
+    motion.setProfileAcceleration(maxAccelerationRPS*60.0f);
+    motion.setProfileDeceleration(maxAccelerationRPS*60.0f);
+}
+
+/**
+ * Sets the maximum Acceleration in RPM/sec.
+ */
+void PositionController::setMaxAccelerationRPM(float maxAccelerationRPM)
+{
+    motion.setProfileAcceleration(maxAccelerationRPM);
+    motion.setProfileDeceleration(maxAccelerationRPM);
+}
+
 void PositionController::run()
 {
     while(true) {
@@ -182,10 +156,16 @@
         actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f);
         actualRotation = actualRotation + actualSpeed/60.0f*TS;
 
+        motion.incrementToPosition(60.0f * desiredRotation, TS);       
+        float desiredRotationMotion = motion.getPosition();
+        float desiredVelocityMotion = motion.getVelocity();
+
         // calculate motor phase voltages
-        desiredSpeed  = p*(desiredRotation - actualRotation);
+        desiredSpeed  = p *(desiredRotationMotion - 60.0f * actualRotation) + 0.0f * desiredVelocityMotion;
+        
         if (desiredSpeed < -max_speed) desiredSpeed = -max_speed;
         else if (desiredSpeed > max_speed) desiredSpeed = max_speed;
+        
         float voltage = kp*(desiredSpeed - actualSpeed) + desiredSpeed/kn;
         // calculate, limit and set duty cycles
         float dutyCycle = 0.5f + 0.5f*voltage/max_voltage;