main.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 #ifdef __SERIAL_DEBUG__ 
00004 #include "USBSerial.h"
00005 #include "STM32_USB48MHz.h"
00006 #endif
00007 #include "Watchdog.h"
00008 #include "LightSaber.h"
00009 #include "LightSaber_sounds.h"
00010 
00011 
00012 
00013 // Light Controls: (Timer 3, ch 1-3)
00014 PwmOut Red_LED(PA_6);
00015 PwmOut Green_LED(PA_7);
00016 PwmOut Blue_LED(PB_0);
00017 
00018 //Speaker drive (Timer 2 ch 2)
00019 PwmOut Speaker_OUT(PA_1);
00020 
00021 //Color change button
00022 DigitalIn Color_Button(PA_3, PullUp);
00023 
00024 //startup config IOs
00025 DigitalIn Startup_Config_IN1(PB_5, PullDown);
00026 DigitalIn Startup_Config_IN2(PB_7, PullDown);
00027 DigitalOut Startup_Config_OUT(PB_6, 0);
00028 
00029 //Spare Buttons
00030 //DigitalIn Spare1_Button(PB_12);
00031 //DigitalIn Spare2_Button(PB_13);
00032 //DigitalIn Spare3_Button(PB_14);
00033 
00034 //Watchdog
00035 //Watchdog wd;
00036 
00037 bool color_button_released = TRUE;
00038 volatile int Color_Button_Count = 0;
00039 
00040 //sounds
00041 volatile int sound_count = 1;
00042 volatile int sound_line = 0;
00043 volatile int next_sound_time = 0;
00044 
00045 //Battery voltage monitor
00046 AnalogIn V_bat(PA_2);
00047 
00048 //Ticker
00049 Ticker button_ticker;
00050 
00051 //Set up I2C, (SDA,SCL)
00052 //I2C MPU_i2c(PB_9, PB_8); //defined in MPU6050.h...
00053 
00054 //MPU interrupt line
00055 DigitalIn MPU_Intr(PB_1);
00056 
00057 //MPU6050 data rate 
00058 int delt_t = 0; // used to control display output rate
00059 
00060 extern float sum;
00061 extern uint32_t sumCount;
00062 
00063 MPU6050 mpu6050;
00064    
00065 Timer t;
00066 
00067 void button_inth()
00068 {
00069     int Color_Button_state;
00070     
00071     Color_Button_state = Color_Button.read(); //pressed = 0, released = 1
00072     
00073     if (color_button_released) {
00074         Color_Button_state = (~Color_Button_state) & 0x01;
00075     }
00076     Color_Button_Count = Color_Button_state*(Color_Button_Count + 1);
00077     
00078 }
00079 
00080 int main()
00081 {
00082     bool motion_is_init = FALSE;
00083     bool new_data = FALSE;
00084     
00085     int current_color;
00086     int light_intensity = NORM_LIGHT_INTENS;
00087     
00088     int v_bat_avg_mem[VBAT_AVG_LEN];
00089     int v_bat_avg_value;
00090     int v_bat_avg_pointer = 0;
00091     bool v_bat_ready = FALSE;
00092     bool vbat_low_flag = FALSE;
00093     int t_vbat_count = 0;
00094     int t_vbat_start = 0;
00095     
00096     MPU_data_type MPU_data[MPU_BUFFER_LEN];
00097     MPU_data_type MPU_avg_data;
00098     int mpu_pointer = 0;
00099     int mpu_data_count = 0;
00100     bool accel_thr_crossed = FALSE;
00101     bool ypr_thr_crossed = FALSE;
00102     int count = 0;
00103     
00104     int sound_to_play;
00105     bool sound_is_playing_flag = FALSE;
00106     
00107     int temp_diff;
00108     
00109     int i = 0;
00110 #ifdef __SERIAL_DEBUG__   
00111     STM32_HSI_USB48MHz(); // HSI,USB48MHz,SYSCLK32MHz
00112     
00113     // serial port for debug etc.
00114     USBSerial serial;
00115 
00116     //USBSerial serial;
00117 
00118     serial.printf("Initializing main");
00119 #endif
00120 
00121 //initialize LEDs : set rate, intensity, and startup color (according to jumper settings)
00122     
00123     init_color(&current_color,light_intensity); 
00124 
00125 //Set up I2C
00126     MPU6050_set_I2C_freq(300000);  // use fast (400 kHz) I2C
00127 
00128 // start "clock"
00129     t.start();
00130     
00131 // play startup sound    
00132     sound_to_play = STARTUP_SOUND;
00133     sound_line = 0;
00134     sound_count = 1;
00135     next_sound_time = t.read_ms();
00136     sound_is_playing_flag = TRUE;
00137     
00138 // initialize the motion sensor
00139     motion_is_init = mpu6050.motion_sensor_init();
00140     
00141     button_ticker.attach(&button_inth, 0.025);
00142         
00143     wait_ms(100); //from MPU example (1000)
00144     
00145 //Initialize watchdog with a 2 second interval
00146  //   wd.Configure(2.0);       
00147 
00148 //main loop
00149     while(1) {
00150         
00151               
00152     
00153         if (vbat_low_flag) //low battery
00154         {
00155             light_intensity = LOW_LIGHT_INTENS; //lower light intensity save some battery
00156             if (t_vbat_start == 0) //starting a new cycle
00157             {
00158                 t_vbat_start = t.read_ms();
00159                 if (t_vbat_count < 3) //flash only in first 3 cycles
00160                 {
00161                     change_color(RED , light_intensity);
00162                 }
00163             } else if ( ((t.read_ms() - t_vbat_start) > 1000) && (t_vbat_count < 3)) // dark period and flash only in first 3 cycles
00164             {
00165                 change_color(RED, 0);
00166             } else // after first 3 cycles return to normal operation (with lower light intensity)
00167             {
00168                 change_color((color_type) current_color, light_intensity);
00169             }
00170             if ( (t.read_ms() - t_vbat_start) > 2000) //end of cycle
00171             {
00172                 t_vbat_start = 0;
00173                 t_vbat_count ++;
00174             }
00175             if (t_vbat_count > 30) // once a minute, starting over
00176             {
00177                 t_vbat_count  = 0;
00178             }
00179             
00180         }
00181         //battery ok or between low bat flashes, check if button was pressed or released
00182         if (!vbat_low_flag || (t_vbat_count >=3))
00183         {        
00184             if ( (Color_Button_Count >= BUTTON_PRESS_THR) && color_button_released )
00185             {
00186                 __disable_irq();
00187                 color_button_released = FALSE;
00188                 Color_Button_Count = 0;
00189                 __enable_irq(); 
00190                 current_color++;
00191                 if (current_color >= NUM_OF_COLORS)
00192                 {
00193                     current_color = BLUE;
00194                 }
00195 #ifdef __SERIAL_DEBUG__
00196                 serial.printf("Color Button pressed");
00197 #endif
00198                 change_color((color_type) current_color, light_intensity);
00199             }   
00200         
00201             if ((Color_Button_Count >= BUTTON_RELEASE_THR) && !color_button_released)
00202             {
00203                 __disable_irq();
00204                 color_button_released = TRUE;
00205                 Color_Button_Count = 0;
00206                 __enable_irq(); 
00207 #ifdef __SERIAL_DEBUG__
00208                 serial.printf("Color Button released");
00209 #endif
00210             }
00211         }
00212             
00213         
00214         
00215         if (motion_is_init)
00216         {
00217             new_data = mpu6050.motion_update_data(&MPU_data[mpu_pointer], t.read_us());
00218         }
00219         
00220         if (new_data)
00221         {
00222             
00223             if ((mpu_data_count > MPU_DATA_STABLE) && !sound_is_playing_flag )
00224             {
00225                 //calculate new average
00226                 mpu_calc_avg(MPU_data, mpu_pointer, &MPU_avg_data, MPU_AVG_LEN);
00227                 
00228                 //check if any thresholds are crossed
00229                 //acceleration
00230                 if ((abs(MPU_avg_data.ax - MPU_data[mpu_pointer].ax) > ACCEL_THR) 
00231                         || (abs(MPU_avg_data.ay - MPU_data[mpu_pointer].ay) > ACCEL_THR) 
00232                         || (abs(MPU_avg_data.az - MPU_data[mpu_pointer].az) > ACCEL_THR))
00233                 {
00234                     accel_thr_crossed = TRUE;
00235                 }
00236                 // YPR
00237                 temp_diff = abs(MPU_avg_data.yaw - MPU_data[mpu_pointer].yaw);
00238                 if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
00239                 {
00240                     ypr_thr_crossed = TRUE;
00241                 }
00242                 
00243                 temp_diff = abs(MPU_avg_data.pitch - MPU_data[mpu_pointer].pitch);
00244                 if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
00245                 {
00246                     ypr_thr_crossed = TRUE;
00247                 }
00248                 
00249                 temp_diff = abs(MPU_avg_data.roll - MPU_data[mpu_pointer].roll);
00250                 if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
00251                 {
00252                     ypr_thr_crossed = TRUE;
00253                 }
00254                 
00255                 if (accel_thr_crossed)
00256                 {
00257                     //play clash sound
00258                     sound_to_play = CLASH_SOUND;
00259                     sound_line = 0;
00260                     sound_count = 1;
00261                     next_sound_time = t.read_ms();
00262                     sound_is_playing_flag = TRUE;
00263                 
00264                 }
00265                 if (ypr_thr_crossed && !accel_thr_crossed)
00266                 {
00267                     //play movement sound
00268                     sound_to_play = MOVEMENT_SOUND;
00269                     sound_line = 0;
00270                     sound_count = 1;
00271                     next_sound_time = t.read_ms();
00272                     sound_is_playing_flag = TRUE;
00273                 }
00274                 
00275                 accel_thr_crossed = FALSE;
00276                 ypr_thr_crossed = FALSE;
00277                                 
00278             } else {
00279                 mpu_data_count++;
00280             }
00281             
00282             mpu_pointer++;
00283             
00284             if (mpu_pointer >= MPU_BUFFER_LEN)
00285             {
00286                 mpu_pointer = 0;
00287             }
00288             
00289             new_data = FALSE;   
00290         }
00291         
00292         delt_t = t.read_ms() - count;
00293         if (delt_t > 1000) 
00294         { // update serial port once per second independent of read rate
00295             
00296 #ifdef __SERIAL_DEBUG__
00297             //serial.printf("Yaw, Pitch, Roll: %f %f %f\n\r", (float) (yaw / 16384.0f), (float) (pitch/16384.0f), (float) (roll/16384.0f));
00298             serial.printf("Yaw, Pitch, Roll: %i %i %i\n\r", (int)(yaw * 10), (int)(pitch * 10), (int)(roll * 10));
00299             serial.printf("Yaw, Pitch, Roll (average): %i %i %i\n\r", MPU_avg_data.yaw, MPU_avg_data.pitch, MPU_avg_data.roll);
00300             
00301             serial.printf("Ax, Ay, Az : %i %i %i\n\r", (int)(ax * 1000), (int)(ay * 1000), (int)(az * 1000));
00302             serial.printf("Ax, Ay, Az (average): %i %i %i\n\r", MPU_avg_data.ax , MPU_avg_data.ay, MPU_avg_data.az);
00303             
00304             serial.printf("average rate = %f\n\r", (float) sumCount/sum);
00305             
00306             if (sound_is_playing_flag)
00307             {
00308                 serial.printf("Sound is playing!  \n\r", (float) sumCount/sum);
00309             }
00310 #endif           
00311             count = t.read_ms();
00312             sum = 0;
00313             sumCount = 0;
00314                       
00315 // Battery voltage monitor
00316             v_bat_avg_mem[v_bat_avg_pointer] = V_bat.read_u16();
00317             v_bat_avg_pointer++;
00318             if (v_bat_avg_pointer >= VBAT_AVG_LEN)
00319             {
00320                 v_bat_avg_pointer = 0;
00321                 v_bat_ready = TRUE;
00322             }
00323             if (v_bat_ready)
00324             {
00325                 //calculate the average
00326                 v_bat_avg_value = 0;
00327                 for (i = 0 ; i < VBAT_AVG_LEN ; i++)
00328                 {
00329                     v_bat_avg_value += v_bat_avg_mem[i];
00330                 }
00331                 
00332                 v_bat_avg_value = v_bat_avg_value/VBAT_AVG_LEN;
00333 #ifdef __SERIAL_DEBUG__                
00334                 serial.printf("Battery average voltage: %i \n\r", v_bat_avg_value);
00335 #endif               
00336                 //check voltage
00337                 if (v_bat_avg_value < VBAT_THR)
00338                 {
00339                     vbat_low_flag = TRUE;
00340                 } else if ((vbat_low_flag == TRUE) && (v_bat_avg_value > (VBAT_THR+VBAT_HYST)))
00341                 {
00342                     vbat_low_flag = FALSE;
00343                 }
00344             }
00345             
00346             
00347         }
00348 // Play sound
00349         if ((t.read_ms() >= next_sound_time) && sound_is_playing_flag ) 
00350         {
00351             switch (sound_to_play) {
00352 
00353                 case STARTUP_SOUND:
00354                     sound_is_playing_flag = sound_player(startup_sound_table, STARTUP_TBL_N_LINES);
00355                     break;
00356                 case MOVEMENT_SOUND:
00357                     sound_is_playing_flag = sound_player(movement_sound_table, MOVEMENT_TBL_N_LINES);
00358                     break;
00359                 case CLASH_SOUND:
00360                     sound_is_playing_flag = sound_player(clash_sound_table, CLASH_TBL_N_LINES);
00361                     break;
00362                 default:
00363                     break;
00364             }
00365         }
00366 //handle timer overflow - even if a sound is playing there are 5 minutes before overflow
00367         if ((t.read_ms() > (30*60*1000)) && !sound_is_playing_flag)
00368         {
00369             t.reset();
00370             // reset mpu statistics as well
00371             count = t.read_ms();
00372             sum = 0;
00373             sumCount = 0;
00374             //reset the low battery variables
00375             t_vbat_start = 0;
00376             t_vbat_count = 0;
00377         }
00378 // kick the dog before the timeout       
00379 //       wd.Service();      
00380     }
00381 }
00382 
00383 
00384 
00385 
00386 void mpu_calc_avg(MPU_data_type * MPU_data,int mpu_pointer, MPU_data_type * MPU_avg_data, int avg_len)
00387 {
00388     int i = 0;
00389     //MPU_data_type MPU_avg_data;
00390     
00391     MPU_avg_data->ax = 0;
00392     MPU_avg_data->ay = 0;
00393     MPU_avg_data->az = 0;
00394     MPU_avg_data->yaw = 0;
00395     MPU_avg_data->pitch = 0;
00396     MPU_avg_data->roll = 0;
00397     
00398     for (i=0 ; i < avg_len ; i++)
00399     {
00400         mpu_pointer++;
00401         if (mpu_pointer >= MPU_BUFFER_LEN)
00402             {
00403                 mpu_pointer = 0;
00404             }
00405         MPU_avg_data->ax += MPU_data[mpu_pointer].ax;
00406         MPU_avg_data->ay += MPU_data[mpu_pointer].ay;
00407         MPU_avg_data->az += MPU_data[mpu_pointer].az;
00408         MPU_avg_data->yaw += MPU_data[mpu_pointer].yaw;
00409         MPU_avg_data->pitch += MPU_data[mpu_pointer].pitch;
00410         MPU_avg_data->roll += MPU_data[mpu_pointer].roll; 
00411     }
00412     
00413     MPU_avg_data->ax = MPU_avg_data->ax / avg_len;
00414     MPU_avg_data->ay = MPU_avg_data->ay / avg_len;
00415     MPU_avg_data->az = MPU_avg_data->az / avg_len;
00416     MPU_avg_data->yaw = MPU_avg_data->yaw / avg_len;
00417     MPU_avg_data->pitch = MPU_avg_data->pitch / avg_len;
00418     MPU_avg_data->roll = MPU_avg_data->roll / avg_len;
00419     
00420     //return MPU_avg_data;
00421 }
00422 
00423 void init_color(int *color, int pulsewidth)
00424 {
00425     Startup_Config_OUT.write(1);
00426     wait(1);
00427     if (Startup_Config_IN1.read() == 1)
00428     {
00429        *color = GREEN;
00430     }else if (Startup_Config_IN2.read() == 1)
00431     {
00432         *color = PURPLE;
00433     }else
00434     {
00435         
00436         *color = BLUE;
00437     }
00438     Startup_Config_OUT.write(0);
00439     
00440     Blue_LED.period_ms(LED_PWM_PERIOD);
00441     
00442     change_color((color_type)*color, pulsewidth);
00443 }
00444         
00445 
00446 void change_color(color_type new_color, int new_pulsewidth)
00447 {
00448     Blue_LED.pulsewidth_us(0);
00449     Green_LED.pulsewidth_us(0);
00450     Red_LED.pulsewidth_us(0);
00451 
00452     switch (new_color) {
00453         case BLUE:
00454             Blue_LED.pulsewidth_us(new_pulsewidth);
00455             break;
00456 
00457         case GREEN:
00458             Green_LED.pulsewidth_us(new_pulsewidth);
00459             break;
00460 
00461         case PURPLE:
00462             Blue_LED.pulsewidth_us(new_pulsewidth);
00463             Red_LED.pulsewidth_us(new_pulsewidth);
00464             break;
00465 
00466         case RED:
00467             Red_LED.pulsewidth_us(new_pulsewidth);
00468             break;
00469 
00470         default:
00471             break;
00472     }
00473 }
00474 
00475 bool sound_player(const int sound_table[][6], int table_lines)
00476 {
00477     int sound_period;
00478     int sound_pulse_width_us;
00479     if (sound_count > sound_table[sound_line][NUM_STEPS_IDX])
00480     {
00481         sound_count = 1;
00482         sound_line++;
00483         if (sound_line >= table_lines)
00484         {
00485             return FALSE;
00486         }
00487     }
00488     
00489     sound_period = sound_table[sound_line][INIT_PERIOD_IDX] + ((sound_count-1) * sound_table[sound_line][STEP_PERIOD_IDX]);
00490     sound_pulse_width_us = ( sound_period * (sound_table[sound_line][INIT_VOL_IDX] + ((sound_count-1) * sound_table[sound_line][STEP_VOL_IDX])) )/ 200;
00491     
00492     // there are no checks for 0/negative values of the above - need to make sure that tables are valid.
00493     // set PWM parameters for current step
00494     
00495     Speaker_OUT.period_us(sound_period);
00496     Speaker_OUT.pulsewidth_us(sound_pulse_width_us);
00497         
00498     //update next_sound_time, step count        
00499     
00500     next_sound_time = t.read_ms() + sound_table[sound_line][STEP_DUR_IDX];
00501     sound_count++;
00502     
00503     return TRUE;
00504 }
00505