Miguel Angel Caycho / Mbed OS Pruebas_Ventilador_Eplimin_01_Backup

para el ventilador

Dependencies:   QEI FastPWM

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?

UserRevisionLine numberNew 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