Miguel Angel Caycho / Mbed OS Pruebas_Ventilador_Eplimin_01_Backup

para el ventilador

Dependencies:   QEI FastPWM

stepper_motor_driver.cpp@5:bd5fc0510e7b, 2020-04-19 (annotated)

Committer:
miguelangel_2511
Date:
Sun Apr 19 05:38:41 2020 +0000
Revision:
5:bd5fc0510e7b
Parent:
4:d42e1953243c
Child:
6:d38287621cca
Los calculos hechos para incluir el tiempo de la rampa se encontraban errados, por lo cual dichos calculos fueron corregidos y simplificados. El sistema funciona correctamente hasta una frecuencia respiratoria de 22 RPM.

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 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 5:bd5fc0510e7b 78 //uint32_t min_inspiration_ramp_period_us;
miguelangel_2511 5:bd5fc0510e7b 79 //uint32_t min_expiration_ramp_period_us;
miguelangel_2511 5:bd5fc0510e7b 80
miguelangel_2511 1:aa5df1878126 81 // Volume setpoint (250mL, 300mL, 350ml ... 750ml) expressed in steps
miguelangel_2511 1:aa5df1878126 82 uint32_t volume_in_steps[VOLUME_SETPOINT_INDEX_LIMIT] = {
miguelangel_2511 3:45299e7882b9 83 720, // 1600, //250 mL
miguelangel_2511 3:45299e7882b9 84 765, // 1700, //300 mL
miguelangel_2511 3:45299e7882b9 85 810, // 1800, //350 mL
miguelangel_2511 3:45299e7882b9 86 855, // 1900, //400 mL
miguelangel_2511 3:45299e7882b9 87 900, // 2000, //450 mL
miguelangel_2511 3:45299e7882b9 88 945, // 2100, //500 mL
miguelangel_2511 3:45299e7882b9 89 990, // 2200, //550 mL
miguelangel_2511 3:45299e7882b9 90 1035, // 2300, //600 mL
miguelangel_2511 3:45299e7882b9 91 1080, // 2400, //650 mL
miguelangel_2511 3:45299e7882b9 92 1125, // 2500, //700 mL
miguelangel_2511 3:45299e7882b9 93 1170 // 2600 //750 mL
miguelangel_2511 1:aa5df1878126 94 };
miguelangel_2511 1:aa5df1878126 95
miguelangel_2511 1:aa5df1878126 96
miguelangel_2511 1:aa5df1878126 97 // Variables used in every state of the stepper motor driver */
miguelangel_2511 1:aa5df1878126 98 volatile uint32_t stepper_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 99 volatile uint32_t stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 100
miguelangel_2511 1:aa5df1878126 101 // State of the stepper motor
miguelangel_2511 1:aa5df1878126 102 volatile Stepper_Driver_State_t stepper_driver_state = Stepper_Stand_By;
miguelangel_2511 1:aa5df1878126 103
miguelangel_2511 1:aa5df1878126 104 /* Function definition */
miguelangel_2511 1:aa5df1878126 105
miguelangel_2511 1:aa5df1878126 106 /* Initialize the control signals for the stepper */
miguelangel_2511 1:aa5df1878126 107 void Stepper_Driver_Init(void){
miguelangel_2511 1:aa5df1878126 108 stepper_en = STEPPER_ENABLED;
miguelangel_2511 1:aa5df1878126 109 stepper_dir = TO_HOME;
miguelangel_2511 1:aa5df1878126 110 stepper_pulse.period_us((int)calibration_stepper_pulse_period_us);
miguelangel_2511 1:aa5df1878126 111 stepper_pulse.write(0.0f);
miguelangel_2511 1:aa5df1878126 112 stepper_pulse_feedback.fall(&Stepper_Driver_Pulse_Signal_Update);
miguelangel_2511 3:45299e7882b9 113 stepper_driver_state = Stepper_Inspiration_Finish;
miguelangel_2511 3:45299e7882b9 114 stepper_parameters_update_flag = 1;
miguelangel_2511 1:aa5df1878126 115 // Just for testing purposes
miguelangel_2511 1:aa5df1878126 116 //inspiration_stepper_pulses_setpoint = volume_in_steps[10];
miguelangel_2511 1:aa5df1878126 117 }
miguelangel_2511 1:aa5df1878126 118
miguelangel_2511 0:9d0b9785d3d6 119
miguelangel_2511 0:9d0b9785d3d6 120
miguelangel_2511 0:9d0b9785d3d6 121
miguelangel_2511 1:aa5df1878126 122 void Stepper_Go_Home_Position(void){
miguelangel_2511 1:aa5df1878126 123 // stepper_driver_state = Stepper_Driver_Home_Ramp;
miguelangel_2511 1:aa5df1878126 124
miguelangel_2511 1:aa5df1878126 125 //while(stepper_driver_state != Stepper_Driver_Stand_By){
miguelangel_2511 1:aa5df1878126 126 // Stepper_Driver_State_Machine();
miguelangel_2511 1:aa5df1878126 127 //}
miguelangel_2511 1:aa5df1878126 128 }
miguelangel_2511 1:aa5df1878126 129
miguelangel_2511 1:aa5df1878126 130
miguelangel_2511 3:45299e7882b9 131 void Stepper_Update_Parameters(void){
miguelangel_2511 3:45299e7882b9 132
miguelangel_2511 3:45299e7882b9 133 int8_t stepper_volume_index;
miguelangel_2511 4:d42e1953243c 134 uint32_t regular_steps;
miguelangel_2511 3:45299e7882b9 135 float total_time, total_time_sec, insp_time_sec, exp_time_sec;
miguelangel_2511 4:d42e1953243c 136 float inspiration_stepper_pulse_period_sec, expiration_stepper_pulse_period_sec;
miguelangel_2511 5:bd5fc0510e7b 137 float ramp_max_pulse_freq_hz, ramp_time_sec, short_ramp_time_sec;
miguelangel_2511 3:45299e7882b9 138
miguelangel_2511 3:45299e7882b9 139 /* Update the value of the volume setpoint (expressed in steps) */
miguelangel_2511 3:45299e7882b9 140 stepper_volume_index = (volume_setpoint - VOLUME_SETPOINT_MINIMUM_VALUE) / VOLUME_SETPOINT_STEP;
miguelangel_2511 3:45299e7882b9 141 inspiration_stepper_pulses_setpoint = volume_in_steps[stepper_volume_index];
miguelangel_2511 3:45299e7882b9 142
miguelangel_2511 3:45299e7882b9 143 /* Update the value of the pulse periods in microseconds */
miguelangel_2511 3:45299e7882b9 144 total_time = inspiration_time + expiration_time;
miguelangel_2511 4:d42e1953243c 145 total_time_sec = 60.0 / resp_frequency;
miguelangel_2511 3:45299e7882b9 146 insp_time_sec = (total_time_sec * inspiration_time) / total_time;
miguelangel_2511 3:45299e7882b9 147 exp_time_sec = (total_time_sec * expiration_time) / total_time;
miguelangel_2511 4:d42e1953243c 148
miguelangel_2511 4:d42e1953243c 149 /* Calculate stepper pulse period for inspiration */
miguelangel_2511 4:d42e1953243c 150 regular_steps = inspiration_stepper_pulses_setpoint - (2 * RAMP_STEPS);
miguelangel_2511 5:bd5fc0510e7b 151 ramp_max_pulse_freq_hz = ((float)inspiration_stepper_pulses_setpoint) / insp_time_sec;
miguelangel_2511 5:bd5fc0510e7b 152 ramp_time_sec = Stepper_Calculate_Insp_Period_Ramp(ramp_max_pulse_freq_hz);
miguelangel_2511 5:bd5fc0510e7b 153 inspiration_stepper_pulse_period_sec = (insp_time_sec - (2*ramp_time_sec)) / (float)regular_steps;
miguelangel_2511 4:d42e1953243c 154 inspiration_stepper_pulse_period_us = (uint32_t)(1000000.0 * inspiration_stepper_pulse_period_sec);
miguelangel_2511 5:bd5fc0510e7b 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 5:bd5fc0510e7b 158 ramp_max_pulse_freq_hz = ((float)inspiration_stepper_pulses_setpoint) / exp_time_sec;
miguelangel_2511 5:bd5fc0510e7b 159 ramp_time_sec = Stepper_Calculate_Exp_Period_Ramp(ramp_max_pulse_freq_hz);
miguelangel_2511 5:bd5fc0510e7b 160 short_ramp_time_sec = Stepper_Calculate_Exp_Short_Period_Ramp(ramp_max_pulse_freq_hz);
miguelangel_2511 5:bd5fc0510e7b 161 expiration_stepper_pulse_period_sec = (exp_time_sec - (ramp_time_sec + short_ramp_time_sec)) / (float)regular_steps;
miguelangel_2511 4:d42e1953243c 162 expiration_stepper_pulse_period_us = (uint32_t)(1000000.0 * expiration_stepper_pulse_period_sec);
miguelangel_2511 3:45299e7882b9 163
miguelangel_2511 3:45299e7882b9 164 /* Just for testing purposes */
miguelangel_2511 3:45299e7882b9 165 pc.printf("Cantidad de pulsos: %d \n\n\r", inspiration_stepper_pulses_setpoint);
miguelangel_2511 4:d42e1953243c 166 pc.printf("Frecuencia de arranque: %d Hz\n\r", STEPPER_START_BRAKE_FREQUENCY_HZ);
miguelangel_2511 4:d42e1953243c 167 pc.printf("Frecuencia de pulsos de ida: %f Hz\n\r", 1.0 / inspiration_stepper_pulse_period_sec);
miguelangel_2511 4:d42e1953243c 168 pc.printf("Periodo de pulsos de ida en us: %d us\n\r", inspiration_stepper_pulse_period_us);
miguelangel_2511 4:d42e1953243c 169 pc.printf("Frecuencia de pulsos de vuelta: %f Hz\n\r", 1.0 / expiration_stepper_pulse_period_us);
miguelangel_2511 4:d42e1953243c 170 pc.printf("Periodo de pulsos de vuelta en us: %d us\n\r", expiration_stepper_pulse_period_us);
miguelangel_2511 3:45299e7882b9 171
miguelangel_2511 3:45299e7882b9 172 }
miguelangel_2511 3:45299e7882b9 173
miguelangel_2511 1:aa5df1878126 174
miguelangel_2511 5:bd5fc0510e7b 175 float Stepper_Calculate_Insp_Period_Ramp(float max_freq){
miguelangel_2511 5:bd5fc0510e7b 176
miguelangel_2511 5:bd5fc0510e7b 177 float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz;
miguelangel_2511 5:bd5fc0510e7b 178 float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec;
miguelangel_2511 5:bd5fc0510e7b 179 uint8_t i;
miguelangel_2511 5:bd5fc0510e7b 180
miguelangel_2511 5:bd5fc0510e7b 181 /* Period rise ramp for inspiration */
miguelangel_2511 5:bd5fc0510e7b 182 initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
miguelangel_2511 5:bd5fc0510e7b 183 final_frequency_hz = max_freq;
miguelangel_2511 5:bd5fc0510e7b 184 frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz;
miguelangel_2511 5:bd5fc0510e7b 185
miguelangel_2511 5:bd5fc0510e7b 186 ramp_time_sec = 0;
miguelangel_2511 1:aa5df1878126 187
miguelangel_2511 5:bd5fc0510e7b 188 for(i = 0; i < RAMP_STEPS; i++){
miguelangel_2511 5:bd5fc0510e7b 189 frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * sigma_ramp[i]);
miguelangel_2511 5:bd5fc0510e7b 190 period_i_sec = 1.0 / frequency_i_hz;
miguelangel_2511 5:bd5fc0510e7b 191 period_i_us = 1000000.0 * period_i_sec;
miguelangel_2511 5:bd5fc0510e7b 192 inspiration_period_rise_ramp_us[i] = (uint32_t)period_i_us;
miguelangel_2511 5:bd5fc0510e7b 193 ramp_time_sec += period_i_sec;
miguelangel_2511 5:bd5fc0510e7b 194 }
miguelangel_2511 5:bd5fc0510e7b 195
miguelangel_2511 5:bd5fc0510e7b 196 return ramp_time_sec;
miguelangel_2511 5:bd5fc0510e7b 197 }
miguelangel_2511 5:bd5fc0510e7b 198
miguelangel_2511 5:bd5fc0510e7b 199
miguelangel_2511 5:bd5fc0510e7b 200 float Stepper_Calculate_Exp_Period_Ramp(float max_freq){
miguelangel_2511 5:bd5fc0510e7b 201
miguelangel_2511 5:bd5fc0510e7b 202 float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz;
miguelangel_2511 5:bd5fc0510e7b 203 float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec;
miguelangel_2511 1:aa5df1878126 204 uint8_t i;
miguelangel_2511 1:aa5df1878126 205
miguelangel_2511 1:aa5df1878126 206 /* Period rise ramp for inspiration */
miguelangel_2511 5:bd5fc0510e7b 207 initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
miguelangel_2511 5:bd5fc0510e7b 208 final_frequency_hz = max_freq;
miguelangel_2511 5:bd5fc0510e7b 209 frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz;
miguelangel_2511 1:aa5df1878126 210
miguelangel_2511 5:bd5fc0510e7b 211 ramp_time_sec = 0;
miguelangel_2511 1:aa5df1878126 212
miguelangel_2511 5:bd5fc0510e7b 213 for(i = 0; i < RAMP_STEPS; i++){
miguelangel_2511 5:bd5fc0510e7b 214 frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * sigma_ramp[i]);
miguelangel_2511 5:bd5fc0510e7b 215 period_i_sec = 1.0 / frequency_i_hz;
miguelangel_2511 5:bd5fc0510e7b 216 period_i_us = 1000000.0 * period_i_sec;
miguelangel_2511 1:aa5df1878126 217 expiration_period_rise_ramp_us[i] = (uint32_t)period_i_us;
miguelangel_2511 5:bd5fc0510e7b 218 ramp_time_sec += period_i_sec;
miguelangel_2511 5:bd5fc0510e7b 219 }
miguelangel_2511 1:aa5df1878126 220
miguelangel_2511 5:bd5fc0510e7b 221 return ramp_time_sec;
miguelangel_2511 5:bd5fc0510e7b 222 }
miguelangel_2511 5:bd5fc0510e7b 223
miguelangel_2511 3:45299e7882b9 224
miguelangel_2511 3:45299e7882b9 225
miguelangel_2511 5:bd5fc0510e7b 226 float Stepper_Calculate_Exp_Short_Period_Ramp(float max_freq){
miguelangel_2511 5:bd5fc0510e7b 227
miguelangel_2511 5:bd5fc0510e7b 228 float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz;
miguelangel_2511 5:bd5fc0510e7b 229 float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec;
miguelangel_2511 5:bd5fc0510e7b 230 uint8_t i;
miguelangel_2511 5:bd5fc0510e7b 231
miguelangel_2511 5:bd5fc0510e7b 232 /* Period rise ramp for inspiration */
miguelangel_2511 5:bd5fc0510e7b 233 initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
miguelangel_2511 5:bd5fc0510e7b 234 final_frequency_hz = max_freq;
miguelangel_2511 5:bd5fc0510e7b 235 frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz;
miguelangel_2511 5:bd5fc0510e7b 236
miguelangel_2511 5:bd5fc0510e7b 237 ramp_time_sec = 0;
miguelangel_2511 5:bd5fc0510e7b 238
miguelangel_2511 5:bd5fc0510e7b 239 for(i = 0; i < SHORT_RAMP_STEPS; i++){
miguelangel_2511 5:bd5fc0510e7b 240 frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * short_sigma_ramp[i]);
miguelangel_2511 5:bd5fc0510e7b 241 period_i_sec = 1.0 / frequency_i_hz;
miguelangel_2511 5:bd5fc0510e7b 242 period_i_us = 1000000.0 * period_i_sec;
miguelangel_2511 5:bd5fc0510e7b 243 expiration_period_short_rise_ramp_us[i] = (uint32_t)period_i_us;
miguelangel_2511 5:bd5fc0510e7b 244 ramp_time_sec += period_i_sec;
miguelangel_2511 5:bd5fc0510e7b 245 }
miguelangel_2511 5:bd5fc0510e7b 246
miguelangel_2511 5:bd5fc0510e7b 247 return ramp_time_sec;
miguelangel_2511 1:aa5df1878126 248 }
miguelangel_2511 1:aa5df1878126 249
miguelangel_2511 1:aa5df1878126 250
miguelangel_2511 5:bd5fc0510e7b 251
miguelangel_2511 5:bd5fc0510e7b 252
miguelangel_2511 5:bd5fc0510e7b 253
miguelangel_2511 1:aa5df1878126 254 void Stepper_Driver_State_Machine(void){
miguelangel_2511 0:9d0b9785d3d6 255
miguelangel_2511 1:aa5df1878126 256 switch(stepper_driver_state){
miguelangel_2511 1:aa5df1878126 257
miguelangel_2511 1:aa5df1878126 258 case Stepper_Inspiration_Rising_Ramp:
miguelangel_2511 1:aa5df1878126 259
miguelangel_2511 1:aa5df1878126 260 break;
miguelangel_2511 1:aa5df1878126 261 // -----------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 262 case Stepper_Inspiration:
miguelangel_2511 1:aa5df1878126 263
miguelangel_2511 1:aa5df1878126 264 break;
miguelangel_2511 1:aa5df1878126 265 // -----------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 266 case Stepper_Inspiration_Falling_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_Finish:
miguelangel_2511 1:aa5df1878126 271
miguelangel_2511 1:aa5df1878126 272 stepper_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 273 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 274 stepper_dir = TO_HOME;
miguelangel_2511 1:aa5df1878126 275 stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
miguelangel_2511 1:aa5df1878126 276 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 277 stepper_driver_state = Stepper_Expiration_Rising_Ramp;
miguelangel_2511 1:aa5df1878126 278
miguelangel_2511 1:aa5df1878126 279 break;
miguelangel_2511 1:aa5df1878126 280 // -----------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 281 case Stepper_Expiration_Rising_Ramp:
miguelangel_2511 1:aa5df1878126 282
miguelangel_2511 1:aa5df1878126 283 break;
miguelangel_2511 1:aa5df1878126 284 // -----------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 285 case Stepper_Expiration:
miguelangel_2511 1:aa5df1878126 286
miguelangel_2511 1:aa5df1878126 287 break;
miguelangel_2511 1:aa5df1878126 288 // -----------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 289 case Stepper_Expiration_Falling_Ramp:
miguelangel_2511 1:aa5df1878126 290
miguelangel_2511 1:aa5df1878126 291 break;
miguelangel_2511 1:aa5df1878126 292 // -----------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 293 case Stepper_Expiration_Finish:
miguelangel_2511 3:45299e7882b9 294
miguelangel_2511 3:45299e7882b9 295 if(stepper_parameters_update_flag){
miguelangel_2511 3:45299e7882b9 296 stepper_parameters_update_flag = 0;
miguelangel_2511 3:45299e7882b9 297 Stepper_Update_Parameters();
miguelangel_2511 5:bd5fc0510e7b 298 //Stepper_Calculate_Period_Ramp();
miguelangel_2511 3:45299e7882b9 299 }
miguelangel_2511 1:aa5df1878126 300
miguelangel_2511 1:aa5df1878126 301 stepper_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 302 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 303 stepper_dir = TO_AIRBAG;
miguelangel_2511 1:aa5df1878126 304 stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
miguelangel_2511 1:aa5df1878126 305 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 306 stepper_driver_state = Stepper_Inspiration_Rising_Ramp;
miguelangel_2511 1:aa5df1878126 307
miguelangel_2511 3:45299e7882b9 308 break;
miguelangel_2511 1:aa5df1878126 309 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 310 case Stepper_Limit_Sensor_Error:
miguelangel_2511 1:aa5df1878126 311 // Stop the system
miguelangel_2511 1:aa5df1878126 312 stepper_en = STEPPER_DISABLED;
miguelangel_2511 1:aa5df1878126 313 tft->locate(340,VALUES_ROW2_Y_POS);
miguelangel_2511 1:aa5df1878126 314 tft->printf("error");
miguelangel_2511 1:aa5df1878126 315 stepper_driver_state = Stepper_Stand_By;
miguelangel_2511 1:aa5df1878126 316 break;
miguelangel_2511 1:aa5df1878126 317 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 318 case Stepper_Stand_By:
miguelangel_2511 1:aa5df1878126 319
miguelangel_2511 1:aa5df1878126 320 break;
miguelangel_2511 1:aa5df1878126 321 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 322 default:
miguelangel_2511 1:aa5df1878126 323
miguelangel_2511 1:aa5df1878126 324 break;
miguelangel_2511 0:9d0b9785d3d6 325 }
miguelangel_2511 0:9d0b9785d3d6 326 }
miguelangel_2511 0:9d0b9785d3d6 327
miguelangel_2511 0:9d0b9785d3d6 328
miguelangel_2511 1:aa5df1878126 329 void Stepper_Driver_Pulse_Signal_Update(void){
miguelangel_2511 1:aa5df1878126 330
miguelangel_2511 1:aa5df1878126 331 // Increment the stepper pulse counter
miguelangel_2511 1:aa5df1878126 332 stepper_pulse_counter++;
miguelangel_2511 1:aa5df1878126 333
miguelangel_2511 1:aa5df1878126 334 // Increment the proper counter
miguelangel_2511 1:aa5df1878126 335 switch(stepper_driver_state){
miguelangel_2511 1:aa5df1878126 336 /* Increase the speed gradually */
miguelangel_2511 1:aa5df1878126 337 case Stepper_Inspiration_Rising_Ramp:
miguelangel_2511 1:aa5df1878126 338 if(stepper_ramp_pulse_counter < RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 339 stepper_pulse.period_us((int)inspiration_period_rise_ramp_us[stepper_ramp_pulse_counter]);
miguelangel_2511 1:aa5df1878126 340 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 341 stepper_ramp_pulse_counter++;
miguelangel_2511 1:aa5df1878126 342 }else{
miguelangel_2511 1:aa5df1878126 343 stepper_pulse.period_us((int)inspiration_stepper_pulse_period_us);
miguelangel_2511 1:aa5df1878126 344 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 345 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 346 stepper_driver_state = Stepper_Inspiration;
miguelangel_2511 1:aa5df1878126 347 }
miguelangel_2511 1:aa5df1878126 348 break;
miguelangel_2511 1:aa5df1878126 349 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 350 case Stepper_Inspiration:
miguelangel_2511 1:aa5df1878126 351 if(stepper_pulse_counter >= (inspiration_stepper_pulses_setpoint - RAMP_STEPS)){
miguelangel_2511 1:aa5df1878126 352 stepper_driver_state = Stepper_Inspiration_Falling_Ramp;
miguelangel_2511 1:aa5df1878126 353 }
miguelangel_2511 1:aa5df1878126 354 break;
miguelangel_2511 1:aa5df1878126 355 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 356 case Stepper_Inspiration_Falling_Ramp:
miguelangel_2511 1:aa5df1878126 357 if(stepper_ramp_pulse_counter < RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 358 stepper_ramp_pulse_counter++;
miguelangel_2511 1:aa5df1878126 359 stepper_pulse.period_us((int)inspiration_period_rise_ramp_us[RAMP_STEPS - stepper_ramp_pulse_counter]);
miguelangel_2511 1:aa5df1878126 360 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 361 }else if(stepper_ramp_pulse_counter == RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 362 stepper_ramp_pulse_counter++;
miguelangel_2511 1:aa5df1878126 363 stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
miguelangel_2511 1:aa5df1878126 364 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 365 }else{
miguelangel_2511 1:aa5df1878126 366 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 367 stepper_pulse.write(0.0);
miguelangel_2511 1:aa5df1878126 368 stepper_driver_state = Stepper_Inspiration_Finish;
miguelangel_2511 1:aa5df1878126 369 }
miguelangel_2511 1:aa5df1878126 370 break;
miguelangel_2511 1:aa5df1878126 371 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 372 case Stepper_Inspiration_Finish:
miguelangel_2511 1:aa5df1878126 373 break;
miguelangel_2511 1:aa5df1878126 374 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 375 case Stepper_Expiration_Rising_Ramp:
miguelangel_2511 1:aa5df1878126 376 if(stepper_ramp_pulse_counter < RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 377 stepper_pulse.period_us((int)expiration_period_rise_ramp_us[stepper_ramp_pulse_counter]);
miguelangel_2511 1:aa5df1878126 378 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 379 stepper_ramp_pulse_counter++;
miguelangel_2511 1:aa5df1878126 380 }else{
miguelangel_2511 1:aa5df1878126 381 stepper_pulse.period_us((int)expiration_stepper_pulse_period_us);
miguelangel_2511 1:aa5df1878126 382 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 383 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 384 stepper_driver_state = Stepper_Expiration;
miguelangel_2511 1:aa5df1878126 385
miguelangel_2511 1:aa5df1878126 386 }
miguelangel_2511 1:aa5df1878126 387 break;
miguelangel_2511 1:aa5df1878126 388 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 389 case Stepper_Expiration:
miguelangel_2511 1:aa5df1878126 390 if(buttons & (1 << LIMIT_SW_01)){
miguelangel_2511 1:aa5df1878126 391 stepper_driver_state = Stepper_Expiration_Falling_Ramp;
miguelangel_2511 1:aa5df1878126 392 }else if(stepper_pulse_counter > STEPPER_DRIVER_MAX_PULSES){
miguelangel_2511 1:aa5df1878126 393 stepper_pulse.write(0.0);
miguelangel_2511 1:aa5df1878126 394 stepper_driver_state = Stepper_Limit_Sensor_Error;
miguelangel_2511 1:aa5df1878126 395 }else{
miguelangel_2511 1:aa5df1878126 396 // Does nothing
miguelangel_2511 1:aa5df1878126 397 }
miguelangel_2511 1:aa5df1878126 398
miguelangel_2511 1:aa5df1878126 399 break;
miguelangel_2511 1:aa5df1878126 400 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 401 case Stepper_Expiration_Falling_Ramp:
miguelangel_2511 3:45299e7882b9 402 if(stepper_ramp_pulse_counter < SHORT_RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 403 stepper_ramp_pulse_counter++;
miguelangel_2511 3:45299e7882b9 404 stepper_pulse.period_us((int)expiration_period_short_rise_ramp_us[SHORT_RAMP_STEPS - stepper_ramp_pulse_counter]);
miguelangel_2511 1:aa5df1878126 405 stepper_pulse.write(0.5);
miguelangel_2511 3:45299e7882b9 406 }else if(stepper_ramp_pulse_counter == SHORT_RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 407 stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
miguelangel_2511 1:aa5df1878126 408 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 409 stepper_ramp_pulse_counter++;
miguelangel_2511 1:aa5df1878126 410 }else{
miguelangel_2511 3:45299e7882b9 411 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 412 stepper_pulse.write(0.0);
miguelangel_2511 1:aa5df1878126 413 stepper_driver_state = Stepper_Expiration_Finish;
miguelangel_2511 1:aa5df1878126 414 }
miguelangel_2511 1:aa5df1878126 415 break;
miguelangel_2511 1:aa5df1878126 416 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 417 case Stepper_Expiration_Finish:
miguelangel_2511 1:aa5df1878126 418 break;
miguelangel_2511 1:aa5df1878126 419 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 420 default:
miguelangel_2511 1:aa5df1878126 421 break;
miguelangel_2511 1:aa5df1878126 422 }
miguelangel_2511 1:aa5df1878126 423
miguelangel_2511 1:aa5df1878126 424 }
miguelangel_2511 1:aa5df1878126 425
miguelangel_2511 1:aa5df1878126 426
miguelangel_2511 1:aa5df1878126 427
miguelangel_2511 1:aa5df1878126 428
miguelangel_2511 1:aa5df1878126 429