leonardo remove profiling

Author: askuric

src/common/base_classes/FOCMotor.cpp

@@ -1,3 +1,4 @@
+#include "../flags.h"
 #include "FOCMotor.h"
 #include "../../communication/SimpleFOCDebug.h"
 
@@ -25,9 +26,11 @@ FOCMotor::FOCMotor()
 
   // default target value
   target = 0;
+  #ifndef SIMPLEFOC_NO_PROFILING
   profile_state.position = 0;
   profile_state.velocity = 0;
   profile_state.initialized = false;
+  #endif
   open_loop_velocity = 0;
   voltage.d = 0;
   voltage.q = 0;
@@ -189,7 +192,7 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
     unsigned int iterations = 40;    // how often the algorithm gets repeated.
     unsigned int cycles = 3;         // averaged measurements for each iteration
     unsigned int risetime_us = 200;  // initially short for worst case scenario with low inductance
-    unsigned int settle_us = 100000; // initially long for worst case scenario with high inductance
+    uint32_t settle_us = 100000; // initially long for worst case scenario with high inductance
 
     // Pre-rotate the angle to the q-axis (only useful with sensor, else no harm in doing it)
     current_electric_angle += 0.5f * _PI;
@@ -421,11 +424,9 @@ float FOCMotor::velocityOpenloop(float target_velocity){
   if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
 
   // if acceleration limit is set, use it to calculate the target velocity
-  if (_isset(acceleration_limit) && acceleration_limit > 0.0f) {
+  if (acceleration_limit > 0.0f) {
     // limit the change in velocity to the acceleration limit
-    float dv_max = acceleration_limit * Ts;
-    float dv = _constrain(target_velocity - open_loop_velocity, -dv_max, dv_max);
-    open_loop_velocity += dv;
+    open_loop_velocity += _constrain(target_velocity - open_loop_velocity, -acceleration_limit * Ts, acceleration_limit * Ts);
   } else {
     open_loop_velocity = target_velocity;
   }
@@ -454,34 +455,31 @@ float FOCMotor::angleOpenloop(float target_angle){
   // calculate the sample time from last call
   float Ts = (now_us - open_loop_timestamp) * 1e-6f;
   // quick fix for strange cases (micros overflow + timestamp not defined)
-  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+  if(Ts <= 0.0f || Ts > 0.5f) Ts = 1e-3f;
 
-  float err = target_angle - shaft_angle;
-  float target_velocity = 0.0f;
   // if acceleration limit is set, use it to calculate the target velocity
   if (acceleration_limit > 0.0f) { 
     // calculate the necessary velocity to stop at the target angle with the given acceleration limit
-    float stop_vel = sqrtf(2.0f * acceleration_limit * fabsf(err));
+    float stop_vel = sqrtf(2.0f * acceleration_limit * fabsf(target_angle - shaft_angle));
     // limit the target velocity to the velocity limit
-    target_velocity = _sign(err) * _constrain(stop_vel, 0.0f, velocity_limit);
+    float target_velocity = _sign(target_angle - shaft_angle) * _constrain(stop_vel, 0.0f, velocity_limit);
 
     // limit the change in velocity to the acceleration limit
-    float dv_max = acceleration_limit * Ts;
-    float dv = _constrain(target_velocity - open_loop_velocity, -dv_max, dv_max);
-    
-    // update the open loop velocity
-    open_loop_velocity += dv;
+    open_loop_velocity += _constrain(target_velocity - open_loop_velocity, -acceleration_limit * Ts, acceleration_limit * Ts);
 
     // integrate the open loop velocity to get the new shaft angle
     float step = open_loop_velocity * Ts;
-    if ((fabsf(err) <= fabsf(step)) || (fabsf(err) < 1e-6f && fabsf(open_loop_velocity) <= dv_max)) {
+    if ((fabsf(target_angle - shaft_angle) <= fabsf(step)) || 
+        (fabsf(target_angle - shaft_angle) < 1e-6f &&  
+         fabsf(open_loop_velocity) <= (acceleration_limit * Ts))) {
       shaft_angle = target_angle;
       open_loop_velocity = 0.0f;
     } else {
       shaft_angle += step;
     }
     shaft_velocity = open_loop_velocity;
-  } else {
+  } 
+  else {
     // if no acceleration limit is set, use the velocity limit to calculate the target velocity
     // calculate the necessary angle to move from current position towards target angle
     // with maximal velocity (velocity_limit)
@@ -599,13 +597,15 @@ void FOCMotor::updateMotionControlType(MotionControlType new_motion_controller)
     break;
   }
 
+  #ifndef SIMPLEFOC_NO_PROFILING
   if (new_motion_controller == MotionControlType::angle_profile) {
     trajectoryResetTrapezoidal(profile_state, shaft_angle, 0.0f, target);
     profile_state.initialized = false;
   } else if (controller == MotionControlType::angle_profile) {
     profile_state.velocity = 0.0f;
     profile_state.initialized = false;
   }
+  #endif
 
   if (new_motion_controller == MotionControlType::angle_openloop || new_motion_controller == MotionControlType::velocity_openloop) {
     open_loop_velocity = 0.0f;
@@ -813,7 +813,9 @@ void FOCMotor::move(float new_target) {
       current_sp = P_angle(shaft_angle_sp - LPF_angle(shaft_angle));
       break;
     }
-    case MotionControlType::angle_profile: {
+    case MotionControlType::angle_profile: 
+    #ifndef SIMPLEFOC_NO_PROFILING
+    {
       // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
       //                        the angles are large. This results in not being able to command small changes at high position values.
       //                        to solve this, the delta-angle has to be calculated in a numerically precise way.
@@ -853,6 +855,7 @@ void FOCMotor::move(float new_target) {
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); 
       break;
     }
+    #endif
     case MotionControlType::angle: 
       // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
       //                        the angles are large. This results in not being able to command small changes at high position values.

src/common/base_classes/FOCMotor.h

@@ -5,12 +5,17 @@
 #include "Sensor.h"
 #include "CurrentSense.h"
 
+#include "../flags.h"
+
+
 #include "../time_utils.h"
 #include "../foc_utils.h"
 #include "../defaults.h"
 #include "../pid.h"
 #include "../lowpass_filter.h"
+#ifndef SIMPLEFOC_NO_PROFILING
 #include "../trajectory.h"
+#endif
 
 #define MOT_ERR "ERR-MOT:"
 #define MOT_WARN "WARN-MOT:"
@@ -55,7 +60,9 @@ enum MotionControlType : uint8_t {
   angle_openloop    = 0x04,
   angle_nocascade   = 0x05,     //!< Position/angle motion control without velocity cascade
   custom            = 0x06,     //!< Custom control method - control method added by user
+  //#ifndef SIMPLEFOC_NO_PROFILING
   angle_profile     = 0x07      //!< Position/angle motion control with trapezoidal profile
+  //#endif
 };
 
 /**
@@ -452,9 +459,11 @@ class FOCMotor
     uint32_t open_loop_timestamp;
     float open_loop_velocity = 0.0f;
 
+    #ifndef SIMPLEFOC_NO_PROFILING
     // angle profile state
-    TrapezoidalProfileState profile_state;
-    
+    TrapezoidalProfileState 2;
+    #endif
+
     // function pointer for custom control method
     float (*customMotionControlCallback)(FOCMotor* motor) = nullptr;
 

src/common/flags.h

@@ -0,0 +1,10 @@
+#ifndef SIMPLEFOC_FLAGS_H
+#define SIMPLEFOC_FLAGS_H
+
+
+#include "Arduino.h"
+#if defined(__AVR_ATmega32U4__) && !defined(SIMPLEFOC_NO_PROFILING)
+	#define SIMPLEFOC_NO_PROFILING
+#endif
+
+#endif

src/common/trajectory.cpp

@@ -1,6 +1,7 @@
 #include "trajectory.h"
-
-// for more information about the trapezoidal profile, see https://en.wikipedia.org/wiki/Trapezoidal_velocity_profile
+#ifndef SIMPLEFOC_NO_PROFILING
+// for more information about the trapezoidal profile, see https://mechatronics.studio/blog/bang-bang-motion-profile/
+// and the main motivation :D - https://www.youtube.com/watch?v=qYJpl7SNoww
 
 // Resets the trapezoidal profile state to the current position and velocity, 
 // and sets the target position.
@@ -12,6 +13,24 @@ void trajectoryResetTrapezoidal(TrapezoidalProfileState& state,
   state.velocity = current_velocity;
   state.initialized = true;
 }
+
+#ifdef __AVR_ATmega32U4__
+// For AVR ATmega32U4 platforms, we provide a simplified version of the trapezoidal profile step function
+TrapezoidalProfileOutput trajectoryStepTrapezoidal(TrapezoidalProfileState& state,
+                                                   float current_position,
+                                                   float current_velocity,
+                                                   float dt,
+                                                   float target_position,
+                                                   float velocity_limit,
+                                                   float acceleration_limit) {
+  TrapezoidalProfileOutput out;
+  out.position = target_position;
+  out.velocity = 0.0f;
+  return out;
+}
+#else
+// For non-AVR platforms, we can use the standard library functions for absolute value and
+
 // Bang-bang acceleration control with trapezoidal velocity profile
 TrapezoidalProfileOutput trajectoryStepTrapezoidal(TrapezoidalProfileState& state,
                                                    float current_position,
@@ -100,4 +119,7 @@ TrapezoidalProfileOutput trajectoryStepTrapezoidal(TrapezoidalProfileState& stat
   out.position = state.position;
   out.velocity = state.velocity;
   return out;
-}
+}
+
+#endif
+#endif

src/common/trajectory.h

@@ -1,8 +1,9 @@
 #ifndef TRAJECTORY_H
 #define TRAJECTORY_H
 
+#include "flags.h"
 #include "foc_utils.h"
-
+#ifndef SIMPLEFOC_NO_PROFILING
 /**
  * State of a trapezoidal motion profile
  */
@@ -55,4 +56,5 @@ TrapezoidalProfileOutput trajectoryStepTrapezoidal(TrapezoidalProfileState& stat
                                                    float velocity_limit,
                                                    float acceleration_limit);
 
+#endif
 #endif