shrink memory

Author: askuric

src/common/base_classes/FOCMotor.cpp

@@ -600,7 +600,8 @@ void FOCMotor::updateMotionControlType(MotionControlType new_motion_controller)
   }
 
   if (new_motion_controller == MotionControlType::angle_profile) {
-    trajectoryResetTrapezoidal(profile_state, shaft_angle, 0.0f, target);
+    profile_state.position = shaft_angle;
+    profile_state.velocity = 0.0f;
     profile_state.initialized = false;
   } else if (controller == MotionControlType::angle_profile) {
     profile_state.velocity = 0.0f;
@@ -825,25 +826,66 @@ void FOCMotor::move(float new_target) {
       // filter the measured angle to reduce noise and improve stability
       float measured_angle = LPF_angle(shaft_angle);
 
-      // if acceleration limit is set, use trapezoidal profile to 
-      // generate the angle and velocity set points
+      // if acceleration limit is set, use inline trapezoidal profile (bang-bang control)
       if (acceleration_limit > 0.0f) {
         // calculate the time step in seconds
         float dt = move_time_us * 1e-6f;
         if (dt <= 0.0f || dt > 0.5f) dt = 1e-3f;
 
-        TrapezoidalProfileOutput prof = trajectoryStepTrapezoidal(profile_state,
-                                                                  measured_angle,
-                                                                  shaft_velocity,
-                                                                  dt,
-                                                                  target,
-                                                                  velocity_limit,
-                                                                  acceleration_limit);
-
-        // update the angle set point with the value from the trapezoidal profile                           
-        shaft_angle_sp = prof.position;
-        // update the feed forward velocity with the value from the trapezoidal profile
-        velocity_ff += prof.velocity; 
+        // Initialize profile on first call
+        if (!profile_state.initialized) {
+          profile_state.position = measured_angle;
+          profile_state.velocity = shaft_velocity;
+          profile_state.initialized = true;
+        }
+        
+        // Bang-bang trapezoidal velocity profile
+        float direction = _sign(target - profile_state.position);
+        float distance_remaining = fabsf(target - profile_state.position);
+        float speed_magnitude = fabsf(profile_state.velocity);
+        
+        // IMPORTANT
+        // for given velocity v and acceleration a, the time to stop t = v/a - (following from v = a.t )
+        // and the distance traveled during that time is d = v.t - 0.5.a.t^2 = v^2 / (2.a)
+        float stopping_distance = (speed_magnitude * speed_magnitude) / (2.0f * acceleration_limit);
+        
+        // Determine commanded acceleration (bang-bang)
+        float commanded_accel = 0.0f;
+        if (profile_state.velocity * direction < 0.0f) {
+          // Moving wrong direction: accelerate toward target
+          commanded_accel = direction * acceleration_limit;
+        } else if (stopping_distance >= distance_remaining) {
+          // Close enough to stop: decelerate
+          commanded_accel = -direction * acceleration_limit;
+        } else if (speed_magnitude < velocity_limit) {
+          // Below speed limit and far from target: accelerate
+          commanded_accel = direction * acceleration_limit;
+        } else {
+          // At speed limit: maintain
+          commanded_accel = 0.0f;
+        }
+        
+        // Integrate: new velocity
+        profile_state.velocity += commanded_accel * dt;
+        profile_state.velocity = _constrain(profile_state.velocity, -velocity_limit, velocity_limit);
+        
+        // Integrate: new position
+        float position_step = profile_state.velocity * dt;
+        if (distance_remaining < 1e-6f && speed_magnitude < (acceleration_limit * dt)) {
+          // Nearly at target and slow: snap to target
+          profile_state.position = target;
+          profile_state.velocity = 0.0f;
+        } else if (distance_remaining <= fabsf(position_step)) {
+          // Final step would overshoot: clamp
+          profile_state.position = target;
+          profile_state.velocity = 0.0f;
+        } else {
+          profile_state.position += position_step;
+        }
+        
+        // Update setpoint with profiled values
+        shaft_angle_sp = profile_state.position;
+        velocity_ff += profile_state.velocity;
       }
 
       // calculate velocity set point

src/common/trajectory.cpp

@@ -1,58 +1,28 @@
 #ifndef TRAJECTORY_H
 #define TRAJECTORY_H
-
-#include "foc_utils.h"
-
 /**
- * State of a trapezoidal motion profile
+ * Trajectory profile implementation - INLINED
+ * 
+ * All motion profile logic has been moved directly into FOCMotor::move()
+ * for the angle_profile control case to reduce flash memory footprint
+ * on resource-constrained boards (e.g., Arduino Leonardo).
+ * 
+ * For reference on bang-bang motion profiles:
+ * - https://mechatronics.studio/blog/bang-bang-motion-profile/
+ * - https://www.youtube.com/watch?v=qYJpl7SNoww
+ * 
+ * This file is kept for reference but generates no code.
  */
-struct TrapezoidalProfileState {
-  float position = 0.0f;
-  float velocity = 0.0f;
-  // float acceleration = 0.0f; // not used so commented to save memory
-  // float target = 0.0f;
-  bool initialized = false;
-};
 
+ 
 /**
- *  Output of a trapezoidal motion profile step
+ * Minimal state for inline trapezoidal profile (no external functions)
+ * Logic is implemented directly in FOCMotor::move() for angle_profile case
  */
-struct TrapezoidalProfileOutput {
+struct TrapezoidalProfileState {
   float position = 0.0f;
   float velocity = 0.0f;
-  float acceleration = 0.0f;
+  bool initialized = false;
 };
 
-/**
- *  Resets the trapezoidal profile state to the current position and velocity, and sets the target position.
- * @param state - the current state of the trapezoidal profile (position, velocity, acceleration, target, initialized)
- * @param current_position - the current position of the system
- * @param current_velocity - the current velocity of the system
- * @param target_position - the desired target position to reach
- */
-void trajectoryResetTrapezoidal(TrapezoidalProfileState& state,
-                                float current_position,
-                                float current_velocity,
-                                float target_position);
-
-/**
- *  Calculates the next step of a trapezoidal motion profile given the current state, target position, and limits.
- *  @param state - the current state of the trapezoidal profile (position, velocity, acceleration, target, initialized)
- *  @param current_position - the current position of the system
- *  @param current_velocity - the current velocity of the system
- *  @param dt - the time step in seconds
- *  @param target_position - the desired target position to reach
- *  @param velocity_limit - the maximum allowed velocity (absolute value)
- *  @param acceleration_limit - the maximum allowed acceleration (absolute value)
- *  @returns a TrapezoidalProfileOutput struct containing the updated position, velocity, and acceleration for the next step of the profile
- * 
- */
-TrapezoidalProfileOutput trajectoryStepTrapezoidal(TrapezoidalProfileState& state,
-                                                   float current_position,
-                                                   float current_velocity,
-                                                   float dt,
-                                                   float target_position,
-                                                   float velocity_limit,
-                                                   float acceleration_limit);
-
 #endif