File content as of revision 6:d38287621cca:

//#include "math.h"
#include "mbed.h"
#include "project_defines.h"
#include "stepper_motor_driver.h"
#include "buttons.h"
#include "nextion_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);


/* 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 inspiration_stepper_pulse_period_sec, expiration_stepper_pulse_period_sec;
    float ramp_max_pulse_freq_hz, ramp_time_sec, short_ramp_time_sec;
    float inspiration_stepper_regular_frequency_hz;
    
    /* 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);
    /* Asume an initial value for the max frrquency of the ramp */
    ramp_max_pulse_freq_hz = TUNING_FACTOR * (((float)inspiration_stepper_pulses_setpoint) / insp_time_sec);
    /* Calculate the period ramp and the total time spent in doing the ramps */
    ramp_time_sec = Stepper_Calculate_Insp_Period_Ramp(ramp_max_pulse_freq_hz);
    inspiration_stepper_regular_frequency_hz = ((float)regular_steps) / (insp_time_sec - (2*ramp_time_sec));
    
    if(inspiration_stepper_regular_frequency_hz > STEPPER_MAX_FREQUENCY_HZ){
        /* Calculate the period ramp and the total time spent in doing the ramps */
        ramp_max_pulse_freq_hz = (float)STEPPER_MAX_FREQUENCY_HZ;
        ramp_time_sec = Stepper_Calculate_Insp_Period_Ramp(ramp_max_pulse_freq_hz);
        inspiration_stepper_regular_frequency_hz = ((float)regular_steps) / (insp_time_sec - (2*ramp_time_sec));
    }
    
    inspiration_stepper_pulse_period_us = (uint32_t)(1000000.0 / inspiration_stepper_regular_frequency_hz);
    
    /* Calculate stepper pulse period for expiration */
    regular_steps = inspiration_stepper_pulses_setpoint - (RAMP_STEPS + SHORT_RAMP_STEPS);
    ramp_max_pulse_freq_hz = TUNING_FACTOR * (((float)inspiration_stepper_pulses_setpoint) / exp_time_sec);
    ramp_time_sec = Stepper_Calculate_Exp_Period_Ramp(ramp_max_pulse_freq_hz);
    short_ramp_time_sec = Stepper_Calculate_Exp_Short_Period_Ramp(ramp_max_pulse_freq_hz);
    expiration_stepper_pulse_period_sec =  (exp_time_sec - (ramp_time_sec + short_ramp_time_sec)) / (float)regular_steps;
    
    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);
    */
}


float Stepper_Calculate_Insp_Period_Ramp(float max_freq){
      
    float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz;
    float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec;
    uint8_t i;
    
    /* Period rise ramp for inspiration */
    initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
    final_frequency_hz = max_freq;
    frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz;
    
    ramp_time_sec = 0;
    
    for(i = 0; i < RAMP_STEPS; i++){
        frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * sigma_ramp[i]);
        period_i_sec = 1.0 / frequency_i_hz;
        period_i_us = 1000000.0 * period_i_sec;
        inspiration_period_rise_ramp_us[i] = (uint32_t)period_i_us;
        ramp_time_sec += period_i_sec;
    }
    
    return ramp_time_sec;
}


float Stepper_Calculate_Exp_Period_Ramp(float max_freq){

    float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz;
    float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec;
    uint8_t i;
    
    /* Period rise ramp for inspiration */
    initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
    final_frequency_hz = max_freq;
    frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz;
    
    ramp_time_sec = 0;
    
     for(i = 0; i < RAMP_STEPS; i++){
        frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * sigma_ramp[i]);
        period_i_sec = 1.0 / frequency_i_hz;
        period_i_us = 1000000.0 * period_i_sec;
        expiration_period_rise_ramp_us[i] = (uint32_t)period_i_us;
        ramp_time_sec += period_i_sec;
    }   

    return ramp_time_sec;
}



float Stepper_Calculate_Exp_Short_Period_Ramp(float max_freq){

    float initial_frequency_hz, final_frequency_hz, frequency_ramp_range_hz;
    float frequency_i_hz, period_i_sec, period_i_us, ramp_time_sec;
    uint8_t i;
    
    /* Period rise ramp for inspiration */
    initial_frequency_hz = (float)STEPPER_START_BRAKE_FREQUENCY_HZ;
    final_frequency_hz = max_freq;
    frequency_ramp_range_hz = final_frequency_hz - initial_frequency_hz;
    
    ramp_time_sec = 0;
    
     for(i = 0; i < SHORT_RAMP_STEPS; i++){
        frequency_i_hz = initial_frequency_hz + (frequency_ramp_range_hz * short_sigma_ramp[i]);
        period_i_sec = 1.0 / frequency_i_hz;
        period_i_us = 1000000.0 * period_i_sec;
        expiration_period_short_rise_ramp_us[i] = (uint32_t)period_i_us;
        ramp_time_sec += period_i_sec;
    }   

    return ramp_time_sec;
}





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;                               
        }
     
}