Diff: stepper_motor_driver.cpp

Revision:

5:bd5fc0510e7b

Parent:

4:d42e1953243c

Child:

6:d38287621cca

--- a/stepper_motor_driver.cpp	Sat Apr 18 05:13:40 2020 +0000
+++ b/stepper_motor_driver.cpp	Sun Apr 19 05:38:41 2020 +0000
@@ -1,5 +1,5 @@
 
-#include "math.h"
+//#include "math.h"
 #include "mbed.h"
 #include "project_defines.h"
 #include "stepper_motor_driver.h"
@@ -75,6 +75,9 @@
 uint32_t expiration_stepper_pulse_period_us = STEPPER_DRIVER_DEFAULT_PULSE_PERIOD_US;
 uint32_t calibration_stepper_pulse_period_us = STEPPER_DRIVER_DEFAULT_PULSE_PERIOD_US;
 
+//uint32_t min_inspiration_ramp_period_us;
+//uint32_t min_expiration_ramp_period_us;
+
 // Volume setpoint (250mL, 300mL, 350ml ... 750ml) expressed in steps
 uint32_t volume_in_steps[VOLUME_SETPOINT_INDEX_LIMIT] = {
                                                        720,   //  1600, //250 mL
@@ -130,9 +133,8 @@
     int8_t stepper_volume_index;
     uint32_t regular_steps;
     float total_time, total_time_sec, insp_time_sec, exp_time_sec;
-    float start_brake_pulse_period_sec = (1.0 / STEPPER_START_BRAKE_FREQUENCY_HZ);
     float inspiration_stepper_pulse_period_sec, expiration_stepper_pulse_period_sec;
-    float a_coeff, b_coeff, c_coeff, determ;  
+    float ramp_max_pulse_freq_hz, ramp_time_sec, short_ramp_time_sec;
     
     /* Update the value of the volume setpoint (expressed in steps) */
     stepper_volume_index = (volume_setpoint - VOLUME_SETPOINT_MINIMUM_VALUE) / VOLUME_SETPOINT_STEP;
@@ -146,20 +148,17 @@
     
     /* Calculate stepper pulse period for inspiration */
     regular_steps = inspiration_stepper_pulses_setpoint - (2 * RAMP_STEPS);
-    a_coeff = (float)regular_steps;
-    b_coeff = (start_brake_pulse_period_sec*(4.0*RAMP_STEPS + regular_steps)) - insp_time_sec;
-    c_coeff = -(start_brake_pulse_period_sec * insp_time_sec);
-    determ = ((b_coeff * b_coeff) - (4 * a_coeff * c_coeff));
-    inspiration_stepper_pulse_period_sec = (sqrt(determ) - b_coeff) / (2.0*a_coeff);
+    ramp_max_pulse_freq_hz = ((float)inspiration_stepper_pulses_setpoint) / insp_time_sec;
+    ramp_time_sec = Stepper_Calculate_Insp_Period_Ramp(ramp_max_pulse_freq_hz);
+    inspiration_stepper_pulse_period_sec =  (insp_time_sec - (2*ramp_time_sec)) / (float)regular_steps;
     inspiration_stepper_pulse_period_us = (uint32_t)(1000000.0 * inspiration_stepper_pulse_period_sec);
-
+    
     /* Calculate stepper pulse period for expiration */
     regular_steps = inspiration_stepper_pulses_setpoint - (RAMP_STEPS + SHORT_RAMP_STEPS);
-    a_coeff = (float)regular_steps;
-    b_coeff = (start_brake_pulse_period_sec*((2.0*(RAMP_STEPS + SHORT_RAMP_STEPS)) + regular_steps)) - exp_time_sec;
-    c_coeff = -(start_brake_pulse_period_sec * exp_time_sec);
-    determ = ((b_coeff * b_coeff) - (4 * a_coeff * c_coeff));
-    expiration_stepper_pulse_period_sec = (sqrt(determ) - b_coeff) / (2.0*a_coeff);
+    ramp_max_pulse_freq_hz = ((float)inspiration_stepper_pulses_setpoint) / exp_time_sec;
+    ramp_time_sec = Stepper_Calculate_Exp_Period_Ramp(ramp_max_pulse_freq_hz);
+    short_ramp_time_sec = Stepper_Calculate_Exp_Short_Period_Ramp(ramp_max_pulse_freq_hz);
+    expiration_stepper_pulse_period_sec =  (exp_time_sec - (ramp_time_sec + short_ramp_time_sec)) / (float)regular_steps;
     expiration_stepper_pulse_period_us = (uint32_t)(1000000.0 * expiration_stepper_pulse_period_sec);
     
     /* Just for testing purposes */
@@ -173,49 +172,85 @@
 }
 
 
