File content as of revision 4:7e4bb0c29d3b:

#include "mbed.h"
#include "MPU6050.h"
#ifdef __SERIAL_DEBUG__ 
#include "USBSerial.h"
#include "STM32_USB48MHz.h"
#endif
#include "Watchdog.h"
#include "LightSaber.h"
#include "LightSaber_sounds.h"



// Light Controls: (Timer 3, ch 1-3)
PwmOut Red_LED(PA_6);
PwmOut Green_LED(PA_7);
PwmOut Blue_LED(PB_0);

//Speaker drive (Timer 2 ch 2)
PwmOut Speaker_OUT(PA_1);

//Color change button
DigitalIn Color_Button(PA_3, PullUp);

//startup config IOs
DigitalIn Startup_Config_IN1(PB_5, PullDown);
DigitalIn Startup_Config_IN2(PB_7, PullDown);
DigitalOut Startup_Config_OUT(PB_6, 0);

//Spare Buttons
//DigitalIn Spare1_Button(PB_12);
//DigitalIn Spare2_Button(PB_13);
//DigitalIn Spare3_Button(PB_14);

//Watchdog
//Watchdog wd;

bool color_button_released = TRUE;
volatile int Color_Button_Count = 0;

//sounds
volatile int sound_count = 1;
volatile int sound_line = 0;
volatile int next_sound_time = 0;

//Battery voltage monitor
AnalogIn V_bat(PA_2);

//Ticker
Ticker button_ticker;

//Set up I2C, (SDA,SCL)
//I2C MPU_i2c(PB_9, PB_8); //defined in MPU6050.h...

//MPU interrupt line
DigitalIn MPU_Intr(PB_1);

//MPU6050 data rate 
int delt_t = 0; // used to control display output rate

extern float sum;
extern uint32_t sumCount;

MPU6050 mpu6050;
   
Timer t;

void button_inth()
{
    int Color_Button_state;
    
    Color_Button_state = Color_Button.read(); //pressed = 0, released = 1
    
    if (color_button_released) {
        Color_Button_state = (~Color_Button_state) & 0x01;
    }
    Color_Button_Count = Color_Button_state*(Color_Button_Count + 1);
    
}

