Miguel Angel Caycho / Mbed OS Pruebas_Ventilador_Eplimin_01_Backup

para el ventilador

Dependencies:   QEI FastPWM

stepper_motor_driver.cpp@4:d42e1953243c, 2020-04-18 (annotated)

Committer:
miguelangel_2511
Date:
Sat Apr 18 05:13:40 2020 +0000
Revision:
4:d42e1953243c
Parent:
3:45299e7882b9
Child:
5:bd5fc0510e7b
Mejora de la precision en la frecuencia respiratoria, considerando el tiempo que toma completar cada rampa.

Who changed what in which revision?

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