Miguel Angel Torres Lazaro / Mbed 2 deprecated Ventilador_Experimentos_03

pachas

Dependencies:   mbed QEI FastPWM

stepper_motor_driver.cpp@12:3bc2465b034a, 2020-05-15 (annotated)

Committer:
miguelangel_2511
Date:
Fri May 15 20:29:30 2020 +0000
Revision:
12:3bc2465b034a
Parent:
11:5cb7ae8bd831 
hola papu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
miguelangel_2511 10:b2d87404309a 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 11:5cb7ae8bd831 10 #include "pressure_sensors.h"
miguelangel_2511 12:3bc2465b034a 11 #include "electric_valves.h"
miguelangel_2511 0:9d0b9785d3d6 12
miguelangel_2511 1:aa5df1878126 13 /* Object definition */
miguelangel_2511 1:aa5df1878126 14 InterruptIn stepper_pulse_feedback(STEPPER_PULSE_FEEDBACK_PIN);
miguelangel_2511 1:aa5df1878126 15 FastPWM stepper_pulse(STEPPER_PULSE_PIN, -1);
miguelangel_2511 0:9d0b9785d3d6 16 DigitalOut stepper_en(STEPPER_ENABLE_PIN);
miguelangel_2511 0:9d0b9785d3d6 17 DigitalOut stepper_dir(STEPPER_DIRECTION_PIN);
miguelangel_2511 0:9d0b9785d3d6 18
miguelangel_2511 3:45299e7882b9 19
miguelangel_2511 1:aa5df1878126 20 /* Variable definition */
miguelangel_2511 0:9d0b9785d3d6 21
miguelangel_2511 3:45299e7882b9 22 volatile uint8_t stepper_parameters_update_flag = 0;
miguelangel_2511 1:aa5df1878126 23
miguelangel_2511 1:aa5df1878126 24
miguelangel_2511 1:aa5df1878126 25 // These parameters are configurer through the graphic interface */
miguelangel_2511 1:aa5df1878126 26 uint32_t inspiration_stepper_pulses_setpoint = STEPPER_DRIVER_INSPIRATION_PULSES_DEFAULT ;
miguelangel_2511 10:b2d87404309a 27
miguelangel_2511 10:b2d87404309a 28 float inspiration_stepper_pulse_period = 0.005;
miguelangel_2511 10:b2d87404309a 29 float expiration_stepper_pulse_period = 0.005;
miguelangel_2511 10:b2d87404309a 30 float calibration_stepper_pulse_period = 0.005;
miguelangel_2511 1:aa5df1878126 31
miguelangel_2511 5:bd5fc0510e7b 32
miguelangel_2511 1:aa5df1878126 33 // Volume setpoint (250mL, 300mL, 350ml ... 750ml) expressed in steps
miguelangel_2511 1:aa5df1878126 34 uint32_t volume_in_steps[VOLUME_SETPOINT_INDEX_LIMIT] = {
miguelangel_2511 11:5cb7ae8bd831 35 70, // 1600, //250 mL//720//47
miguelangel_2511 11:5cb7ae8bd831 36 75, // 1700, //300 mL//765
miguelangel_2511 11:5cb7ae8bd831 37 80, // 1800, //350 mL//810
miguelangel_2511 11:5cb7ae8bd831 38 85, // 1900, //400 mL//855
miguelangel_2511 11:5cb7ae8bd831 39 90, // 2000, //450 mL//900
miguelangel_2511 11:5cb7ae8bd831 40 95, // 2100, //500 mL//945
miguelangel_2511 11:5cb7ae8bd831 41 100, // 2200, //550 mL//990
miguelangel_2511 11:5cb7ae8bd831 42 105, // 2300, //600 mL //1035
miguelangel_2511 11:5cb7ae8bd831 43 110, // 2400, //650 mL//1080
miguelangel_2511 11:5cb7ae8bd831 44 115, // 2500, //700 mL//1125
miguelangel_2511 11:5cb7ae8bd831 45 120 // 2600 //750 mL//1170
miguelangel_2511 12:3bc2465b034a 46 };
miguelangel_2511 1:aa5df1878126 47
miguelangel_2511 1:aa5df1878126 48
miguelangel_2511 1:aa5df1878126 49 // Variables used in every state of the stepper motor driver */
miguelangel_2511 1:aa5df1878126 50 volatile uint32_t stepper_pulse_counter = 0;
miguelangel_2511 10:b2d87404309a 51 //volatile uint32_t stepper_ramp_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 52
miguelangel_2511 1:aa5df1878126 53 // State of the stepper motor
miguelangel_2511 1:aa5df1878126 54 volatile Stepper_Driver_State_t stepper_driver_state = Stepper_Stand_By;
miguelangel_2511 1:aa5df1878126 55
miguelangel_2511 1:aa5df1878126 56 /* Function definition */
miguelangel_2511 1:aa5df1878126 57
miguelangel_2511 1:aa5df1878126 58 /* Initialize the control signals for the stepper */
miguelangel_2511 1:aa5df1878126 59 void Stepper_Driver_Init(void){
miguelangel_2511 1:aa5df1878126 60 stepper_en = STEPPER_ENABLED;
miguelangel_2511 1:aa5df1878126 61 stepper_dir = TO_HOME;
miguelangel_2511 10:b2d87404309a 62 stepper_pulse.period(calibration_stepper_pulse_period);
miguelangel_2511 1:aa5df1878126 63 stepper_pulse.write(0.0f);
miguelangel_2511 1:aa5df1878126 64 stepper_pulse_feedback.fall(&Stepper_Driver_Pulse_Signal_Update);
miguelangel_2511 3:45299e7882b9 65 stepper_driver_state = Stepper_Inspiration_Finish;
miguelangel_2511 3:45299e7882b9 66 stepper_parameters_update_flag = 1;
miguelangel_2511 1:aa5df1878126 67 // Just for testing purposes
miguelangel_2511 1:aa5df1878126 68 //inspiration_stepper_pulses_setpoint = volume_in_steps[10];
miguelangel_2511 1:aa5df1878126 69 }
miguelangel_2511 1:aa5df1878126 70
miguelangel_2511 0:9d0b9785d3d6 71
miguelangel_2511 1:aa5df1878126 72 void Stepper_Go_Home_Position(void){
miguelangel_2511 1:aa5df1878126 73 // stepper_driver_state = Stepper_Driver_Home_Ramp;
miguelangel_2511 1:aa5df1878126 74
miguelangel_2511 1:aa5df1878126 75 //while(stepper_driver_state != Stepper_Driver_Stand_By){
miguelangel_2511 1:aa5df1878126 76 // Stepper_Driver_State_Machine();
miguelangel_2511 1:aa5df1878126 77 //}
miguelangel_2511 1:aa5df1878126 78 }
miguelangel_2511 1:aa5df1878126 79
miguelangel_2511 1:aa5df1878126 80
miguelangel_2511 3:45299e7882b9 81 void Stepper_Update_Parameters(void){
miguelangel_2511 3:45299e7882b9 82
miguelangel_2511 3:45299e7882b9 83 int8_t stepper_volume_index;
miguelangel_2511 3:45299e7882b9 84 float total_time, total_time_sec, insp_time_sec, exp_time_sec;
miguelangel_2511 3:45299e7882b9 85
miguelangel_2511 3:45299e7882b9 86 /* Update the value of the volume setpoint (expressed in steps) */
miguelangel_2511 3:45299e7882b9 87 stepper_volume_index = (volume_setpoint - VOLUME_SETPOINT_MINIMUM_VALUE) / VOLUME_SETPOINT_STEP;
miguelangel_2511 3:45299e7882b9 88 inspiration_stepper_pulses_setpoint = volume_in_steps[stepper_volume_index];
miguelangel_2511 3:45299e7882b9 89
miguelangel_2511 3:45299e7882b9 90 /* Update the value of the pulse periods in microseconds */
miguelangel_2511 3:45299e7882b9 91 total_time = inspiration_time + expiration_time;
miguelangel_2511 4:d42e1953243c 92 total_time_sec = 60.0 / resp_frequency;
miguelangel_2511 3:45299e7882b9 93 insp_time_sec = (total_time_sec * inspiration_time) / total_time;
miguelangel_2511 3:45299e7882b9 94 exp_time_sec = (total_time_sec * expiration_time) / total_time;
miguelangel_2511 4:d42e1953243c 95
miguelangel_2511 4:d42e1953243c 96 /* Calculate stepper pulse period for inspiration */
miguelangel_2511 10:b2d87404309a 97 inspiration_stepper_pulse_period = ((float)insp_time_sec) / ((float)inspiration_stepper_pulses_setpoint);
miguelangel_2511 4:d42e1953243c 98 /* Calculate stepper pulse period for expiration */
miguelangel_2511 10:b2d87404309a 99 expiration_stepper_pulse_period = ((float)exp_time_sec) / ((float)inspiration_stepper_pulses_setpoint);
miguelangel_2511 1:aa5df1878126 100
miguelangel_2511 11:5cb7ae8bd831 101 volume_ml = 0.0;
miguelangel_2511 11:5cb7ae8bd831 102
miguelangel_2511 5:bd5fc0510e7b 103 }
miguelangel_2511 5:bd5fc0510e7b 104
miguelangel_2511 5:bd5fc0510e7b 105
miguelangel_2511 5:bd5fc0510e7b 106
miguelangel_2511 1:aa5df1878126 107 void Stepper_Driver_State_Machine(void){
miguelangel_2511 0:9d0b9785d3d6 108
miguelangel_2511 1:aa5df1878126 109 switch(stepper_driver_state){
miguelangel_2511 10:b2d87404309a 110
miguelangel_2511 12:3bc2465b034a 111 case Stepper_Inspiration:
miguelangel_2511 12:3bc2465b034a 112
miguelangel_2511 12:3bc2465b034a 113 if(INSPIRATION_VALVE_IS_OPEN){
miguelangel_2511 12:3bc2465b034a 114 INSPIRATION_VALVE_STOP();
miguelangel_2511 12:3bc2465b034a 115 }
miguelangel_2511 1:aa5df1878126 116
miguelangel_2511 12:3bc2465b034a 117 if(EXPIRATION_VALVE_IS_CLOSED){
miguelangel_2511 12:3bc2465b034a 118 EXPIRATION_VALVE_STOP();
miguelangel_2511 12:3bc2465b034a 119 }
miguelangel_2511 12:3bc2465b034a 120
miguelangel_2511 1:aa5df1878126 121 break;
miguelangel_2511 1:aa5df1878126 122 // -----------------------------------------------------------------------
miguelangel_2511 10:b2d87404309a 123
miguelangel_2511 1:aa5df1878126 124 case Stepper_Inspiration_Finish:
miguelangel_2511 1:aa5df1878126 125
miguelangel_2511 12:3bc2465b034a 126 stepper_pulse_counter = 0;
miguelangel_2511 12:3bc2465b034a 127 stepper_dir = TO_HOME;
miguelangel_2511 12:3bc2465b034a 128 stepper_pulse.period(expiration_stepper_pulse_period);
miguelangel_2511 12:3bc2465b034a 129 stepper_pulse.write(0.5);
miguelangel_2511 12:3bc2465b034a 130 stepper_driver_state = Stepper_Expiration;
miguelangel_2511 12:3bc2465b034a 131 INSPIRATION_VALVE_CLOSE();
miguelangel_2511 12:3bc2465b034a 132 EXPIRATION_VALVE_OPEN();
miguelangel_2511 1:aa5df1878126 133 break;
miguelangel_2511 10:b2d87404309a 134 // -----------------------------------------------------------------------
miguelangel_2511 12:3bc2465b034a 135 case Stepper_Expiration:
miguelangel_2511 12:3bc2465b034a 136
miguelangel_2511 12:3bc2465b034a 137 if(peak_volume_flag == 1){// FOr test
miguelangel_2511 12:3bc2465b034a 138 pressure_sensor_display_update_flag = 1;
miguelangel_2511 12:3bc2465b034a 139 peak_volume_flag = 0;
miguelangel_2511 12:3bc2465b034a 140 } // For test
miguelangel_2511 12:3bc2465b034a 141
miguelangel_2511 12:3bc2465b034a 142 if(INSPIRATION_VALVE_IS_CLOSED){
miguelangel_2511 12:3bc2465b034a 143 INSPIRATION_VALVE_STOP();
miguelangel_2511 12:3bc2465b034a 144 }
miguelangel_2511 12:3bc2465b034a 145
miguelangel_2511 12:3bc2465b034a 146 if(EXPIRATION_VALVE_IS_OPEN){
miguelangel_2511 12:3bc2465b034a 147 EXPIRATION_VALVE_STOP();
miguelangel_2511 12:3bc2465b034a 148 }
miguelangel_2511 12:3bc2465b034a 149
miguelangel_2511 1:aa5df1878126 150 break;
miguelangel_2511 10:b2d87404309a 151 // -----------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 152 case Stepper_Expiration_Finish:
miguelangel_2511 3:45299e7882b9 153
miguelangel_2511 3:45299e7882b9 154 if(stepper_parameters_update_flag){
miguelangel_2511 3:45299e7882b9 155 stepper_parameters_update_flag = 0;
miguelangel_2511 3:45299e7882b9 156 Stepper_Update_Parameters();
miguelangel_2511 3:45299e7882b9 157 }
miguelangel_2511 1:aa5df1878126 158
miguelangel_2511 10:b2d87404309a 159 stepper_pulse_counter = 0;
miguelangel_2511 1:aa5df1878126 160 stepper_dir = TO_AIRBAG;
miguelangel_2511 10:b2d87404309a 161 stepper_pulse.period(inspiration_stepper_pulse_period);
miguelangel_2511 1:aa5df1878126 162 stepper_pulse.write(0.5);
miguelangel_2511 10:b2d87404309a 163 stepper_driver_state = Stepper_Inspiration;
miguelangel_2511 1:aa5df1878126 164
miguelangel_2511 12:3bc2465b034a 165 pressure_sensor_display_update_flag = 1;
miguelangel_2511 12:3bc2465b034a 166 volume_ml = 0; // Para pruebas
miguelangel_2511 12:3bc2465b034a 167
miguelangel_2511 12:3bc2465b034a 168 EXPIRATION_VALVE_CLOSE();
miguelangel_2511 12:3bc2465b034a 169 INSPIRATION_VALVE_OPEN();
miguelangel_2511 11:5cb7ae8bd831 170
miguelangel_2511 3:45299e7882b9 171 break;
miguelangel_2511 1:aa5df1878126 172 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 173 case Stepper_Limit_Sensor_Error:
miguelangel_2511 1:aa5df1878126 174 // Stop the system
miguelangel_2511 1:aa5df1878126 175 stepper_en = STEPPER_DISABLED;
miguelangel_2511 6:d38287621cca 176 // tft->locate(340,VALUES_ROW2_Y_POS);
miguelangel_2511 6:d38287621cca 177 // tft->printf("error");
miguelangel_2511 1:aa5df1878126 178 stepper_driver_state = Stepper_Stand_By;
miguelangel_2511 1:aa5df1878126 179 break;
miguelangel_2511 1:aa5df1878126 180 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 181 case Stepper_Stand_By:
miguelangel_2511 1:aa5df1878126 182
miguelangel_2511 1:aa5df1878126 183 break;
miguelangel_2511 1:aa5df1878126 184 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 185 default:
miguelangel_2511 1:aa5df1878126 186
miguelangel_2511 1:aa5df1878126 187 break;
miguelangel_2511 0:9d0b9785d3d6 188 }
miguelangel_2511 0:9d0b9785d3d6 189 }
miguelangel_2511 0:9d0b9785d3d6 190
miguelangel_2511 0:9d0b9785d3d6 191
miguelangel_2511 1:aa5df1878126 192 void Stepper_Driver_Pulse_Signal_Update(void){
miguelangel_2511 1:aa5df1878126 193
miguelangel_2511 1:aa5df1878126 194 // Increment the stepper pulse counter
miguelangel_2511 1:aa5df1878126 195 stepper_pulse_counter++;
miguelangel_2511 1:aa5df1878126 196
miguelangel_2511 10:b2d87404309a 197
miguelangel_2511 1:aa5df1878126 198 switch(stepper_driver_state){
miguelangel_2511 10:b2d87404309a 199
miguelangel_2511 1:aa5df1878126 200 case Stepper_Inspiration:
miguelangel_2511 10:b2d87404309a 201 if(stepper_pulse_counter >= inspiration_stepper_pulses_setpoint){
miguelangel_2511 10:b2d87404309a 202 stepper_driver_state = Stepper_Inspiration_Finish;
miguelangel_2511 10:b2d87404309a 203 stepper_pulse.write(0.0);
miguelangel_2511 1:aa5df1878126 204 }
miguelangel_2511 1:aa5df1878126 205 break;
miguelangel_2511 10:b2d87404309a 206 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 207 case Stepper_Inspiration_Finish:
miguelangel_2511 1:aa5df1878126 208 break;
miguelangel_2511 10:b2d87404309a 209 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 210 case Stepper_Expiration:
miguelangel_2511 1:aa5df1878126 211 if(buttons & (1 << LIMIT_SW_01)){
miguelangel_2511 10:b2d87404309a 212 stepper_driver_state = Stepper_Expiration_Finish;
miguelangel_2511 10:b2d87404309a 213 stepper_pulse.write(0.0);
miguelangel_2511 1:aa5df1878126 214 }else if(stepper_pulse_counter > STEPPER_DRIVER_MAX_PULSES){
miguelangel_2511 10:b2d87404309a 215 stepper_driver_state = Stepper_Limit_Sensor_Error;
miguelangel_2511 1:aa5df1878126 216 stepper_pulse.write(0.0);
miguelangel_2511 1:aa5df1878126 217 }else{
miguelangel_2511 1:aa5df1878126 218 // Does nothing
miguelangel_2511 1:aa5df1878126 219 }
miguelangel_2511 1:aa5df1878126 220
miguelangel_2511 1:aa5df1878126 221 break;
miguelangel_2511 10:b2d87404309a 222 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 223 case Stepper_Expiration_Finish:
miguelangel_2511 1:aa5df1878126 224 break;
miguelangel_2511 1:aa5df1878126 225 // --------------------------------------------------------------------
miguelangel_2511 1:aa5df1878126 226 default:
miguelangel_2511 1:aa5df1878126 227 break;
miguelangel_2511 1:aa5df1878126 228 }
miguelangel_2511 1:aa5df1878126 229
miguelangel_2511 1:aa5df1878126 230 }
miguelangel_2511 1:aa5df1878126 231
miguelangel_2511 1:aa5df1878126 232
miguelangel_2511 1:aa5df1878126 233
miguelangel_2511 1:aa5df1878126 234
miguelangel_2511 1:aa5df1878126 235