Miguel Angel Caycho
para el ventilador
Diff: stepper_motor_driver.cpp
- Revision:
- 1:aa5df1878126
- Parent:
- 0:9d0b9785d3d6
- Child:
- 2:6f618b905d4f
--- a/stepper_motor_driver.cpp Sat Apr 11 22:53:05 2020 +0000
+++ b/stepper_motor_driver.cpp Thu Apr 16 13:02:36 2020 +0000
@@ -3,30 +3,316 @@
#include "project_defines.h"
#include "stepper_motor_driver.h"
#include "buttons.h"
+#include "tft_interface.h"
+#include "FastPWM.h"
-PwmOut stepper_pulse(STEPPER_PULSE_PIN);
+/* 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);
+/* Variable definition */
-uint8_t stepper_motor_state = STEPPER_MOTOR_GO_HOME ;
+
+// ----------------------- 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.998f
+};
+
+#else
+ #error "Seleccionar un valor permitido para RAMP_STEPS: Puede ser 50 o 100"
+#endif
+
+
+const float i_e_ratio_stepper_table[I_E_RATIO_INDEX_LIMIT][2]={
+ {1.0,2.0},
+ {1.0,2.5},
+ {1.0,3.0}
+ };
+
+// -----------------------------------------------------------------------------
+
+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];
+
+// 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] = {
+ 1600, //250 mL
+ 1700, //300 mL
+ 1800, //350 mL
+ 1900, //400 mL
+ 2000, //450 mL
+ 2100, //500 mL
+ 2200, //550 mL
+ 2300, //600 mL
+ 2400, //650 mL
+ 2500, //700 mL
+ 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;
+ // Just for testing purposes
+ //inspiration_stepper_pulses_setpoint = volume_in_steps[10];
+}
+
-void Stepper_Motor_Go_Home(void){
+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_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;
+ }
+
+}
+
+
+void Stepper_Driver_State_Machine(void){
- stepper_en = 1;
- stepper_dir = 0;
-
- if(buttons & (1 << LIMIT_SW_01)){
- stepper_motor_state = STEPPER_MOTOR_GO_AHEAD;
+ 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:
+
+ 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_Motor_Go_Ahead(void){
- stepper_en = 1;
- stepper_dir = 1;
- wait_ms(1200);
- stepper_motor_state = STEPPER_MOTOR_GO_HOME;
-}
\ No newline at end of file
+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 < RAMP_STEPS){
+ stepper_ramp_pulse_counter++;
+ stepper_pulse.period_us((int)expiration_period_rise_ramp_us[RAMP_STEPS - stepper_ramp_pulse_counter]);
+ stepper_pulse.write(0.5);
+ }else if(stepper_ramp_pulse_counter == RAMP_STEPS){
+ stepper_pulse.period_us((int)STEPPER_START_BRAKE_PERIOD_US);
+ stepper_pulse.write(0.5);
+ stepper_ramp_pulse_counter++;
+ }else{
+ stepper_pulse.write(0.0);
+ stepper_driver_state = Stepper_Expiration_Finish;
+ }
+ break;
+ // --------------------------------------------------------------------
+ case Stepper_Expiration_Finish:
+ break;
+ // --------------------------------------------------------------------
+ default:
+ break;
+ }
+
+}
+
+
+
+
+
\ No newline at end of file