int main()
{
    bool motion_is_init = FALSE;
    bool new_data = FALSE;
    
    int current_color;
    int light_intensity = NORM_LIGHT_INTENS;
    
    int v_bat_avg_mem[VBAT_AVG_LEN];
    int v_bat_avg_value;
    int v_bat_avg_pointer = 0;
    bool v_bat_ready = FALSE;
    bool vbat_low_flag = FALSE;
    int t_vbat_count = 0;
    int t_vbat_start = 0;
    
    MPU_data_type MPU_data[MPU_BUFFER_LEN];
    MPU_data_type MPU_avg_data;
    int mpu_pointer = 0;
    int mpu_data_count = 0;
    bool accel_thr_crossed = FALSE;
    bool ypr_thr_crossed = FALSE;
    int count = 0;
    
    int sound_to_play;
    bool sound_is_playing_flag = FALSE;
    
    int temp_diff;
    
    int i = 0;
#ifdef __SERIAL_DEBUG__   
    STM32_HSI_USB48MHz(); // HSI,USB48MHz,SYSCLK32MHz
    
    // serial port for debug etc.
    USBSerial serial;

    //USBSerial serial;

    serial.printf("Initializing main");
#endif

//initialize LEDs : set rate, intensity, and startup color (according to jumper settings)
    
    init_color(&current_color,light_intensity); 

//Set up I2C
    MPU6050_set_I2C_freq(300000);  // use fast (400 kHz) I2C

// start "clock"
    t.start();
    
// play startup sound    
    sound_to_play = STARTUP_SOUND;
    sound_line = 0;
    sound_count = 1;
    next_sound_time = t.read_ms();
    sound_is_playing_flag = TRUE;
    
// initialize the motion sensor
    motion_is_init = mpu6050.motion_sensor_init();
    
    button_ticker.attach(&button_inth, 0.025);
        
    wait_ms(100); //from MPU example (1000)
    
//Initialize watchdog with a 2 second interval
 //   wd.Configure(2.0);       

//main loop
    while(1) {
        
              
    
        if (vbat_low_flag) //low battery
        {
            light_intensity = LOW_LIGHT_INTENS; //lower light intensity save some battery
            if (t_vbat_start == 0) //starting a new cycle
            {
                t_vbat_start = t.read_ms();
                if (t_vbat_count < 3) //flash only in first 3 cycles
                {
                    change_color(RED , light_intensity);
                }
            } else if ( ((t.read_ms() - t_vbat_start) > 1000) && (t_vbat_count < 3)) // dark period and flash only in first 3 cycles
            {
                change_color(RED, 0);
            } else // after first 3 cycles return to normal operation (with lower light intensity)
            {
                change_color((color_type) current_color, light_intensity);
            }
            if ( (t.read_ms() - t_vbat_start) > 2000) //end of cycle
            {
                t_vbat_start = 0;
                t_vbat_count ++;
            }
            if (t_vbat_count > 30) // once a minute, starting over
            {
                t_vbat_count  = 0;
            }
            
        }
        //battery ok or between low bat flashes, check if button was pressed or released
        if (!vbat_low_flag || (t_vbat_count >=3))
        {        
            if ( (Color_Button_Count >= BUTTON_PRESS_THR) && color_button_released )
            {
                __disable_irq();
                color_button_released = FALSE;
                Color_Button_Count = 0;
                __enable_irq(); 
                current_color++;
                if (current_color >= NUM_OF_COLORS)
                {
                    current_color = BLUE;
                }
#ifdef __SERIAL_DEBUG__
                serial.printf("Color Button pressed");
#endif
                change_color((color_type) current_color, light_intensity);
            }   
        
            if ((Color_Button_Count >= BUTTON_RELEASE_THR) && !color_button_released)
            {
                __disable_irq();
                color_button_released = TRUE;
                Color_Button_Count = 0;
                __enable_irq(); 
#ifdef __SERIAL_DEBUG__
                serial.printf("Color Button released");
#endif
            }
        }
            
        
        
        if (motion_is_init)
        {
            new_data = mpu6050.motion_update_data(&MPU_data[mpu_pointer], t.read_us());
        }
        
        if (new_data)
        {
            
            if ((mpu_data_count > MPU_DATA_STABLE) && !sound_is_playing_flag )
            {
                //calculate new average
                mpu_calc_avg(MPU_data, mpu_pointer, &MPU_avg_data, MPU_AVG_LEN);
                
                //check if any thresholds are crossed
                //acceleration
                if ((abs(MPU_avg_data.ax - MPU_data[mpu_pointer].ax) > ACCEL_THR) 
                        || (abs(MPU_avg_data.ay - MPU_data[mpu_pointer].ay) > ACCEL_THR) 
                        || (abs(MPU_avg_data.az - MPU_data[mpu_pointer].az) > ACCEL_THR))
                {
                    accel_thr_crossed = TRUE;
                }
                // YPR
                temp_diff = abs(MPU_avg_data.yaw - MPU_data[mpu_pointer].yaw);
                if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
                {
                    ypr_thr_crossed = TRUE;
                }
                
                temp_diff = abs(MPU_avg_data.pitch - MPU_data[mpu_pointer].pitch);
                if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
                {
                    ypr_thr_crossed = TRUE;
                }
                
                temp_diff = abs(MPU_avg_data.roll - MPU_data[mpu_pointer].roll);
                if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
                {
                    ypr_thr_crossed = TRUE;
                }
                
                if (accel_thr_crossed)
                {
                    //play clash sound
                    sound_to_play = CLASH_SOUND;
                    sound_line = 0;
                    sound_count = 1;
                    next_sound_time = t.read_ms();
                    sound_is_playing_flag = TRUE;
                
                }
                if (ypr_thr_crossed && !accel_thr_crossed)
                {
                    //play movement sound
                    sound_to_play = MOVEMENT_SOUND;
                    sound_line = 0;
                    sound_count = 1;
                    next_sound_time = t.read_ms();
                    sound_is_playing_flag = TRUE;
                }
                
                accel_thr_crossed = FALSE;
                ypr_thr_crossed = FALSE;
                                
            } else {
                mpu_data_count++;
            }
            
            mpu_pointer++;
            
            if (mpu_pointer >= MPU_BUFFER_LEN)
            {
                mpu_pointer = 0;
            }
            
            new_data = FALSE;   
        }
        
        delt_t = t.read_ms() - count;
        if (delt_t > 1000) 
        { // update serial port once per second independent of read rate
            
#ifdef __SERIAL_DEBUG__
            //serial.printf("Yaw, Pitch, Roll: %f %f %f\n\r", (float) (yaw / 16384.0f), (float) (pitch/16384.0f), (float) (roll/16384.0f));
            serial.printf("Yaw, Pitch, Roll: %i %i %i\n\r", (int)(yaw * 10), (int)(pitch * 10), (int)(roll * 10));
            serial.printf("Yaw, Pitch, Roll (average): %i %i %i\n\r", MPU_avg_data.yaw, MPU_avg_data.pitch, MPU_avg_data.roll);
            
            serial.printf("Ax, Ay, Az : %i %i %i\n\r", (int)(ax * 1000), (int)(ay * 1000), (int)(az * 1000));
            serial.printf("Ax, Ay, Az (average): %i %i %i\n\r", MPU_avg_data.ax , MPU_avg_data.ay, MPU_avg_data.az);
            
            serial.printf("average rate = %f\n\r", (float) sumCount/sum);
            
            if (sound_is_playing_flag)
            {
                serial.printf("Sound is playing!  \n\r", (float) sumCount/sum);
            }
#endif           
            count = t.read_ms();
            sum = 0;
            sumCount = 0;
                      
// Battery voltage monitor
            v_bat_avg_mem[v_bat_avg_pointer] = V_bat.read_u16();
            v_bat_avg_pointer++;
            if (v_bat_avg_pointer >= VBAT_AVG_LEN)
            {
                v_bat_avg_pointer = 0;
                v_bat_ready = TRUE;
            }
            if (v_bat_ready)
            {
                //calculate the average
                v_bat_avg_value = 0;
                for (i = 0 ; i < VBAT_AVG_LEN ; i++)
                {
                    v_bat_avg_value += v_bat_avg_mem[i];
                }
                
                v_bat_avg_value = v_bat_avg_value/VBAT_AVG_LEN;
#ifdef __SERIAL_DEBUG__                
                serial.printf("Battery average voltage: %i \n\r", v_bat_avg_value);
#endif               
                //check voltage
                if (v_bat_avg_value < VBAT_THR)
                {
                    vbat_low_flag = TRUE;
                } else if ((vbat_low_flag == TRUE) && (v_bat_avg_value > (VBAT_THR+VBAT_HYST)))
                {
                    vbat_low_flag = FALSE;
                }
            }
            
            
        }
// Play sound
        if ((t.read_ms() >= next_sound_time) && sound_is_playing_flag ) 
        {
            switch (sound_to_play) {

                case STARTUP_SOUND:
                    sound_is_playing_flag = sound_player(startup_sound_table, STARTUP_TBL_N_LINES);
                    break;
                case MOVEMENT_SOUND:
                    sound_is_playing_flag = sound_player(movement_sound_table, MOVEMENT_TBL_N_LINES);
                    break;
                case CLASH_SOUND:
                    sound_is_playing_flag = sound_player(clash_sound_table, CLASH_TBL_N_LINES);
                    break;
                default:
                    break;
            }
        }
//handle timer overflow - even if a sound is playing there are 5 minutes before overflow
        if ((t.read_ms() > (30*60*1000)) && !sound_is_playing_flag)
        {
            t.reset();
            // reset mpu statistics as well
            count = t.read_ms();
            sum = 0;
            sumCount = 0;
            //reset the low battery variables
            t_vbat_start = 0;
            t_vbat_count = 0;
        }
// kick the dog before the timeout       
//       wd.Service();      
    }
}




