![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
para el ventilador
stepper_motor_driver.cpp
- Committer:
- miguelangel_2511
- Date:
- 2020-04-18
- Revision:
- 4:d42e1953243c
- Parent:
- 3:45299e7882b9
- Child:
- 5:bd5fc0510e7b
File content as of revision 4:d42e1953243c:
#include "math.h" #include "mbed.h" #include "project_defines.h" #include "stepper_motor_driver.h" #include "buttons.h" #include "tft_interface.h" #include "FastPWM.h" #include "ventilator.h" /* Object definition */ InterruptIn stepper_pulse_feedback(STEPPER_PULSE_FEEDBACK_PIN); FastPWM stepper_pulse(STEPPER_PULSE_PIN, -1); DigitalOut stepper_en(STEPPER_ENABLE_PIN); DigitalOut stepper_dir(STEPPER_DIRECTION_PIN); // For testing pusposes only Serial pc (USBTX, USBRX); /* Variable definition */ volatile uint8_t stepper_parameters_update_flag = 0; // ----------------------- Tables used for calculations ------------------------ #if RAMP_STEPS == 50 const float sigma_ramp[RAMP_STEPS] = { 0.002, 0.003, 0.004, 0.005, 0.006, 0.008, 0.010, 0.013, 0.017, 0.021, 0.027, 0.034, 0.042, 0.053, 0.067, 0.083, 0.103, 0.128, 0.157, 0.192, 0.231, 0.277, 0.327, 0.382, 0.440, 0.500, 0.560, 0.618, 0.673, 0.723, 0.769, 0.808, 0.843, 0.872, 0.897, 0.917, 0.933, 0.947, 0.958, 0.966, 0.973, 0.979, 0.983, 0.987, 0.990, 0.992, 0.994, 0.995, 0.996, 0.997 }; #elif RAMP_STEPS == 100 const float sigma_ramp[RAMP_STEPS] = { 0.002, 0.003, 0.003, 0.004, 0.004, 0.005, 0.005, 0.006, 0.006, 0.007, 0.008, 0.009, 0.011, 0.012, 0.013, 0.015, 0.017, 0.019, 0.021, 0.024, 0.027, 0.031, 0.034, 0.039, 0.043, 0.049, 0.055, 0.061, 0.069, 0.077, 0.086, 0.096, 0.107, 0.119, 0.133, 0.147, 0.163, 0.180, 0.199, 0.219, 0.240, 0.263, 0.287, 0.313, 0.339, 0.367, 0.396, 0.425, 0.455, 0.485, 0.515, 0.545, 0.575, 0.604, 0.633, 0.661, 0.687, 0.713, 0.737, 0.760, 0.781, 0.801, 0.820, 0.837, 0.853, 0.867, 0.881, 0.893, 0.904, 0.914, 0.923, 0.931, 0.939, 0.945, 0.951, 0.957, 0.961, 0.966, 0.969, 0.973, 0.976, 0.979, 0.981, 0.983, 0.985, 0.987, 0.988, 0.989, 0.991, 0.992, 0.993, 0.994, 0.994, 0.995, 0.995, 0.996, 0.996, 0.997, 0.997, 0.998 }; #else #error "Seleccionar un valor permitido para RAMP_STEPS: Puede ser 50 o 100" #endif const float short_sigma_ramp[SHORT_RAMP_STEPS] = { 0.002, 0.004, 0.007, 0.011, 0.018, 0.029, 0.047, 0.076, 0.119, 0.182, 0.269, 0.378, 0.500, 0.622, 0.731, 0.818, 0.881, 0.924, 0.953, 0.971, 0.982, 0.989, 0.993, 0.996, 0.998 }; // ----------------------------------------------------------------------------- uint32_t calibration_period_rise_ramp_us[RAMP_STEPS]; uint32_t inspiration_period_rise_ramp_us[RAMP_STEPS]; uint32_t expiration_period_rise_ramp_us[RAMP_STEPS]; uint32_t expiration_period_short_rise_ramp_us[SHORT_RAMP_STEPS]; // These parameters are configurer through the graphic interface */ uint32_t inspiration_stepper_pulses_setpoint = STEPPER_DRIVER_INSPIRATION_PULSES_DEFAULT ; //uint32_t expiration_stepper_pulses_setpoint = STEPPER_DRIVER_MAX_PULSES ; // Not used in this version uint32_t inspiration_stepper_pulse_period_us = STEPPER_DRIVER_DEFAULT_PULSE_PERIOD_US; uint32_t expiration_stepper_pulse_period_us = STEPPER_DRIVER_DEFAULT_PULSE_PERIOD_US; uint32_t calibration_stepper_pulse_period_us = STEPPER_DRIVER_DEFAULT_PULSE_PERIOD_US; // Volume setpoint (250mL, 300mL, 350ml ... 750ml) expressed in steps uint32_t volume_in_steps[VOLUME_SETPOINT_INDEX_LIMIT] = { 720, // 1600, //250 mL 765, // 1700, //300 mL 810, // 1800, //350 mL 855, // 1900, //400 mL 900, // 2000, //450 mL 945, // 2100, //500 mL 990, // 2200, //550 mL 1035, // 2300, //600 mL 1080, // 2400, //650 mL 1125, // 2500, //700 mL 1170 // 2600 //750 mL }; // Variables used in every state of the stepper motor driver */ volatile uint32_t stepper_pulse_counter = 0; volatile uint32_t stepper_ramp_pulse_counter = 0; // State of the stepper motor volatile Stepper_Driver_State_t stepper_driver_state = Stepper_Stand_By; /* Function definition */ /* Initialize the control signals for the stepper */ void Stepper_Driver_Init(void){ stepper_en = STEPPER_ENABLED; stepper_dir = TO_HOME; stepper_pulse.period_us((int)calibration_stepper_pulse_period_us); stepper_pulse.write(0.0f); stepper_pulse_feedback.fall(&Stepper_Driver_Pulse_Signal_Update); stepper_driver_state = Stepper_Inspiration_Finish; stepper_parameters_update_flag = 1; // Just for testing purposes //inspiration_stepper_pulses_setpoint = volume_in_steps[10]; } void Stepper_Go_Home_Position(void){ // stepper_driver_state = Stepper_Driver_Home_Ramp; //while(stepper_driver_state != Stepper_Driver_Stand_By){ // Stepper_Driver_State_Machine(); //} } void Stepper_Update_Parameters(void){ int8_t stepper_volume_index; uint32_t regular_steps; float total_time, total_time_sec, insp_time_sec, exp_time_sec; float start_brake_pulse_period_sec = (1.0 / STEPPER_START_BRAKE_FREQUENCY_HZ); float inspiration_stepper_pulse_period_sec, expiration_stepper_pulse_period_sec; float a_coeff, b_coeff, c_coeff, determ; /* Update the value of the volume setpoint (expressed in steps) */ stepper_volume_index = (volume_setpoint - VOLUME_SETPOINT_MINIMUM_VALUE) / VOLUME_SETPOINT_STEP; inspiration_stepper_pulses_setpoint = volume_in_steps[stepper_volume_index]; /* Update the value of the pulse periods in microseconds */ total_time = inspiration_time + expiration_time; total_time_sec = 60.0 / resp_frequency; insp_time_sec = (total_time_sec * inspiration_time) / total_time; exp_time_sec = (total_time_sec * expiration_time) / total_time; /* Calculate stepper pulse period for inspiration */ regular_steps = inspiration_stepper_pulses_setpoint - (2 * RAMP_STEPS); a_coeff = (float)regular_steps; b_coeff = (start_brake_pulse_period_sec*(4.0*RAMP_STEPS + regular_steps)) - insp_time_sec; c_coeff = -(start_brake_pulse_period_sec * insp_time_sec); determ = ((b_coeff * b_coeff) - (4 * a_coeff * c_coeff)); inspiration_stepper_pulse_period_sec = (sqrt(determ) - b_coeff) / (2.0*a_coeff); inspiration_stepper_pulse_period_us = (uint32_t)(1000000.0 * inspiration_stepper_pulse_period_sec); /* Calculate stepper pulse period for expiration */ regular_steps = inspiration_stepper_pulses_setpoint - (RAMP_STEPS + SHORT_RAMP_STEPS); a_coeff = (float)regular_steps; b_coeff = (start_brake_pulse_period_sec*((2.0*(RAMP_STEPS + SHORT_RAMP_STEPS)) + regular_steps)) - exp_time_sec; c_coeff = -(start_brake_pulse_period_sec * exp_time_sec); determ = ((b_coeff * b_coeff) - (4 * a_coeff * c_coeff)); expiration_stepper_pulse_period_sec = (sqrt(determ) - b_coeff) / (2.0*a_coeff); expiration_stepper_pulse_period_us = (uint32_t)(1000000.0 * expiration_stepper_pulse_period_sec); /* Just for testing purposes */ pc.printf("Cantidad de pulsos: %d \n\n\r", inspiration_stepper_pulses_setpoint); pc.printf("Frecuencia de arranque: %d Hz\n\r", STEPPER_START_BRAKE_FREQUENCY_HZ); pc.printf("Frecuencia de pulsos de ida: %f Hz\n\r", 1.0 / inspiration_stepper_pulse_period_sec); pc.printf("Periodo de pulsos de ida en us: %d us\n\r", inspiration_stepper_pulse_period_us); pc.printf("Frecuencia de pulsos de vuelta: %f Hz\n\r", 1.0 / expiration_stepper_pulse_period_us); pc.printf("Periodo de pulsos de vuelta en us: %d us\n\r", expiration_stepper_pulse_period_us); } void Stepper_Calculate_Period_Ramp(void){ float initial_frequency, final_frequency, frequency_ramp_range; float frequency_i_hz, period_i_us; uint8_t i; /* Period rise ramp for inspiration */ initial_frequency = (float)STEPPER_START_BRAKE_FREQUENCY_HZ; final_frequency = 1000000.0 / (float)inspiration_stepper_pulse_period_us; frequency_ramp_range = final_frequency - initial_frequency; for(i = 0; i < RAMP_STEPS; i++){ frequency_i_hz = initial_frequency + (frequency_ramp_range * sigma_ramp[i]); period_i_us = 1000000.0 / frequency_i_hz; inspiration_period_rise_ramp_us[i] = (uint32_t)period_i_us; } /* Period rise ramp for expiration */ initial_frequency = (float)STEPPER_START_BRAKE_FREQUENCY_HZ; final_frequency = 1000000.0 / (float)expiration_stepper_pulse_period_us; frequency_ramp_range = final_frequency - initial_frequency; for(i = 0; i < RAMP_STEPS; i++){ frequency_i_hz = initial_frequency + (frequency_ramp_range * sigma_ramp[i]); period_i_us = 1000000.0 / frequency_i_hz; expiration_period_rise_ramp_us[i] = (uint32_t)period_i_us; } /* Period short rise ramp for expiration */ initial_frequency = (float)STEPPER_START_BRAKE_FREQUENCY_HZ; final_frequency = 1000000.0 / (float)expiration_stepper_pulse_period_us; frequency_ramp_range = final_frequency - initial_frequency; for(i = 0; i < SHORT_RAMP_STEPS; i++){ frequency_i_hz = initial_frequency + (frequency_ramp_range * sigma_ramp[i]); period_i_us = 1000000.0 / frequency_i_hz; expiration_period_short_rise_ramp_us[i] = (uint32_t)period_i_us; } } void Stepper_Driver_State_Machine(void){ switch(stepper_driver_state){ case Stepper_Inspiration_Rising_Ramp: break; // ----------------------------------------------------------------------- case Stepper_Inspiration: break; // ----------------------------------------------------------------------- case Stepper_Inspiration_Falling_Ramp: break; // ----------------------------------------------------------------------- case Stepper_Inspiration_Finish: stepper_pulse_counter = 0; stepper_ramp_pulse_counter = 0; stepper_dir = TO_HOME; stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US); stepper_pulse.write(0.5); stepper_driver_state = Stepper_Expiration_Rising_Ramp; break; // ----------------------------------------------------------------------- case Stepper_Expiration_Rising_Ramp: break; // ----------------------------------------------------------------------- case Stepper_Expiration: break; // ----------------------------------------------------------------------- case Stepper_Expiration_Falling_Ramp: break; // ----------------------------------------------------------------------- case Stepper_Expiration_Finish: if(stepper_parameters_update_flag){ stepper_parameters_update_flag = 0; Stepper_Update_Parameters(); Stepper_Calculate_Period_Ramp(); } stepper_pulse_counter = 0; stepper_ramp_pulse_counter = 0; stepper_dir = TO_AIRBAG; stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US); stepper_pulse.write(0.5); stepper_driver_state = Stepper_Inspiration_Rising_Ramp; break; // -------------------------------------------------------------------- case Stepper_Limit_Sensor_Error: // Stop the system stepper_en = STEPPER_DISABLED; tft->locate(340,VALUES_ROW2_Y_POS); tft->printf("error"); stepper_driver_state = Stepper_Stand_By; break; // -------------------------------------------------------------------- case Stepper_Stand_By: break; // -------------------------------------------------------------------- default: break; } } void Stepper_Driver_Pulse_Signal_Update(void){ // Increment the stepper pulse counter stepper_pulse_counter++; // Increment the proper counter switch(stepper_driver_state){ /* Increase the speed gradually */ case Stepper_Inspiration_Rising_Ramp: if(stepper_ramp_pulse_counter < RAMP_STEPS){ stepper_pulse.period_us((int)inspiration_period_rise_ramp_us[stepper_ramp_pulse_counter]); stepper_pulse.write(0.5); stepper_ramp_pulse_counter++; }else{ stepper_pulse.period_us((int)inspiration_stepper_pulse_period_us); stepper_pulse.write(0.5); stepper_ramp_pulse_counter = 0; stepper_driver_state = Stepper_Inspiration; } break; // -------------------------------------------------------------------- case Stepper_Inspiration: if(stepper_pulse_counter >= (inspiration_stepper_pulses_setpoint - RAMP_STEPS)){ stepper_driver_state = Stepper_Inspiration_Falling_Ramp; } break; // -------------------------------------------------------------------- case Stepper_Inspiration_Falling_Ramp: if(stepper_ramp_pulse_counter < RAMP_STEPS){ stepper_ramp_pulse_counter++; stepper_pulse.period_us((int)inspiration_period_rise_ramp_us[RAMP_STEPS - stepper_ramp_pulse_counter]); stepper_pulse.write(0.5); }else if(stepper_ramp_pulse_counter == RAMP_STEPS){ stepper_ramp_pulse_counter++; stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US); stepper_pulse.write(0.5); }else{ stepper_ramp_pulse_counter = 0; stepper_pulse.write(0.0); stepper_driver_state = Stepper_Inspiration_Finish; } break; // -------------------------------------------------------------------- case Stepper_Inspiration_Finish: break; // -------------------------------------------------------------------- case Stepper_Expiration_Rising_Ramp: if(stepper_ramp_pulse_counter < RAMP_STEPS){ stepper_pulse.period_us((int)expiration_period_rise_ramp_us[stepper_ramp_pulse_counter]); stepper_pulse.write(0.5); stepper_ramp_pulse_counter++; }else{ stepper_pulse.period_us((int)expiration_stepper_pulse_period_us); stepper_pulse.write(0.5); stepper_ramp_pulse_counter = 0; stepper_driver_state = Stepper_Expiration; } break; // -------------------------------------------------------------------- case Stepper_Expiration: if(buttons & (1 << LIMIT_SW_01)){ stepper_driver_state = Stepper_Expiration_Falling_Ramp; }else if(stepper_pulse_counter > STEPPER_DRIVER_MAX_PULSES){ stepper_pulse.write(0.0); stepper_driver_state = Stepper_Limit_Sensor_Error; }else{ // Does nothing } break; // -------------------------------------------------------------------- case Stepper_Expiration_Falling_Ramp: if(stepper_ramp_pulse_counter < SHORT_RAMP_STEPS){ stepper_ramp_pulse_counter++; stepper_pulse.period_us((int)expiration_period_short_rise_ramp_us[SHORT_RAMP_STEPS - stepper_ramp_pulse_counter]); stepper_pulse.write(0.5); }else if(stepper_ramp_pulse_counter == SHORT_RAMP_STEPS){ stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US); stepper_pulse.write(0.5); stepper_ramp_pulse_counter++; }else{ stepper_ramp_pulse_counter = 0; stepper_pulse.write(0.0); stepper_driver_state = Stepper_Expiration_Finish; } break; // -------------------------------------------------------------------- case Stepper_Expiration_Finish: break; // -------------------------------------------------------------------- default: break; } }