Diff: SpeedController.cpp

Revision:

29:335fb9b01ca7

Parent:

19:518ed284d98b

--- a/SpeedController.cpp	Thu May 05 11:10:12 2022 +0000
+++ b/SpeedController.cpp	Fri May 13 20:53:33 2022 +0000
@@ -23,6 +23,11 @@
     desiredSpeed = 0.0f;
     actualSpeed = 0.0f;
     // actualAngle = 0.0f;
+    
+    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, &SpeedController::run));
@@ -84,6 +89,46 @@
     this->kp = kp;
 }
 
+/**
+ * Sets the maximum Velocity in RPS.
+ */
+void SpeedController::setMaxVelocityRPS(float maxVelocityRPS)
+{
+    if (maxVelocityRPS*60.0f <= max_voltage * kn)
+        motion.setProfileVelocity(maxVelocityRPS*60.0f);
+    else
+        motion.setProfileVelocity(max_voltage * kn);
+}
+
+/**
+ * Sets the maximum Velocity in RPM.
+ */
+void SpeedController::setMaxVelocityRPM(float maxVelocityRPM)
+{
+    if (maxVelocityRPM <= max_voltage * kn)
+        motion.setProfileVelocity(maxVelocityRPM);
+    else
+        motion.setProfileVelocity(max_voltage * kn);
+}
+
+/**
+ * Sets the maximum Acceleration in RPS/sec.
+ */
+void SpeedController::setMaxAccelerationRPS(float maxAccelerationRPS)
+{
+    motion.setProfileAcceleration(maxAccelerationRPS*60.0f);
+    motion.setProfileDeceleration(maxAccelerationRPS*60.0f);
+}
+
+/**
+ * Sets the maximum Acceleration in RPM/sec.
+ */
+void SpeedController::setMaxAccelerationRPM(float maxAccelerationRPM)
+{
+    motion.setProfileAcceleration(maxAccelerationRPM);
+    motion.setProfileDeceleration(maxAccelerationRPM);
+}
+
 void SpeedController::run()
 {
     while(true) {
@@ -97,8 +142,11 @@
         actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f);
         // actualAngle = actualAngle + actualSpeed/60.0f*TS;
 
+        motion.incrementToVelocity(desiredSpeed, TS);       
+        float desiredSpeedMotion = motion.getVelocity();
+
         // calculate motor phase voltages
-        float voltage = kp*(desiredSpeed - actualSpeed) + desiredSpeed/kn;
+        float voltage = kp*(desiredSpeedMotion - actualSpeed) + desiredSpeedMotion/kn;
 
         // calculate, limit and set duty cycles
         float dutyCycle = 0.5f + 0.5f*voltage/max_voltage;