void mpu_calc_avg(MPU_data_type * MPU_data,int mpu_pointer, MPU_data_type * MPU_avg_data, int avg_len)
{
    int i = 0;
    //MPU_data_type MPU_avg_data;
    
    MPU_avg_data->ax = 0;
    MPU_avg_data->ay = 0;
    MPU_avg_data->az = 0;
    MPU_avg_data->yaw = 0;
    MPU_avg_data->pitch = 0;
    MPU_avg_data->roll = 0;
    
    for (i=0 ; i < avg_len ; i++)
    {
        mpu_pointer++;
        if (mpu_pointer >= MPU_BUFFER_LEN)
            {
                mpu_pointer = 0;
            }
        MPU_avg_data->ax += MPU_data[mpu_pointer].ax;
        MPU_avg_data->ay += MPU_data[mpu_pointer].ay;
        MPU_avg_data->az += MPU_data[mpu_pointer].az;
        MPU_avg_data->yaw += MPU_data[mpu_pointer].yaw;
        MPU_avg_data->pitch += MPU_data[mpu_pointer].pitch;
        MPU_avg_data->roll += MPU_data[mpu_pointer].roll; 
    }
    
    MPU_avg_data->ax = MPU_avg_data->ax / avg_len;
    MPU_avg_data->ay = MPU_avg_data->ay / avg_len;
    MPU_avg_data->az = MPU_avg_data->az / avg_len;
    MPU_avg_data->yaw = MPU_avg_data->yaw / avg_len;
    MPU_avg_data->pitch = MPU_avg_data->pitch / avg_len;
    MPU_avg_data->roll = MPU_avg_data->roll / avg_len;
    
    //return MPU_avg_data;
}

