Diff: stepper_motor_driver.cpp

Revision:

4:d42e1953243c

Parent:

3:45299e7882b9

Child:

5:bd5fc0510e7b

--- a/stepper_motor_driver.cpp	Fri Apr 17 11:22:43 2020 +0000
+++ b/stepper_motor_driver.cpp	Sat Apr 18 05:13:40 2020 +0000
@@ -1,5 +1,5 @@
 
-
+#include "math.h"
 #include "mbed.h"
 #include "project_defines.h"
 #include "stepper_motor_driver.h"
@@ -128,7 +128,11 @@
 void Stepper_Update_Parameters(void){
     
     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;  
     
     /* Update the value of the volume setpoint (expressed in steps) */
     stepper_volume_index = (volume_setpoint - VOLUME_SETPOINT_MINIMUM_VALUE) / VOLUME_SETPOINT_STEP;
@@ -136,17 +140,35 @@
     
     /* Update the value of the pulse periods in microseconds */
     total_time = inspiration_time + expiration_time;
-    total_time_sec = 60 / resp_frequency;
+    total_time_sec = 60.0 / resp_frequency;
     insp_time_sec = (total_time_sec * inspiration_time) / total_time;
     exp_time_sec = (total_time_sec * expiration_time) / total_time;
-    inspiration_stepper_pulse_period_us = (uint32_t)((1000000.0 * insp_time_sec) / inspiration_stepper_pulses_setpoint);
-    expiration_stepper_pulse_period_us = (uint32_t)((1000000.0 * exp_time_sec) / inspiration_stepper_pulses_setpoint);
+    
+    /* 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);
+    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);
+    expiration_stepper_pulse_period_us = (uint32_t)(1000000.0 * expiration_stepper_pulse_period_sec);
     
     /* Just for testing purposes */
-    pc.printf("Periodo de pulsos de ida: %d us\n\r", inspiration_stepper_pulse_period_us);
-    pc.printf("Periodo de pulsos de vuelta: %d us\n\r", expiration_stepper_pulse_period_us);
     pc.printf("Cantidad de pulsos: %d \n\n\r", inspiration_stepper_pulses_setpoint);
-
+    pc.printf("Frecuencia de arranque: %d Hz\n\r", STEPPER_START_BRAKE_FREQUENCY_HZ);
+    pc.printf("Frecuencia de pulsos de ida: %f Hz\n\r", 1.0 / inspiration_stepper_pulse_period_sec);
+    pc.printf("Periodo de pulsos de ida en us: %d us\n\r", inspiration_stepper_pulse_period_us);
+    pc.printf("Frecuencia de pulsos de vuelta: %f Hz\n\r", 1.0 / expiration_stepper_pulse_period_us);
+    pc.printf("Periodo de pulsos de vuelta en us: %d us\n\r", expiration_stepper_pulse_period_us);
     
 }