![](/media/cache/profiles/4bd40d95b1a5ea73753bb83497730d2d.jpg.50x50_q85.png)
A Jedi light saber controller program with the following "features": - Using RGB LEDs - User can change light colors with a button - Motion dependent (PWM) sounds with a MPU6050 motion sensor - Low voltage detection
Dependencies: L152RE_USBDevice STM32_USB48MHz Watchdog mbed
main.cpp
- Committer:
- nightmechanic
- Date:
- 2016-03-26
- Revision:
- 3:0c2d9355ed8c
- Parent:
- 2:59a7d4677474
File content as of revision 3:0c2d9355ed8c:
#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(¤t_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; }