void init_color(int *color, int pulsewidth)
{
    Startup_Config_OUT.write(1);
    wait(1);
    if (Startup_Config_IN1.read() == 1)
    {
       *color = GREEN;
    }else if (Startup_Config_IN2.read() == 1)
    {
        *color = PURPLE;
    }else
    {
        
        *color = BLUE;
    }
    Startup_Config_OUT.write(0);
    
    Blue_LED.period_ms(LED_PWM_PERIOD);
    
    change_color((color_type)*color, pulsewidth);
}
        

void change_color(color_type new_color, int new_pulsewidth)
{
    Blue_LED.pulsewidth_us(0);
    Green_LED.pulsewidth_us(0);
    Red_LED.pulsewidth_us(0);

    switch (new_color) {
        case BLUE:
            Blue_LED.pulsewidth_us(new_pulsewidth);
            break;

        case GREEN:
            Green_LED.pulsewidth_us(new_pulsewidth);
            break;

        case PURPLE:
            Blue_LED.pulsewidth_us(new_pulsewidth);
            Red_LED.pulsewidth_us(new_pulsewidth);
            break;

        case RED:
            Red_LED.pulsewidth_us(new_pulsewidth);
            break;

        default:
            break;
    }
}

bool sound_player(const int sound_table[][6], int table_lines)
{
    int sound_period;
    int sound_pulse_width_us;
    if (sound_count > sound_table[sound_line][NUM_STEPS_IDX])
    {
        sound_count = 1;
        sound_line++;
        if (sound_line >= table_lines)
        {
            return FALSE;
        }
    }
    
    sound_period = sound_table[sound_line][INIT_PERIOD_IDX] + ((sound_count-1) * sound_table[sound_line][STEP_PERIOD_IDX]);
    sound_pulse_width_us = ( sound_period * (sound_table[sound_line][INIT_VOL_IDX] + ((sound_count-1) * sound_table[sound_line][STEP_VOL_IDX])) )/ 200;
    
    // there are no checks for 0/negative values of the above - need to make sure that tables are valid.
    // set PWM parameters for current step
    
    Speaker_OUT.period_us(sound_period);
    Speaker_OUT.pulsewidth_us(sound_pulse_width_us);
        
    //update next_sound_time, step count        
    
    next_sound_time = t.read_ms() + sound_table[sound_line][STEP_DUR_IDX];
    sound_count++;
    
    return TRUE;
}