Miguel Angel Caycho
para el ventilador
Diff: stepper_motor_driver.cpp
- Revision:
- 10:b2d87404309a
- Parent:
- 9:95fdcdc0977e
- Child:
- 11:5cb7ae8bd831
--- a/stepper_motor_driver.cpp Sun Apr 26 08:50:12 2020 +0000
+++ b/stepper_motor_driver.cpp Mon Apr 27 19:52:02 2020 +0000
@@ -1,4 +1,4 @@
-
+
//#include "math.h"
#include "mbed.h"
#include "project_defines.h"
@@ -19,79 +19,38 @@
volatile uint8_t stepper_parameters_update_flag = 0;
-// ----------------------- Tables used for calculations ------------------------
-
-#if RAMP_STEPS == 50
-
-const float sigma_ramp[RAMP_STEPS] = {
- 0.002, 0.003, 0.004, 0.005, 0.006, 0.008, 0.010, 0.013, 0.017, 0.021,
- 0.027, 0.034, 0.042, 0.053, 0.067, 0.083, 0.103, 0.128, 0.157, 0.192,
- 0.231, 0.277, 0.327, 0.382, 0.440, 0.500, 0.560, 0.618, 0.673, 0.723,
- 0.769, 0.808, 0.843, 0.872, 0.897, 0.917, 0.933, 0.947, 0.958, 0.966,
- 0.973, 0.979, 0.983, 0.987, 0.990, 0.992, 0.994, 0.995, 0.996, 0.997
-};
-
-#elif RAMP_STEPS == 100
-
- const float sigma_ramp[RAMP_STEPS] = {
- 0.002, 0.003, 0.003, 0.004, 0.004, 0.005, 0.005, 0.006, 0.006, 0.007,
- 0.008, 0.009, 0.011, 0.012, 0.013, 0.015, 0.017, 0.019, 0.021, 0.024,
- 0.027, 0.031, 0.034, 0.039, 0.043, 0.049, 0.055, 0.061, 0.069, 0.077,
- 0.086, 0.096, 0.107, 0.119, 0.133, 0.147, 0.163, 0.180, 0.199, 0.219,
- 0.240, 0.263, 0.287, 0.313, 0.339, 0.367, 0.396, 0.425, 0.455, 0.485,
- 0.515, 0.545, 0.575, 0.604, 0.633, 0.661, 0.687, 0.713, 0.737, 0.760,
- 0.781, 0.801, 0.820, 0.837, 0.853, 0.867, 0.881, 0.893, 0.904, 0.914,
- 0.923, 0.931, 0.939, 0.945, 0.951, 0.957, 0.961, 0.966, 0.969, 0.973,
- 0.976, 0.979, 0.981, 0.983, 0.985, 0.987, 0.988, 0.989, 0.991, 0.992,
- 0.993, 0.994, 0.994, 0.995, 0.995, 0.996, 0.996, 0.997, 0.997, 0.998
-};
-
-#else
- #error "Seleccionar un valor permitido para RAMP_STEPS: Puede ser 50 o 100"
-#endif
-
-const float short_sigma_ramp[SHORT_RAMP_STEPS] = {
- 0.002, 0.004, 0.007, 0.011, 0.018, 0.029, 0.047, 0.076, 0.119, 0.182,
- 0.269, 0.378, 0.500, 0.622, 0.731, 0.818, 0.881, 0.924, 0.953, 0.971,
- 0.982, 0.989, 0.993, 0.996, 0.998
-};
-
-
-
-// -----------------------------------------------------------------------------
-
-uint32_t calibration_period_rise_ramp_us[RAMP_STEPS];
-uint32_t inspiration_period_rise_ramp_us[RAMP_STEPS];
-uint32_t expiration_period_rise_ramp_us[RAMP_STEPS];
-uint32_t expiration_period_short_rise_ramp_us[SHORT_RAMP_STEPS];
// These parameters are configurer through the graphic interface */
uint32_t inspiration_stepper_pulses_setpoint = STEPPER_DRIVER_INSPIRATION_PULSES_DEFAULT ;
//uint32_t expiration_stepper_pulses_setpoint = STEPPER_DRIVER_MAX_PULSES ; // Not used in this version
-uint32_t inspiration_stepper_pulse_period_us = STEPPER_DRIVER_DEFAULT_PULSE_PERIOD_US;
-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 inspiration_stepper_pulse_period_us = STEPPER_DRIVER_DEFAULT_PULSE_PERIOD_US;
+//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;
+
+float inspiration_stepper_pulse_period = 0.005;
+float expiration_stepper_pulse_period = 0.005;
+float calibration_stepper_pulse_period = 0.005;
// Volume setpoint (250mL, 300mL, 350ml ... 750ml) expressed in steps
uint32_t volume_in_steps[VOLUME_SETPOINT_INDEX_LIMIT] = {
- 720, // 1600, //250 mL
- 765, // 1700, //300 mL
- 810, // 1800, //350 mL
- 855, // 1900, //400 mL
- 900, // 2000, //450 mL
- 945, // 2100, //500 mL
- 990, // 2200, //550 mL
- 1035, // 2300, //600 mL
- 1080, // 2400, //650 mL
- 1125, // 2500, //700 mL
- 1170 // 2600 //750 mL
+ 40, // 1600, //250 mL//720
+ 44, // 1700, //300 mL//765
+ 46, // 1800, //350 mL//810
+ 49, // 1900, //400 mL//855
+ 52, // 2000, //450 mL//900
+ 55, // 2100, //500 mL//945
+ 57, // 2200, //550 mL//990
+ 60, // 2300, //600 mL //1035
+ 63, // 2400, //650 mL//1080
+ 65, // 2500, //700 mL//1125
+ 67 // 2600 //750 mL//1170
};
// Variables used in every state of the stepper motor driver */
volatile uint32_t stepper_pulse_counter = 0;
-volatile uint32_t stepper_ramp_pulse_counter = 0;
+//volatile uint32_t stepper_ramp_pulse_counter = 0;
// State of the stepper motor
volatile Stepper_Driver_State_t stepper_driver_state = Stepper_Stand_By;
@@ -102,7 +61,7 @@
void Stepper_Driver_Init(void){
stepper_en = STEPPER_ENABLED;
stepper_dir = TO_HOME;
- stepper_pulse.period_us((int)calibration_stepper_pulse_period_us);
+ stepper_pulse.period(calibration_stepper_pulse_period);
stepper_pulse.write(0.0f);
stepper_pulse_feedback.fall(&Stepper_Driver_Pulse_Signal_Update);
stepper_driver_state = Stepper_Inspiration_Finish;
@@ -112,8 +71,6 @@
}
-
-
void Stepper_Go_Home_Position(void){
// stepper_driver_state = Stepper_Driver_Home_Ramp;
@@ -143,176 +100,50 @@
exp_time_sec = (total_time_sec * expiration_time) / total_time;
/* Calculate stepper pulse period for inspiration */
- regular_steps = inspiration_stepper_pulses_setpoint - (2 * RAMP_STEPS);
- /* Asume an initial value for the max frrquency of the ramp */
- ramp_max_pulse_freq_hz = TUNING_FACTOR * (((float)inspiration_stepper_pulses_setpoint) / insp_time_sec);
- /* Calculate the period ramp and the total time spent in doing the ramps */
- ramp_time_sec = Stepper_Calculate_Insp_Period_Ramp(ramp_max_pulse_freq_hz);
- inspiration_stepper_regular_frequency_hz = ((float)regular_steps) / (insp_time_sec - (2*ramp_time_sec));
-
- if(inspiration_stepper_regular_frequency_hz > STEPPER_MAX_FREQUENCY_HZ){
- /* Calculate the period ramp and the total time spent in doing the ramps */
- ramp_max_pulse_freq_hz = (float)STEPPER_MAX_FREQUENCY_HZ;
- ramp_time_sec = Stepper_Calculate_Insp_Period_Ramp(ramp_max_pulse_freq_hz);
- inspiration_stepper_regular_frequency_hz = ((float)regular_steps) / (insp_time_sec - (2*ramp_time_sec));
- }
-
- inspiration_stepper_pulse_period_us = (uint32_t)(1000000.0 / inspiration_stepper_regular_frequency_hz);
-
+ inspiration_stepper_pulse_period = ((float)insp_time_sec) / ((float)inspiration_stepper_pulses_setpoint);
/* Calculate stepper pulse period for expiration */
- regular_steps = inspiration_stepper_pulses_setpoint - (RAMP_STEPS + SHORT_RAMP_STEPS);
- ramp_max_pulse_freq_hz = TUNING_FACTOR * (((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 */
- /*
- 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);
- */
-}
-
+ expiration_stepper_pulse_period = ((float)exp_time_sec) / ((float)inspiration_stepper_pulses_setpoint);
-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;
-
- 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_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 < 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;
- }
-
- 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){
-
- case Stepper_Inspiration_Rising_Ramp:
-
- break;
- // -----------------------------------------------------------------------
+
case Stepper_Inspiration:
break;
// -----------------------------------------------------------------------
- case Stepper_Inspiration_Falling_Ramp:
-
- break;
- // -----------------------------------------------------------------------
+
case Stepper_Inspiration_Finish:
stepper_pulse_counter = 0;
- stepper_ramp_pulse_counter = 0;
stepper_dir = TO_HOME;
- stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
+ stepper_pulse.period(expiration_stepper_pulse_period);
stepper_pulse.write(0.5);
- stepper_driver_state = Stepper_Expiration_Rising_Ramp;
- pressure_sensor_display_update_flag = 1;
+ stepper_driver_state = Stepper_Expiration;
+ // pressure_sensor_display_update_flag = 1;
break;
- // -----------------------------------------------------------------------
- case Stepper_Expiration_Rising_Ramp:
-
- break;
- // -----------------------------------------------------------------------
+ // -----------------------------------------------------------------------
case Stepper_Expiration:
break;
- // -----------------------------------------------------------------------
- case Stepper_Expiration_Falling_Ramp:
-
- break;
- // -----------------------------------------------------------------------
+ // -----------------------------------------------------------------------
case Stepper_Expiration_Finish:
if(stepper_parameters_update_flag){
stepper_parameters_update_flag = 0;
Stepper_Update_Parameters();
- //Stepper_Calculate_Period_Ramp();
}
- stepper_pulse_counter = 0;
- stepper_ramp_pulse_counter = 0;
+ stepper_pulse_counter = 0;
stepper_dir = TO_AIRBAG;
- stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
+ stepper_pulse.period(inspiration_stepper_pulse_period);
stepper_pulse.write(0.5);
- stepper_driver_state = Stepper_Inspiration_Rising_Ramp;
+ stepper_driver_state = Stepper_Inspiration;
break;
// --------------------------------------------------------------------
@@ -340,89 +171,32 @@
// Increment the stepper pulse counter
stepper_pulse_counter++;
- // Increment the proper counter
+
switch(stepper_driver_state){
- /* Increase the speed gradually */
- case Stepper_Inspiration_Rising_Ramp:
- if(stepper_ramp_pulse_counter < RAMP_STEPS){
- stepper_pulse.period_us((int)inspiration_period_rise_ramp_us[stepper_ramp_pulse_counter]);
- stepper_pulse.write(0.5);
- stepper_ramp_pulse_counter++;
- }else{
- stepper_pulse.period_us((int)inspiration_stepper_pulse_period_us);
- stepper_pulse.write(0.5);
- stepper_ramp_pulse_counter = 0;
- stepper_driver_state = Stepper_Inspiration;
- }
- break;
- // --------------------------------------------------------------------
+
case Stepper_Inspiration:
- if(stepper_pulse_counter >= (inspiration_stepper_pulses_setpoint - RAMP_STEPS)){
- stepper_driver_state = Stepper_Inspiration_Falling_Ramp;
+ if(stepper_pulse_counter >= inspiration_stepper_pulses_setpoint){
+ stepper_driver_state = Stepper_Inspiration_Finish;
+ stepper_pulse.write(0.0);
}
break;
- // --------------------------------------------------------------------
- case Stepper_Inspiration_Falling_Ramp:
- if(stepper_ramp_pulse_counter < RAMP_STEPS){
- stepper_ramp_pulse_counter++;
- stepper_pulse.period_us((int)inspiration_period_rise_ramp_us[RAMP_STEPS - stepper_ramp_pulse_counter]);
- stepper_pulse.write(0.5);
- }else if(stepper_ramp_pulse_counter == RAMP_STEPS){
- stepper_ramp_pulse_counter++;
- stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
- stepper_pulse.write(0.5);
- }else{
- stepper_ramp_pulse_counter = 0;
- stepper_pulse.write(0.0);
- stepper_driver_state = Stepper_Inspiration_Finish;
- }
- break;
- // --------------------------------------------------------------------
+ // --------------------------------------------------------------------
case Stepper_Inspiration_Finish:
break;
- // --------------------------------------------------------------------
- case Stepper_Expiration_Rising_Ramp:
- if(stepper_ramp_pulse_counter < RAMP_STEPS){
- stepper_pulse.period_us((int)expiration_period_rise_ramp_us[stepper_ramp_pulse_counter]);
- stepper_pulse.write(0.5);
- stepper_ramp_pulse_counter++;
- }else{
- stepper_pulse.period_us((int)expiration_stepper_pulse_period_us);
- stepper_pulse.write(0.5);
- stepper_ramp_pulse_counter = 0;
- stepper_driver_state = Stepper_Expiration;
-
- }
- break;
- // --------------------------------------------------------------------
+ // --------------------------------------------------------------------
case Stepper_Expiration:
if(buttons & (1 << LIMIT_SW_01)){
- stepper_driver_state = Stepper_Expiration_Falling_Ramp;
+ stepper_driver_state = Stepper_Expiration_Finish;
+ stepper_pulse.write(0.0);
}else if(stepper_pulse_counter > STEPPER_DRIVER_MAX_PULSES){
+ stepper_driver_state = Stepper_Limit_Sensor_Error;
stepper_pulse.write(0.0);
- stepper_driver_state = Stepper_Limit_Sensor_Error;
}else{
// Does nothing
}
break;
- // --------------------------------------------------------------------
- case Stepper_Expiration_Falling_Ramp:
- if(stepper_ramp_pulse_counter < SHORT_RAMP_STEPS){
- stepper_ramp_pulse_counter++;
- stepper_pulse.period_us((int)expiration_period_short_rise_ramp_us[SHORT_RAMP_STEPS - stepper_ramp_pulse_counter]);
- stepper_pulse.write(0.5);
- }else if(stepper_ramp_pulse_counter == SHORT_RAMP_STEPS){
- stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
- stepper_pulse.write(0.5);
- stepper_ramp_pulse_counter++;
- }else{
- stepper_ramp_pulse_counter = 0;
- stepper_pulse.write(0.0);
- stepper_driver_state = Stepper_Expiration_Finish;
- }
- break;
- // --------------------------------------------------------------------
+ // --------------------------------------------------------------------
case Stepper_Expiration_Finish:
break;
// --------------------------------------------------------------------