-void Stepper_Calculate_Period_Ramp(void){
+float Stepper_Calculate_Insp_Period_Ramp(float max_freq){
+      
+    float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz;
+    float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec;
+    uint8_t i;
+    
+    /* Period rise ramp for inspiration */
+    initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
+    final_frequency_hz = max_freq;
+    frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz;
+    
+    ramp_time_sec = 0;
     
-    float initial_frequency, final_frequency, frequency_ramp_range;
-    float frequency_i_hz, period_i_us;
+    for(i = 0; i < RAMP_STEPS; i++){
+        frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * sigma_ramp[i]);
+        period_i_sec = 1.0 / frequency_i_hz;
+        period_i_us = 1000000.0 * period_i_sec;
+        inspiration_period_rise_ramp_us[i] = (uint32_t)period_i_us;
+        ramp_time_sec += period_i_sec;
+    }
+    
+    return ramp_time_sec;
+}
+
+
+float Stepper_Calculate_Exp_Period_Ramp(float max_freq){
+
+    float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz;
+    float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec;
     uint8_t i;
     
     /* Period rise ramp for inspiration */
-    initial_frequency = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
-    final_frequency = 1000000.0 / (float)inspiration_stepper_pulse_period_us;
-    frequency_ramp_range = final_frequency - initial_frequency;
+    initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
+    final_frequency_hz = max_freq;
+    frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz;
     
-    for(i = 0; i < RAMP_STEPS; i++){
-        frequency_i_hz = initial_frequency + (frequency_ramp_range * sigma_ramp[i]);
-        period_i_us = 1000000.0 / frequency_i_hz;
-        inspiration_period_rise_ramp_us[i] = (uint32_t)period_i_us;
-    }
-    
-    /* Period rise ramp for expiration */
-    initial_frequency = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
-    final_frequency = 1000000.0 / (float)expiration_stepper_pulse_period_us;
-    frequency_ramp_range = final_frequency - initial_frequency;
+    ramp_time_sec = 0;
     
-    for(i = 0; i < RAMP_STEPS; i++){
-        frequency_i_hz = initial_frequency + (frequency_ramp_range * sigma_ramp[i]);
-        period_i_us = 1000000.0 / frequency_i_hz;
+     for(i = 0; i < RAMP_STEPS; i++){
+        frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * sigma_ramp[i]);
+        period_i_sec = 1.0 / frequency_i_hz;
+        period_i_us = 1000000.0 * period_i_sec;
         expiration_period_rise_ramp_us[i] = (uint32_t)period_i_us;
-    }
+        ramp_time_sec += period_i_sec;
+    }   
 
-    /* Period short rise ramp for expiration */
-    initial_frequency = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
-    final_frequency = 1000000.0 / (float)expiration_stepper_pulse_period_us;
-    frequency_ramp_range = final_frequency - initial_frequency;
-    
-    for(i = 0; i < SHORT_RAMP_STEPS; i++){
-        frequency_i_hz = initial_frequency + (frequency_ramp_range * sigma_ramp[i]);
-        period_i_us = 1000000.0 / frequency_i_hz;
-        expiration_period_short_rise_ramp_us[i] = (uint32_t)period_i_us;
-    }
+    return ramp_time_sec;
+}
+
 
 
+float Stepper_Calculate_Exp_Short_Period_Ramp(float max_freq){
+
+    float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz;
+    float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec;
+    uint8_t i;
+    
+    /* Period rise ramp for inspiration */
+    initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
+    final_frequency_hz = max_freq;
+    frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz;
+    
+    ramp_time_sec = 0;
+    
+     for(i = 0; i < SHORT_RAMP_STEPS; i++){
+        frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * short_sigma_ramp[i]);
+        period_i_sec = 1.0 / frequency_i_hz;
+        period_i_us = 1000000.0 * period_i_sec;
+        expiration_period_short_rise_ramp_us[i] = (uint32_t)period_i_us;
+        ramp_time_sec += period_i_sec;
+    }   
+
+    return ramp_time_sec;
 }
 
 
+
+
+
 void Stepper_Driver_State_Machine(void){
     
     switch(stepper_driver_state){
@@ -260,7 +295,7 @@
             if(stepper_parameters_update_flag){
                 stepper_parameters_update_flag = 0;
                 Stepper_Update_Parameters();
-                Stepper_Calculate_Period_Ramp();
+                //Stepper_Calculate_Period_Ramp();
             }
                 
             stepper_pulse_counter = 0;