![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
para el ventilador
Diff: stepper_motor_driver.cpp
- Revision:
- 10:b2d87404309a
- Parent:
- 9:95fdcdc0977e
- Child:
- 11:5cb7ae8bd831
diff -r 95fdcdc0977e -r b2d87404309a stepper_motor_driver.cpp --- 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; // --------------------------------------------------------------------