Miguel Angel Caycho / Mbed OS Pruebas_Ventilador_Eplimin_01_Backup

para el ventilador

Dependencies:   QEI FastPWM

stepper_motor_driver.cpp@6:d38287621cca, 2020-04-20 (annotated)

Committer:
miguelangel_2511
Date:
Mon Apr 20 11:39:30 2020 +0000
Revision:
6:d38287621cca
Parent:
5:bd5fc0510e7b
Child:
9:95fdcdc0977e
La pantalla TFT fue reemplazada por una pantalla Nextion que se comunica por el puerto serial (UART). El control de la pantalla sigue realizandose a traves de un encoder.

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 1:aa5df1878126 286
miguelangel_2511 1:aa5df1878126 287 break;
miguelangel_2511 1:aa5df1878126 288 // -----------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 289 case Stepper_Expiration_Rising_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:
miguelangel_2511 1:aa5df1878126 294
miguelangel_2511 1:aa5df1878126 295 break;
miguelangel_2511 1:aa5df1878126 296 // -----------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 297 case Stepper_Expiration_Falling_Ramp:
miguelangel_2511 1:aa5df1878126 298
miguelangel_2511 1:aa5df1878126 299 break;
miguelangel_2511 1:aa5df1878126 300 // -----------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 301 case Stepper_Expiration_Finish:
miguelangel_2511 3:45299e7882b9 302
miguelangel_2511 3:45299e7882b9 303 if(stepper_parameters_update_flag){
miguelangel_2511 3:45299e7882b9 304 stepper_parameters_update_flag = 0;
miguelangel_2511 3:45299e7882b9 305 Stepper_Update_Parameters();
miguelangel_2511 5:bd5fc0510e7b 306 //Stepper_Calculate_Period_Ramp();
miguelangel_2511 3:45299e7882b9 307 }
miguelangel_2511 1:aa5df1878126 308
miguelangel_2511 1:aa5df1878126 309 stepper_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 310 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 311 stepper_dir = TO_AIRBAG;
miguelangel_2511 1:aa5df1878126 312 stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
miguelangel_2511 1:aa5df1878126 313 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 314 stepper_driver_state = Stepper_Inspiration_Rising_Ramp;
miguelangel_2511 1:aa5df1878126 315
miguelangel_2511 3:45299e7882b9 316 break;
miguelangel_2511 1:aa5df1878126 317 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 318 case Stepper_Limit_Sensor_Error:
miguelangel_2511 1:aa5df1878126 319 // Stop the system
miguelangel_2511 1:aa5df1878126 320 stepper_en = STEPPER_DISABLED;
miguelangel_2511 6:d38287621cca 321 // tft->locate(340,VALUES_ROW2_Y_POS);
miguelangel_2511 6:d38287621cca 322 // tft->printf("error");
miguelangel_2511 1:aa5df1878126 323 stepper_driver_state = Stepper_Stand_By;
miguelangel_2511 1:aa5df1878126 324 break;
miguelangel_2511 1:aa5df1878126 325 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 326 case Stepper_Stand_By:
miguelangel_2511 1:aa5df1878126 327
miguelangel_2511 1:aa5df1878126 328 break;
miguelangel_2511 1:aa5df1878126 329 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 330 default:
miguelangel_2511 1:aa5df1878126 331
miguelangel_2511 1:aa5df1878126 332 break;
miguelangel_2511 0:9d0b9785d3d6 333 }
miguelangel_2511 0:9d0b9785d3d6 334 }
miguelangel_2511 0:9d0b9785d3d6 335
miguelangel_2511 0:9d0b9785d3d6 336
miguelangel_2511 1:aa5df1878126 337 void Stepper_Driver_Pulse_Signal_Update(void){
miguelangel_2511 1:aa5df1878126 338
miguelangel_2511 1:aa5df1878126 339 // Increment the stepper pulse counter
miguelangel_2511 1:aa5df1878126 340 stepper_pulse_counter++;
miguelangel_2511 1:aa5df1878126 341
miguelangel_2511 1:aa5df1878126 342 // Increment the proper counter
miguelangel_2511 1:aa5df1878126 343 switch(stepper_driver_state){
miguelangel_2511 1:aa5df1878126 344 /* Increase the speed gradually */
miguelangel_2511 1:aa5df1878126 345 case Stepper_Inspiration_Rising_Ramp:
miguelangel_2511 1:aa5df1878126 346 if(stepper_ramp_pulse_counter < RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 347 stepper_pulse.period_us((int)inspiration_period_rise_ramp_us[stepper_ramp_pulse_counter]);
miguelangel_2511 1:aa5df1878126 348 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 349 stepper_ramp_pulse_counter++;
miguelangel_2511 1:aa5df1878126 350 }else{
miguelangel_2511 1:aa5df1878126 351 stepper_pulse.period_us((int)inspiration_stepper_pulse_period_us);
miguelangel_2511 1:aa5df1878126 352 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 353 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 354 stepper_driver_state = Stepper_Inspiration;
miguelangel_2511 1:aa5df1878126 355 }
miguelangel_2511 1:aa5df1878126 356 break;
miguelangel_2511 1:aa5df1878126 357 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 358 case Stepper_Inspiration:
miguelangel_2511 1:aa5df1878126 359 if(stepper_pulse_counter >= (inspiration_stepper_pulses_setpoint - RAMP_STEPS)){
miguelangel_2511 1:aa5df1878126 360 stepper_driver_state = Stepper_Inspiration_Falling_Ramp;
miguelangel_2511 1:aa5df1878126 361 }
miguelangel_2511 1:aa5df1878126 362 break;
miguelangel_2511 1:aa5df1878126 363 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 364 case Stepper_Inspiration_Falling_Ramp:
miguelangel_2511 1:aa5df1878126 365 if(stepper_ramp_pulse_counter < RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 366 stepper_ramp_pulse_counter++;
miguelangel_2511 1:aa5df1878126 367 stepper_pulse.period_us((int)inspiration_period_rise_ramp_us[RAMP_STEPS - stepper_ramp_pulse_counter]);
miguelangel_2511 1:aa5df1878126 368 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 369 }else if(stepper_ramp_pulse_counter == RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 370 stepper_ramp_pulse_counter++;
miguelangel_2511 1:aa5df1878126 371 stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
miguelangel_2511 1:aa5df1878126 372 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 373 }else{
miguelangel_2511 1:aa5df1878126 374 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 375 stepper_pulse.write(0.0);
miguelangel_2511 1:aa5df1878126 376 stepper_driver_state = Stepper_Inspiration_Finish;
miguelangel_2511 1:aa5df1878126 377 }
miguelangel_2511 1:aa5df1878126 378 break;
miguelangel_2511 1:aa5df1878126 379 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 380 case Stepper_Inspiration_Finish:
miguelangel_2511 1:aa5df1878126 381 break;
miguelangel_2511 1:aa5df1878126 382 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 383 case Stepper_Expiration_Rising_Ramp:
miguelangel_2511 1:aa5df1878126 384 if(stepper_ramp_pulse_counter < RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 385 stepper_pulse.period_us((int)expiration_period_rise_ramp_us[stepper_ramp_pulse_counter]);
miguelangel_2511 1:aa5df1878126 386 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 387 stepper_ramp_pulse_counter++;
miguelangel_2511 1:aa5df1878126 388 }else{
miguelangel_2511 1:aa5df1878126 389 stepper_pulse.period_us((int)expiration_stepper_pulse_period_us);
miguelangel_2511 1:aa5df1878126 390 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 391 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 392 stepper_driver_state = Stepper_Expiration;
miguelangel_2511 1:aa5df1878126 393
miguelangel_2511 1:aa5df1878126 394 }
miguelangel_2511 1:aa5df1878126 395 break;
miguelangel_2511 1:aa5df1878126 396 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 397 case Stepper_Expiration:
miguelangel_2511 1:aa5df1878126 398 if(buttons & (1 << LIMIT_SW_01)){
miguelangel_2511 1:aa5df1878126 399 stepper_driver_state = Stepper_Expiration_Falling_Ramp;
miguelangel_2511 1:aa5df1878126 400 }else if(stepper_pulse_counter > STEPPER_DRIVER_MAX_PULSES){
miguelangel_2511 1:aa5df1878126 401 stepper_pulse.write(0.0);
miguelangel_2511 1:aa5df1878126 402 stepper_driver_state = Stepper_Limit_Sensor_Error;
miguelangel_2511 1:aa5df1878126 403 }else{
miguelangel_2511 1:aa5df1878126 404 // Does nothing
miguelangel_2511 1:aa5df1878126 405 }
miguelangel_2511 1:aa5df1878126 406
miguelangel_2511 1:aa5df1878126 407 break;
miguelangel_2511 1:aa5df1878126 408 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 409 case Stepper_Expiration_Falling_Ramp:
miguelangel_2511 3:45299e7882b9 410 if(stepper_ramp_pulse_counter < SHORT_RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 411 stepper_ramp_pulse_counter++;
miguelangel_2511 3:45299e7882b9 412 stepper_pulse.period_us((int)expiration_period_short_rise_ramp_us[SHORT_RAMP_STEPS - stepper_ramp_pulse_counter]);
miguelangel_2511 1:aa5df1878126 413 stepper_pulse.write(0.5);
miguelangel_2511 3:45299e7882b9 414 }else if(stepper_ramp_pulse_counter == SHORT_RAMP_STEPS){
miguelangel_2511 1:aa5df1878126 415 stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
miguelangel_2511 1:aa5df1878126 416 stepper_pulse.write(0.5);
miguelangel_2511 1:aa5df1878126 417 stepper_ramp_pulse_counter++;
miguelangel_2511 1:aa5df1878126 418 }else{
miguelangel_2511 3:45299e7882b9 419 stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 420 stepper_pulse.write(0.0);
miguelangel_2511 1:aa5df1878126 421 stepper_driver_state = Stepper_Expiration_Finish;
miguelangel_2511 1:aa5df1878126 422 }
miguelangel_2511 1:aa5df1878126 423 break;
miguelangel_2511 1:aa5df1878126 424 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 425 case Stepper_Expiration_Finish:
miguelangel_2511 1:aa5df1878126 426 break;
miguelangel_2511 1:aa5df1878126 427 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 428 default:
miguelangel_2511 1:aa5df1878126 429 break;
miguelangel_2511 1:aa5df1878126 430 }
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