![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
para el ventilador
stepper_motor_driver.cpp@9:95fdcdc0977e, 2020-04-26 (annotated)
- Committer:
- miguelangel_2511
- Date:
- Sun Apr 26 08:50:12 2020 +0000
- Revision:
- 9:95fdcdc0977e
- Parent:
- 6:d38287621cca
- Child:
- 10:b2d87404309a
Prueba del ventilador con las graficas de volumen, flujo y presion. Aun no se ha tomado en cuenta la influencia del tiempo de impresion sobre la frecuencia de muestreo eficaz.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
miguelangel_2511 | 3:45299e7882b9 | 1 | |
miguelangel_2511 | 5:bd5fc0510e7b | 2 | //#include "math.h" |
miguelangel_2511 | 0:9d0b9785d3d6 | 3 | #include "mbed.h" |
miguelangel_2511 | 0:9d0b9785d3d6 | 4 | #include "project_defines.h" |
miguelangel_2511 | 0:9d0b9785d3d6 | 5 | #include "stepper_motor_driver.h" |
miguelangel_2511 | 0:9d0b9785d3d6 | 6 | #include "buttons.h" |
miguelangel_2511 | 6:d38287621cca | 7 | #include "nextion_interface.h" |
miguelangel_2511 | 1:aa5df1878126 | 8 | #include "FastPWM.h" |
miguelangel_2511 | 3:45299e7882b9 | 9 | #include "ventilator.h" |
miguelangel_2511 | 0:9d0b9785d3d6 | 10 | |
miguelangel_2511 | 1:aa5df1878126 | 11 | /* Object definition */ |
miguelangel_2511 | 1:aa5df1878126 | 12 | InterruptIn stepper_pulse_feedback(STEPPER_PULSE_FEEDBACK_PIN); |
miguelangel_2511 | 1:aa5df1878126 | 13 | FastPWM stepper_pulse(STEPPER_PULSE_PIN, -1); |
miguelangel_2511 | 0:9d0b9785d3d6 | 14 | DigitalOut stepper_en(STEPPER_ENABLE_PIN); |
miguelangel_2511 | 0:9d0b9785d3d6 | 15 | DigitalOut stepper_dir(STEPPER_DIRECTION_PIN); |
miguelangel_2511 | 0:9d0b9785d3d6 | 16 | |
miguelangel_2511 | 3:45299e7882b9 | 17 | |
miguelangel_2511 | 1:aa5df1878126 | 18 | /* Variable definition */ |
miguelangel_2511 | 0:9d0b9785d3d6 | 19 | |
miguelangel_2511 | 3:45299e7882b9 | 20 | volatile uint8_t stepper_parameters_update_flag = 0; |
miguelangel_2511 | 1:aa5df1878126 | 21 | |
miguelangel_2511 | 1:aa5df1878126 | 22 | // ----------------------- Tables used for calculations ------------------------ |
miguelangel_2511 | 1:aa5df1878126 | 23 | |
miguelangel_2511 | 1:aa5df1878126 | 24 | #if RAMP_STEPS == 50 |
miguelangel_2511 | 1:aa5df1878126 | 25 | |
miguelangel_2511 | 1:aa5df1878126 | 26 | const float sigma_ramp[RAMP_STEPS] = { |
miguelangel_2511 | 1:aa5df1878126 | 27 | 0.002, 0.003, 0.004, 0.005, 0.006, 0.008, 0.010, 0.013, 0.017, 0.021, |
miguelangel_2511 | 1:aa5df1878126 | 28 | 0.027, 0.034, 0.042, 0.053, 0.067, 0.083, 0.103, 0.128, 0.157, 0.192, |
miguelangel_2511 | 1:aa5df1878126 | 29 | 0.231, 0.277, 0.327, 0.382, 0.440, 0.500, 0.560, 0.618, 0.673, 0.723, |
miguelangel_2511 | 1:aa5df1878126 | 30 | 0.769, 0.808, 0.843, 0.872, 0.897, 0.917, 0.933, 0.947, 0.958, 0.966, |
miguelangel_2511 | 1:aa5df1878126 | 31 | 0.973, 0.979, 0.983, 0.987, 0.990, 0.992, 0.994, 0.995, 0.996, 0.997 |
miguelangel_2511 | 1:aa5df1878126 | 32 | }; |
miguelangel_2511 | 1:aa5df1878126 | 33 | |
miguelangel_2511 | 1:aa5df1878126 | 34 | #elif RAMP_STEPS == 100 |
miguelangel_2511 | 1:aa5df1878126 | 35 | |
miguelangel_2511 | 1:aa5df1878126 | 36 | const float sigma_ramp[RAMP_STEPS] = { |
miguelangel_2511 | 1:aa5df1878126 | 37 | 0.002, 0.003, 0.003, 0.004, 0.004, 0.005, 0.005, 0.006, 0.006, 0.007, |
miguelangel_2511 | 1:aa5df1878126 | 38 | 0.008, 0.009, 0.011, 0.012, 0.013, 0.015, 0.017, 0.019, 0.021, 0.024, |
miguelangel_2511 | 1:aa5df1878126 | 39 | 0.027, 0.031, 0.034, 0.039, 0.043, 0.049, 0.055, 0.061, 0.069, 0.077, |
miguelangel_2511 | 1:aa5df1878126 | 40 | 0.086, 0.096, 0.107, 0.119, 0.133, 0.147, 0.163, 0.180, 0.199, 0.219, |
miguelangel_2511 | 1:aa5df1878126 | 41 | 0.240, 0.263, 0.287, 0.313, 0.339, 0.367, 0.396, 0.425, 0.455, 0.485, |
miguelangel_2511 | 1:aa5df1878126 | 42 | 0.515, 0.545, 0.575, 0.604, 0.633, 0.661, 0.687, 0.713, 0.737, 0.760, |
miguelangel_2511 | 1:aa5df1878126 | 43 | 0.781, 0.801, 0.820, 0.837, 0.853, 0.867, 0.881, 0.893, 0.904, 0.914, |
miguelangel_2511 | 1:aa5df1878126 | 44 | 0.923, 0.931, 0.939, 0.945, 0.951, 0.957, 0.961, 0.966, 0.969, 0.973, |
miguelangel_2511 | 1:aa5df1878126 | 45 | 0.976, 0.979, 0.981, 0.983, 0.985, 0.987, 0.988, 0.989, 0.991, 0.992, |
miguelangel_2511 | 3:45299e7882b9 | 46 | 0.993, 0.994, 0.994, 0.995, 0.995, 0.996, 0.996, 0.997, 0.997, 0.998 |
miguelangel_2511 | 1:aa5df1878126 | 47 | }; |
miguelangel_2511 | 1:aa5df1878126 | 48 | |
miguelangel_2511 | 1:aa5df1878126 | 49 | #else |
miguelangel_2511 | 1:aa5df1878126 | 50 | #error "Seleccionar un valor permitido para RAMP_STEPS: Puede ser 50 o 100" |
miguelangel_2511 | 1:aa5df1878126 | 51 | #endif |
miguelangel_2511 | 3:45299e7882b9 | 52 | |
miguelangel_2511 | 3:45299e7882b9 | 53 | const float short_sigma_ramp[SHORT_RAMP_STEPS] = { |
miguelangel_2511 | 3:45299e7882b9 | 54 | 0.002, 0.004, 0.007, 0.011, 0.018, 0.029, 0.047, 0.076, 0.119, 0.182, |
miguelangel_2511 | 3:45299e7882b9 | 55 | 0.269, 0.378, 0.500, 0.622, 0.731, 0.818, 0.881, 0.924, 0.953, 0.971, |
miguelangel_2511 | 3:45299e7882b9 | 56 | 0.982, 0.989, 0.993, 0.996, 0.998 |
miguelangel_2511 | 3:45299e7882b9 | 57 | }; |
miguelangel_2511 | 3:45299e7882b9 | 58 | |
miguelangel_2511 | 3:45299e7882b9 | 59 | |
miguelangel_2511 | 1:aa5df1878126 | 60 | |
miguelangel_2511 | 1:aa5df1878126 | 61 | // ----------------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 62 | |
miguelangel_2511 | 1:aa5df1878126 | 63 | uint32_t calibration_period_rise_ramp_us[RAMP_STEPS]; |
miguelangel_2511 | 1:aa5df1878126 | 64 | uint32_t inspiration_period_rise_ramp_us[RAMP_STEPS]; |
miguelangel_2511 | 1:aa5df1878126 | 65 | uint32_t expiration_period_rise_ramp_us[RAMP_STEPS]; |
miguelangel_2511 | 3:45299e7882b9 | 66 | uint32_t expiration_period_short_rise_ramp_us[SHORT_RAMP_STEPS]; |
miguelangel_2511 | 1:aa5df1878126 | 67 | |
miguelangel_2511 | 1:aa5df1878126 | 68 | // These parameters are configurer through the graphic interface */ |
miguelangel_2511 | 1:aa5df1878126 | 69 | uint32_t inspiration_stepper_pulses_setpoint = STEPPER_DRIVER_INSPIRATION_PULSES_DEFAULT ; |
miguelangel_2511 | 1:aa5df1878126 | 70 | //uint32_t expiration_stepper_pulses_setpoint = STEPPER_DRIVER_MAX_PULSES ; // Not used in this version |
miguelangel_2511 | 1:aa5df1878126 | 71 | uint32_t inspiration_stepper_pulse_period_us = STEPPER_DRIVER_DEFAULT_PULSE_PERIOD_US; |
miguelangel_2511 | 1:aa5df1878126 | 72 | uint32_t expiration_stepper_pulse_period_us = STEPPER_DRIVER_DEFAULT_PULSE_PERIOD_US; |
miguelangel_2511 | 1:aa5df1878126 | 73 | uint32_t calibration_stepper_pulse_period_us = STEPPER_DRIVER_DEFAULT_PULSE_PERIOD_US; |
miguelangel_2511 | 1:aa5df1878126 | 74 | |
miguelangel_2511 | 5:bd5fc0510e7b | 75 | |
miguelangel_2511 | 1:aa5df1878126 | 76 | // Volume setpoint (250mL, 300mL, 350ml ... 750ml) expressed in steps |
miguelangel_2511 | 1:aa5df1878126 | 77 | uint32_t volume_in_steps[VOLUME_SETPOINT_INDEX_LIMIT] = { |
miguelangel_2511 | 3:45299e7882b9 | 78 | 720, // 1600, //250 mL |
miguelangel_2511 | 3:45299e7882b9 | 79 | 765, // 1700, //300 mL |
miguelangel_2511 | 3:45299e7882b9 | 80 | 810, // 1800, //350 mL |
miguelangel_2511 | 3:45299e7882b9 | 81 | 855, // 1900, //400 mL |
miguelangel_2511 | 3:45299e7882b9 | 82 | 900, // 2000, //450 mL |
miguelangel_2511 | 3:45299e7882b9 | 83 | 945, // 2100, //500 mL |
miguelangel_2511 | 3:45299e7882b9 | 84 | 990, // 2200, //550 mL |
miguelangel_2511 | 3:45299e7882b9 | 85 | 1035, // 2300, //600 mL |
miguelangel_2511 | 3:45299e7882b9 | 86 | 1080, // 2400, //650 mL |
miguelangel_2511 | 3:45299e7882b9 | 87 | 1125, // 2500, //700 mL |
miguelangel_2511 | 3:45299e7882b9 | 88 | 1170 // 2600 //750 mL |
miguelangel_2511 | 1:aa5df1878126 | 89 | }; |
miguelangel_2511 | 1:aa5df1878126 | 90 | |
miguelangel_2511 | 1:aa5df1878126 | 91 | |
miguelangel_2511 | 1:aa5df1878126 | 92 | // Variables used in every state of the stepper motor driver */ |
miguelangel_2511 | 1:aa5df1878126 | 93 | volatile uint32_t stepper_pulse_counter = 0; |
miguelangel_2511 | 1:aa5df1878126 | 94 | volatile uint32_t stepper_ramp_pulse_counter = 0; |
miguelangel_2511 | 1:aa5df1878126 | 95 | |
miguelangel_2511 | 1:aa5df1878126 | 96 | // State of the stepper motor |
miguelangel_2511 | 1:aa5df1878126 | 97 | volatile Stepper_Driver_State_t stepper_driver_state = Stepper_Stand_By; |
miguelangel_2511 | 1:aa5df1878126 | 98 | |
miguelangel_2511 | 1:aa5df1878126 | 99 | /* Function definition */ |
miguelangel_2511 | 1:aa5df1878126 | 100 | |
miguelangel_2511 | 1:aa5df1878126 | 101 | /* Initialize the control signals for the stepper */ |
miguelangel_2511 | 1:aa5df1878126 | 102 | void Stepper_Driver_Init(void){ |
miguelangel_2511 | 1:aa5df1878126 | 103 | stepper_en = STEPPER_ENABLED; |
miguelangel_2511 | 1:aa5df1878126 | 104 | stepper_dir = TO_HOME; |
miguelangel_2511 | 1:aa5df1878126 | 105 | stepper_pulse.period_us((int)calibration_stepper_pulse_period_us); |
miguelangel_2511 | 1:aa5df1878126 | 106 | stepper_pulse.write(0.0f); |
miguelangel_2511 | 1:aa5df1878126 | 107 | stepper_pulse_feedback.fall(&Stepper_Driver_Pulse_Signal_Update); |
miguelangel_2511 | 3:45299e7882b9 | 108 | stepper_driver_state = Stepper_Inspiration_Finish; |
miguelangel_2511 | 3:45299e7882b9 | 109 | stepper_parameters_update_flag = 1; |
miguelangel_2511 | 1:aa5df1878126 | 110 | // Just for testing purposes |
miguelangel_2511 | 1:aa5df1878126 | 111 | //inspiration_stepper_pulses_setpoint = volume_in_steps[10]; |
miguelangel_2511 | 1:aa5df1878126 | 112 | } |
miguelangel_2511 | 1:aa5df1878126 | 113 | |
miguelangel_2511 | 0:9d0b9785d3d6 | 114 | |
miguelangel_2511 | 0:9d0b9785d3d6 | 115 | |
miguelangel_2511 | 0:9d0b9785d3d6 | 116 | |
miguelangel_2511 | 1:aa5df1878126 | 117 | void Stepper_Go_Home_Position(void){ |
miguelangel_2511 | 1:aa5df1878126 | 118 | // stepper_driver_state = Stepper_Driver_Home_Ramp; |
miguelangel_2511 | 1:aa5df1878126 | 119 | |
miguelangel_2511 | 1:aa5df1878126 | 120 | //while(stepper_driver_state != Stepper_Driver_Stand_By){ |
miguelangel_2511 | 1:aa5df1878126 | 121 | // Stepper_Driver_State_Machine(); |
miguelangel_2511 | 1:aa5df1878126 | 122 | //} |
miguelangel_2511 | 1:aa5df1878126 | 123 | } |
miguelangel_2511 | 1:aa5df1878126 | 124 | |
miguelangel_2511 | 1:aa5df1878126 | 125 | |
miguelangel_2511 | 3:45299e7882b9 | 126 | void Stepper_Update_Parameters(void){ |
miguelangel_2511 | 3:45299e7882b9 | 127 | |
miguelangel_2511 | 3:45299e7882b9 | 128 | int8_t stepper_volume_index; |
miguelangel_2511 | 4:d42e1953243c | 129 | uint32_t regular_steps; |
miguelangel_2511 | 3:45299e7882b9 | 130 | float total_time, total_time_sec, insp_time_sec, exp_time_sec; |
miguelangel_2511 | 4:d42e1953243c | 131 | float inspiration_stepper_pulse_period_sec, expiration_stepper_pulse_period_sec; |
miguelangel_2511 | 5:bd5fc0510e7b | 132 | float ramp_max_pulse_freq_hz, ramp_time_sec, short_ramp_time_sec; |
miguelangel_2511 | 6:d38287621cca | 133 | float inspiration_stepper_regular_frequency_hz; |
miguelangel_2511 | 3:45299e7882b9 | 134 | |
miguelangel_2511 | 3:45299e7882b9 | 135 | /* Update the value of the volume setpoint (expressed in steps) */ |
miguelangel_2511 | 3:45299e7882b9 | 136 | stepper_volume_index = (volume_setpoint - VOLUME_SETPOINT_MINIMUM_VALUE) / VOLUME_SETPOINT_STEP; |
miguelangel_2511 | 3:45299e7882b9 | 137 | inspiration_stepper_pulses_setpoint = volume_in_steps[stepper_volume_index]; |
miguelangel_2511 | 3:45299e7882b9 | 138 | |
miguelangel_2511 | 3:45299e7882b9 | 139 | /* Update the value of the pulse periods in microseconds */ |
miguelangel_2511 | 3:45299e7882b9 | 140 | total_time = inspiration_time + expiration_time; |
miguelangel_2511 | 4:d42e1953243c | 141 | total_time_sec = 60.0 / resp_frequency; |
miguelangel_2511 | 3:45299e7882b9 | 142 | insp_time_sec = (total_time_sec * inspiration_time) / total_time; |
miguelangel_2511 | 3:45299e7882b9 | 143 | exp_time_sec = (total_time_sec * expiration_time) / total_time; |
miguelangel_2511 | 4:d42e1953243c | 144 | |
miguelangel_2511 | 4:d42e1953243c | 145 | /* Calculate stepper pulse period for inspiration */ |
miguelangel_2511 | 4:d42e1953243c | 146 | regular_steps = inspiration_stepper_pulses_setpoint - (2 * RAMP_STEPS); |
miguelangel_2511 | 6:d38287621cca | 147 | /* Asume an initial value for the max frrquency of the ramp */ |
miguelangel_2511 | 6:d38287621cca | 148 | ramp_max_pulse_freq_hz = TUNING_FACTOR * (((float)inspiration_stepper_pulses_setpoint) / insp_time_sec); |
miguelangel_2511 | 6:d38287621cca | 149 | /* Calculate the period ramp and the total time spent in doing the ramps */ |
miguelangel_2511 | 5:bd5fc0510e7b | 150 | ramp_time_sec = Stepper_Calculate_Insp_Period_Ramp(ramp_max_pulse_freq_hz); |
miguelangel_2511 | 6:d38287621cca | 151 | inspiration_stepper_regular_frequency_hz = ((float)regular_steps) / (insp_time_sec - (2*ramp_time_sec)); |
miguelangel_2511 | 6:d38287621cca | 152 | |
miguelangel_2511 | 6:d38287621cca | 153 | if(inspiration_stepper_regular_frequency_hz > STEPPER_MAX_FREQUENCY_HZ){ |
miguelangel_2511 | 6:d38287621cca | 154 | /* Calculate the period ramp and the total time spent in doing the ramps */ |
miguelangel_2511 | 6:d38287621cca | 155 | ramp_max_pulse_freq_hz = (float)STEPPER_MAX_FREQUENCY_HZ; |
miguelangel_2511 | 6:d38287621cca | 156 | ramp_time_sec = Stepper_Calculate_Insp_Period_Ramp(ramp_max_pulse_freq_hz); |
miguelangel_2511 | 6:d38287621cca | 157 | inspiration_stepper_regular_frequency_hz = ((float)regular_steps) / (insp_time_sec - (2*ramp_time_sec)); |
miguelangel_2511 | 6:d38287621cca | 158 | } |
miguelangel_2511 | 6:d38287621cca | 159 | |
miguelangel_2511 | 6:d38287621cca | 160 | inspiration_stepper_pulse_period_us = (uint32_t)(1000000.0 / inspiration_stepper_regular_frequency_hz); |
miguelangel_2511 | 5:bd5fc0510e7b | 161 | |
miguelangel_2511 | 4:d42e1953243c | 162 | /* Calculate stepper pulse period for expiration */ |
miguelangel_2511 | 4:d42e1953243c | 163 | regular_steps = inspiration_stepper_pulses_setpoint - (RAMP_STEPS + SHORT_RAMP_STEPS); |
miguelangel_2511 | 6:d38287621cca | 164 | ramp_max_pulse_freq_hz = TUNING_FACTOR * (((float)inspiration_stepper_pulses_setpoint) / exp_time_sec); |
miguelangel_2511 | 5:bd5fc0510e7b | 165 | ramp_time_sec = Stepper_Calculate_Exp_Period_Ramp(ramp_max_pulse_freq_hz); |
miguelangel_2511 | 5:bd5fc0510e7b | 166 | short_ramp_time_sec = Stepper_Calculate_Exp_Short_Period_Ramp(ramp_max_pulse_freq_hz); |
miguelangel_2511 | 5:bd5fc0510e7b | 167 | expiration_stepper_pulse_period_sec = (exp_time_sec - (ramp_time_sec + short_ramp_time_sec)) / (float)regular_steps; |
miguelangel_2511 | 6:d38287621cca | 168 | |
miguelangel_2511 | 4:d42e1953243c | 169 | expiration_stepper_pulse_period_us = (uint32_t)(1000000.0 * expiration_stepper_pulse_period_sec); |
miguelangel_2511 | 3:45299e7882b9 | 170 | |
miguelangel_2511 | 3:45299e7882b9 | 171 | /* Just for testing purposes */ |
miguelangel_2511 | 6:d38287621cca | 172 | /* |
miguelangel_2511 | 3:45299e7882b9 | 173 | pc.printf("Cantidad de pulsos: %d \n\n\r", inspiration_stepper_pulses_setpoint); |
miguelangel_2511 | 4:d42e1953243c | 174 | pc.printf("Frecuencia de arranque: %d Hz\n\r", STEPPER_START_BRAKE_FREQUENCY_HZ); |
miguelangel_2511 | 4:d42e1953243c | 175 | pc.printf("Frecuencia de pulsos de ida: %f Hz\n\r", 1.0 / inspiration_stepper_pulse_period_sec); |
miguelangel_2511 | 4:d42e1953243c | 176 | pc.printf("Periodo de pulsos de ida en us: %d us\n\r", inspiration_stepper_pulse_period_us); |
miguelangel_2511 | 4:d42e1953243c | 177 | pc.printf("Frecuencia de pulsos de vuelta: %f Hz\n\r", 1.0 / expiration_stepper_pulse_period_us); |
miguelangel_2511 | 4:d42e1953243c | 178 | pc.printf("Periodo de pulsos de vuelta en us: %d us\n\r", expiration_stepper_pulse_period_us); |
miguelangel_2511 | 6:d38287621cca | 179 | */ |
miguelangel_2511 | 3:45299e7882b9 | 180 | } |
miguelangel_2511 | 3:45299e7882b9 | 181 | |
miguelangel_2511 | 1:aa5df1878126 | 182 | |
miguelangel_2511 | 5:bd5fc0510e7b | 183 | float Stepper_Calculate_Insp_Period_Ramp(float max_freq){ |
miguelangel_2511 | 5:bd5fc0510e7b | 184 | |
miguelangel_2511 | 5:bd5fc0510e7b | 185 | float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz; |
miguelangel_2511 | 5:bd5fc0510e7b | 186 | float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec; |
miguelangel_2511 | 5:bd5fc0510e7b | 187 | uint8_t i; |
miguelangel_2511 | 5:bd5fc0510e7b | 188 | |
miguelangel_2511 | 5:bd5fc0510e7b | 189 | /* Period rise ramp for inspiration */ |
miguelangel_2511 | 5:bd5fc0510e7b | 190 | initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ; |
miguelangel_2511 | 5:bd5fc0510e7b | 191 | final_frequency_hz = max_freq; |
miguelangel_2511 | 5:bd5fc0510e7b | 192 | frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz; |
miguelangel_2511 | 5:bd5fc0510e7b | 193 | |
miguelangel_2511 | 5:bd5fc0510e7b | 194 | ramp_time_sec = 0; |
miguelangel_2511 | 1:aa5df1878126 | 195 | |
miguelangel_2511 | 5:bd5fc0510e7b | 196 | for(i = 0; i < RAMP_STEPS; i++){ |
miguelangel_2511 | 5:bd5fc0510e7b | 197 | frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * sigma_ramp[i]); |
miguelangel_2511 | 5:bd5fc0510e7b | 198 | period_i_sec = 1.0 / frequency_i_hz; |
miguelangel_2511 | 5:bd5fc0510e7b | 199 | period_i_us = 1000000.0 * period_i_sec; |
miguelangel_2511 | 5:bd5fc0510e7b | 200 | inspiration_period_rise_ramp_us[i] = (uint32_t)period_i_us; |
miguelangel_2511 | 5:bd5fc0510e7b | 201 | ramp_time_sec += period_i_sec; |
miguelangel_2511 | 5:bd5fc0510e7b | 202 | } |
miguelangel_2511 | 5:bd5fc0510e7b | 203 | |
miguelangel_2511 | 5:bd5fc0510e7b | 204 | return ramp_time_sec; |
miguelangel_2511 | 5:bd5fc0510e7b | 205 | } |
miguelangel_2511 | 5:bd5fc0510e7b | 206 | |
miguelangel_2511 | 5:bd5fc0510e7b | 207 | |
miguelangel_2511 | 5:bd5fc0510e7b | 208 | float Stepper_Calculate_Exp_Period_Ramp(float max_freq){ |
miguelangel_2511 | 5:bd5fc0510e7b | 209 | |
miguelangel_2511 | 5:bd5fc0510e7b | 210 | float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz; |
miguelangel_2511 | 5:bd5fc0510e7b | 211 | float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec; |
miguelangel_2511 | 1:aa5df1878126 | 212 | uint8_t i; |
miguelangel_2511 | 1:aa5df1878126 | 213 | |
miguelangel_2511 | 1:aa5df1878126 | 214 | /* Period rise ramp for inspiration */ |
miguelangel_2511 | 5:bd5fc0510e7b | 215 | initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ; |
miguelangel_2511 | 5:bd5fc0510e7b | 216 | final_frequency_hz = max_freq; |
miguelangel_2511 | 5:bd5fc0510e7b | 217 | frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz; |
miguelangel_2511 | 1:aa5df1878126 | 218 | |
miguelangel_2511 | 5:bd5fc0510e7b | 219 | ramp_time_sec = 0; |
miguelangel_2511 | 1:aa5df1878126 | 220 | |
miguelangel_2511 | 5:bd5fc0510e7b | 221 | for(i = 0; i < RAMP_STEPS; i++){ |
miguelangel_2511 | 5:bd5fc0510e7b | 222 | frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * sigma_ramp[i]); |
miguelangel_2511 | 5:bd5fc0510e7b | 223 | period_i_sec = 1.0 / frequency_i_hz; |
miguelangel_2511 | 5:bd5fc0510e7b | 224 | period_i_us = 1000000.0 * period_i_sec; |
miguelangel_2511 | 1:aa5df1878126 | 225 | expiration_period_rise_ramp_us[i] = (uint32_t)period_i_us; |
miguelangel_2511 | 5:bd5fc0510e7b | 226 | ramp_time_sec += period_i_sec; |
miguelangel_2511 | 5:bd5fc0510e7b | 227 | } |
miguelangel_2511 | 1:aa5df1878126 | 228 | |
miguelangel_2511 | 5:bd5fc0510e7b | 229 | return ramp_time_sec; |
miguelangel_2511 | 5:bd5fc0510e7b | 230 | } |
miguelangel_2511 | 5:bd5fc0510e7b | 231 | |
miguelangel_2511 | 3:45299e7882b9 | 232 | |
miguelangel_2511 | 3:45299e7882b9 | 233 | |
miguelangel_2511 | 5:bd5fc0510e7b | 234 | float Stepper_Calculate_Exp_Short_Period_Ramp(float max_freq){ |
miguelangel_2511 | 5:bd5fc0510e7b | 235 | |
miguelangel_2511 | 5:bd5fc0510e7b | 236 | float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz; |
miguelangel_2511 | 5:bd5fc0510e7b | 237 | float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec; |
miguelangel_2511 | 5:bd5fc0510e7b | 238 | uint8_t i; |
miguelangel_2511 | 5:bd5fc0510e7b | 239 | |
miguelangel_2511 | 5:bd5fc0510e7b | 240 | /* Period rise ramp for inspiration */ |
miguelangel_2511 | 5:bd5fc0510e7b | 241 | initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ; |
miguelangel_2511 | 5:bd5fc0510e7b | 242 | final_frequency_hz = max_freq; |
miguelangel_2511 | 5:bd5fc0510e7b | 243 | frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz; |
miguelangel_2511 | 5:bd5fc0510e7b | 244 | |
miguelangel_2511 | 5:bd5fc0510e7b | 245 | ramp_time_sec = 0; |
miguelangel_2511 | 5:bd5fc0510e7b | 246 | |
miguelangel_2511 | 5:bd5fc0510e7b | 247 | for(i = 0; i < SHORT_RAMP_STEPS; i++){ |
miguelangel_2511 | 5:bd5fc0510e7b | 248 | frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * short_sigma_ramp[i]); |
miguelangel_2511 | 5:bd5fc0510e7b | 249 | period_i_sec = 1.0 / frequency_i_hz; |
miguelangel_2511 | 5:bd5fc0510e7b | 250 | period_i_us = 1000000.0 * period_i_sec; |
miguelangel_2511 | 5:bd5fc0510e7b | 251 | expiration_period_short_rise_ramp_us[i] = (uint32_t)period_i_us; |
miguelangel_2511 | 5:bd5fc0510e7b | 252 | ramp_time_sec += period_i_sec; |
miguelangel_2511 | 5:bd5fc0510e7b | 253 | } |
miguelangel_2511 | 5:bd5fc0510e7b | 254 | |
miguelangel_2511 | 5:bd5fc0510e7b | 255 | return ramp_time_sec; |
miguelangel_2511 | 1:aa5df1878126 | 256 | } |
miguelangel_2511 | 1:aa5df1878126 | 257 | |
miguelangel_2511 | 1:aa5df1878126 | 258 | |
miguelangel_2511 | 5:bd5fc0510e7b | 259 | |
miguelangel_2511 | 5:bd5fc0510e7b | 260 | |
miguelangel_2511 | 5:bd5fc0510e7b | 261 | |
miguelangel_2511 | 1:aa5df1878126 | 262 | void Stepper_Driver_State_Machine(void){ |
miguelangel_2511 | 0:9d0b9785d3d6 | 263 | |
miguelangel_2511 | 1:aa5df1878126 | 264 | switch(stepper_driver_state){ |
miguelangel_2511 | 1:aa5df1878126 | 265 | |
miguelangel_2511 | 1:aa5df1878126 | 266 | case Stepper_Inspiration_Rising_Ramp: |
miguelangel_2511 | 1:aa5df1878126 | 267 | |
miguelangel_2511 | 1:aa5df1878126 | 268 | break; |
miguelangel_2511 | 1:aa5df1878126 | 269 | // ----------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 270 | case Stepper_Inspiration: |
miguelangel_2511 | 1:aa5df1878126 | 271 | |
miguelangel_2511 | 1:aa5df1878126 | 272 | break; |
miguelangel_2511 | 1:aa5df1878126 | 273 | // ----------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 274 | case Stepper_Inspiration_Falling_Ramp: |
miguelangel_2511 | 1:aa5df1878126 | 275 | |
miguelangel_2511 | 1:aa5df1878126 | 276 | break; |
miguelangel_2511 | 1:aa5df1878126 | 277 | // ----------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 278 | case Stepper_Inspiration_Finish: |
miguelangel_2511 | 1:aa5df1878126 | 279 | |
miguelangel_2511 | 1:aa5df1878126 | 280 | stepper_pulse_counter = 0; |
miguelangel_2511 | 1:aa5df1878126 | 281 | stepper_ramp_pulse_counter = 0; |
miguelangel_2511 | 1:aa5df1878126 | 282 | stepper_dir = TO_HOME; |
miguelangel_2511 | 1:aa5df1878126 | 283 | stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US); |
miguelangel_2511 | 1:aa5df1878126 | 284 | stepper_pulse.write(0.5); |
miguelangel_2511 | 1:aa5df1878126 | 285 | stepper_driver_state = Stepper_Expiration_Rising_Ramp; |
miguelangel_2511 | 9:95fdcdc0977e | 286 | pressure_sensor_display_update_flag = 1; |
miguelangel_2511 | 1:aa5df1878126 | 287 | |
miguelangel_2511 | 1:aa5df1878126 | 288 | break; |
miguelangel_2511 | 1:aa5df1878126 | 289 | // ----------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 290 | case Stepper_Expiration_Rising_Ramp: |
miguelangel_2511 | 1:aa5df1878126 | 291 | |
miguelangel_2511 | 1:aa5df1878126 | 292 | break; |
miguelangel_2511 | 1:aa5df1878126 | 293 | // ----------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 294 | case Stepper_Expiration: |
miguelangel_2511 | 1:aa5df1878126 | 295 | |
miguelangel_2511 | 1:aa5df1878126 | 296 | break; |
miguelangel_2511 | 1:aa5df1878126 | 297 | // ----------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 298 | case Stepper_Expiration_Falling_Ramp: |
miguelangel_2511 | 1:aa5df1878126 | 299 | |
miguelangel_2511 | 1:aa5df1878126 | 300 | break; |
miguelangel_2511 | 1:aa5df1878126 | 301 | // ----------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 302 | case Stepper_Expiration_Finish: |
miguelangel_2511 | 3:45299e7882b9 | 303 | |
miguelangel_2511 | 3:45299e7882b9 | 304 | if(stepper_parameters_update_flag){ |
miguelangel_2511 | 3:45299e7882b9 | 305 | stepper_parameters_update_flag = 0; |
miguelangel_2511 | 3:45299e7882b9 | 306 | Stepper_Update_Parameters(); |
miguelangel_2511 | 5:bd5fc0510e7b | 307 | //Stepper_Calculate_Period_Ramp(); |
miguelangel_2511 | 3:45299e7882b9 | 308 | } |
miguelangel_2511 | 1:aa5df1878126 | 309 | |
miguelangel_2511 | 1:aa5df1878126 | 310 | stepper_pulse_counter = 0; |
miguelangel_2511 | 1:aa5df1878126 | 311 | stepper_ramp_pulse_counter = 0; |
miguelangel_2511 | 1:aa5df1878126 | 312 | stepper_dir = TO_AIRBAG; |
miguelangel_2511 | 1:aa5df1878126 | 313 | stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US); |
miguelangel_2511 | 1:aa5df1878126 | 314 | stepper_pulse.write(0.5); |
miguelangel_2511 | 1:aa5df1878126 | 315 | stepper_driver_state = Stepper_Inspiration_Rising_Ramp; |
miguelangel_2511 | 1:aa5df1878126 | 316 | |
miguelangel_2511 | 3:45299e7882b9 | 317 | break; |
miguelangel_2511 | 1:aa5df1878126 | 318 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 319 | case Stepper_Limit_Sensor_Error: |
miguelangel_2511 | 1:aa5df1878126 | 320 | // Stop the system |
miguelangel_2511 | 1:aa5df1878126 | 321 | stepper_en = STEPPER_DISABLED; |
miguelangel_2511 | 6:d38287621cca | 322 | // tft->locate(340,VALUES_ROW2_Y_POS); |
miguelangel_2511 | 6:d38287621cca | 323 | // tft->printf("error"); |
miguelangel_2511 | 1:aa5df1878126 | 324 | stepper_driver_state = Stepper_Stand_By; |
miguelangel_2511 | 1:aa5df1878126 | 325 | break; |
miguelangel_2511 | 1:aa5df1878126 | 326 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 327 | case Stepper_Stand_By: |
miguelangel_2511 | 1:aa5df1878126 | 328 | |
miguelangel_2511 | 1:aa5df1878126 | 329 | break; |
miguelangel_2511 | 1:aa5df1878126 | 330 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 331 | default: |
miguelangel_2511 | 1:aa5df1878126 | 332 | |
miguelangel_2511 | 1:aa5df1878126 | 333 | break; |
miguelangel_2511 | 0:9d0b9785d3d6 | 334 | } |
miguelangel_2511 | 0:9d0b9785d3d6 | 335 | } |
miguelangel_2511 | 0:9d0b9785d3d6 | 336 | |
miguelangel_2511 | 0:9d0b9785d3d6 | 337 | |
miguelangel_2511 | 1:aa5df1878126 | 338 | void Stepper_Driver_Pulse_Signal_Update(void){ |
miguelangel_2511 | 1:aa5df1878126 | 339 | |
miguelangel_2511 | 1:aa5df1878126 | 340 | // Increment the stepper pulse counter |
miguelangel_2511 | 1:aa5df1878126 | 341 | stepper_pulse_counter++; |
miguelangel_2511 | 1:aa5df1878126 | 342 | |
miguelangel_2511 | 1:aa5df1878126 | 343 | // Increment the proper counter |
miguelangel_2511 | 1:aa5df1878126 | 344 | switch(stepper_driver_state){ |
miguelangel_2511 | 1:aa5df1878126 | 345 | /* Increase the speed gradually */ |
miguelangel_2511 | 1:aa5df1878126 | 346 | case Stepper_Inspiration_Rising_Ramp: |
miguelangel_2511 | 1:aa5df1878126 | 347 | if(stepper_ramp_pulse_counter < RAMP_STEPS){ |
miguelangel_2511 | 1:aa5df1878126 | 348 | stepper_pulse.period_us((int)inspiration_period_rise_ramp_us[stepper_ramp_pulse_counter]); |
miguelangel_2511 | 1:aa5df1878126 | 349 | stepper_pulse.write(0.5); |
miguelangel_2511 | 1:aa5df1878126 | 350 | stepper_ramp_pulse_counter++; |
miguelangel_2511 | 1:aa5df1878126 | 351 | }else{ |
miguelangel_2511 | 1:aa5df1878126 | 352 | stepper_pulse.period_us((int)inspiration_stepper_pulse_period_us); |
miguelangel_2511 | 1:aa5df1878126 | 353 | stepper_pulse.write(0.5); |
miguelangel_2511 | 1:aa5df1878126 | 354 | stepper_ramp_pulse_counter = 0; |
miguelangel_2511 | 1:aa5df1878126 | 355 | stepper_driver_state = Stepper_Inspiration; |
miguelangel_2511 | 1:aa5df1878126 | 356 | } |
miguelangel_2511 | 1:aa5df1878126 | 357 | break; |
miguelangel_2511 | 1:aa5df1878126 | 358 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 359 | case Stepper_Inspiration: |
miguelangel_2511 | 1:aa5df1878126 | 360 | if(stepper_pulse_counter >= (inspiration_stepper_pulses_setpoint - RAMP_STEPS)){ |
miguelangel_2511 | 1:aa5df1878126 | 361 | stepper_driver_state = Stepper_Inspiration_Falling_Ramp; |
miguelangel_2511 | 1:aa5df1878126 | 362 | } |
miguelangel_2511 | 1:aa5df1878126 | 363 | break; |
miguelangel_2511 | 1:aa5df1878126 | 364 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 365 | case Stepper_Inspiration_Falling_Ramp: |
miguelangel_2511 | 1:aa5df1878126 | 366 | if(stepper_ramp_pulse_counter < RAMP_STEPS){ |
miguelangel_2511 | 1:aa5df1878126 | 367 | stepper_ramp_pulse_counter++; |
miguelangel_2511 | 1:aa5df1878126 | 368 | stepper_pulse.period_us((int)inspiration_period_rise_ramp_us[RAMP_STEPS - stepper_ramp_pulse_counter]); |
miguelangel_2511 | 1:aa5df1878126 | 369 | stepper_pulse.write(0.5); |
miguelangel_2511 | 1:aa5df1878126 | 370 | }else if(stepper_ramp_pulse_counter == RAMP_STEPS){ |
miguelangel_2511 | 1:aa5df1878126 | 371 | stepper_ramp_pulse_counter++; |
miguelangel_2511 | 1:aa5df1878126 | 372 | stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US); |
miguelangel_2511 | 1:aa5df1878126 | 373 | stepper_pulse.write(0.5); |
miguelangel_2511 | 1:aa5df1878126 | 374 | }else{ |
miguelangel_2511 | 1:aa5df1878126 | 375 | stepper_ramp_pulse_counter = 0; |
miguelangel_2511 | 1:aa5df1878126 | 376 | stepper_pulse.write(0.0); |
miguelangel_2511 | 1:aa5df1878126 | 377 | stepper_driver_state = Stepper_Inspiration_Finish; |
miguelangel_2511 | 1:aa5df1878126 | 378 | } |
miguelangel_2511 | 1:aa5df1878126 | 379 | break; |
miguelangel_2511 | 1:aa5df1878126 | 380 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 381 | case Stepper_Inspiration_Finish: |
miguelangel_2511 | 1:aa5df1878126 | 382 | break; |
miguelangel_2511 | 1:aa5df1878126 | 383 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 384 | case Stepper_Expiration_Rising_Ramp: |
miguelangel_2511 | 1:aa5df1878126 | 385 | if(stepper_ramp_pulse_counter < RAMP_STEPS){ |
miguelangel_2511 | 1:aa5df1878126 | 386 | stepper_pulse.period_us((int)expiration_period_rise_ramp_us[stepper_ramp_pulse_counter]); |
miguelangel_2511 | 1:aa5df1878126 | 387 | stepper_pulse.write(0.5); |
miguelangel_2511 | 1:aa5df1878126 | 388 | stepper_ramp_pulse_counter++; |
miguelangel_2511 | 1:aa5df1878126 | 389 | }else{ |
miguelangel_2511 | 1:aa5df1878126 | 390 | stepper_pulse.period_us((int)expiration_stepper_pulse_period_us); |
miguelangel_2511 | 1:aa5df1878126 | 391 | stepper_pulse.write(0.5); |
miguelangel_2511 | 1:aa5df1878126 | 392 | stepper_ramp_pulse_counter = 0; |
miguelangel_2511 | 1:aa5df1878126 | 393 | stepper_driver_state = Stepper_Expiration; |
miguelangel_2511 | 1:aa5df1878126 | 394 | |
miguelangel_2511 | 1:aa5df1878126 | 395 | } |
miguelangel_2511 | 1:aa5df1878126 | 396 | break; |
miguelangel_2511 | 1:aa5df1878126 | 397 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 398 | case Stepper_Expiration: |
miguelangel_2511 | 1:aa5df1878126 | 399 | if(buttons & (1 << LIMIT_SW_01)){ |
miguelangel_2511 | 1:aa5df1878126 | 400 | stepper_driver_state = Stepper_Expiration_Falling_Ramp; |
miguelangel_2511 | 1:aa5df1878126 | 401 | }else if(stepper_pulse_counter > STEPPER_DRIVER_MAX_PULSES){ |
miguelangel_2511 | 1:aa5df1878126 | 402 | stepper_pulse.write(0.0); |
miguelangel_2511 | 1:aa5df1878126 | 403 | stepper_driver_state = Stepper_Limit_Sensor_Error; |
miguelangel_2511 | 1:aa5df1878126 | 404 | }else{ |
miguelangel_2511 | 1:aa5df1878126 | 405 | // Does nothing |
miguelangel_2511 | 1:aa5df1878126 | 406 | } |
miguelangel_2511 | 1:aa5df1878126 | 407 | |
miguelangel_2511 | 1:aa5df1878126 | 408 | break; |
miguelangel_2511 | 1:aa5df1878126 | 409 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 410 | case Stepper_Expiration_Falling_Ramp: |
miguelangel_2511 | 3:45299e7882b9 | 411 | if(stepper_ramp_pulse_counter < SHORT_RAMP_STEPS){ |
miguelangel_2511 | 1:aa5df1878126 | 412 | stepper_ramp_pulse_counter++; |
miguelangel_2511 | 3:45299e7882b9 | 413 | stepper_pulse.period_us((int)expiration_period_short_rise_ramp_us[SHORT_RAMP_STEPS - stepper_ramp_pulse_counter]); |
miguelangel_2511 | 1:aa5df1878126 | 414 | stepper_pulse.write(0.5); |
miguelangel_2511 | 3:45299e7882b9 | 415 | }else if(stepper_ramp_pulse_counter == SHORT_RAMP_STEPS){ |
miguelangel_2511 | 1:aa5df1878126 | 416 | stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US); |
miguelangel_2511 | 1:aa5df1878126 | 417 | stepper_pulse.write(0.5); |
miguelangel_2511 | 1:aa5df1878126 | 418 | stepper_ramp_pulse_counter++; |
miguelangel_2511 | 1:aa5df1878126 | 419 | }else{ |
miguelangel_2511 | 3:45299e7882b9 | 420 | stepper_ramp_pulse_counter = 0; |
miguelangel_2511 | 1:aa5df1878126 | 421 | stepper_pulse.write(0.0); |
miguelangel_2511 | 1:aa5df1878126 | 422 | stepper_driver_state = Stepper_Expiration_Finish; |
miguelangel_2511 | 1:aa5df1878126 | 423 | } |
miguelangel_2511 | 1:aa5df1878126 | 424 | break; |
miguelangel_2511 | 1:aa5df1878126 | 425 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 426 | case Stepper_Expiration_Finish: |
miguelangel_2511 | 1:aa5df1878126 | 427 | break; |
miguelangel_2511 | 1:aa5df1878126 | 428 | // -------------------------------------------------------------------- |
miguelangel_2511 | 1:aa5df1878126 | 429 | default: |
miguelangel_2511 | 1:aa5df1878126 | 430 | break; |
miguelangel_2511 | 1:aa5df1878126 | 431 | } |
miguelangel_2511 | 1:aa5df1878126 | 432 | |
miguelangel_2511 | 1:aa5df1878126 | 433 | } |
miguelangel_2511 | 1:aa5df1878126 | 434 | |
miguelangel_2511 | 1:aa5df1878126 | 435 | |
miguelangel_2511 | 1:aa5df1878126 | 436 | |
miguelangel_2511 | 1:aa5df1878126 | 437 | |
miguelangel_2511 | 1:aa5df1878126 | 438 |