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-22
- Revision:
- 0:0bb3687e39da
- Child:
- 2:59a7d4677474
File content as of revision 0:0bb3687e39da:
#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 int sound_to_play; volatile int sound_count = 1; volatile int sound_line = 0; volatile int next_sound_time = 0; volatile bool sound_is_playing_flag = FALSE; //Battery voltage monitor AnalogIn V_bat(PA_2); //Ticker Ticker button_ticker, sound_ticker; //MPU 6050 float sum = 0; uint32_t sumCount = 0; //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 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); } 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; } void sound_inth() { 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; } } } 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 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 MPU_i2c.frequency(300000); // use fast (400 kHz) I2C // start "clock" t.start(); // attach and set sound interrupt handler (every 10msec) sound_ticker.attach(&sound_inth, 0.01); // 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 = motion_sensor_init(); button_ticker.attach(&button_inth, 0.025); wait_ms(1000); //from MPU example //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 = motion_update_data(&MPU_data[mpu_pointer]); } 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; } } } //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(); } } bool motion_sensor_init() { // Read the WHO_AM_I register, this is a good test of communication uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 //serial.printf("I AM 0x%x\n\r", whoami); //serial.printf("I SHOULD BE 0x68\n\r"); if (whoami == 0x68) { // WHO_AM_I should always be 0x68 // serial.printf("MPU6050 is online..."); wait(1); mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values /* serial.printf("x-axis self test: acceleration trim within : "); serial.printf("%f", SelfTest[0]); serial.printf("% of factory value \n\r"); serial.printf("y-axis self test: acceleration trim within : "); serial.printf("%f", SelfTest[1]); serial.printf("% of factory value \n\r"); serial.printf("z-axis self test: acceleration trim within : "); serial.printf("%f", SelfTest[2]); serial.printf("% of factory value \n\r"); serial.printf("x-axis self test: gyration trim within : "); serial.printf("%f", SelfTest[3]); serial.printf("% of factory value \n\r"); serial.printf("y-axis self test: gyration trim within : "); serial.printf("%f", SelfTest[4]); serial.printf("% of factory value \n\r"); serial.printf("z-axis self test: gyration trim within : "); serial.printf("%f", SelfTest[5]); serial.printf("% of factory value \n\r"); */ wait(1); if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) { mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers mpu6050.initMPU6050(); //serial.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature return TRUE; } else { //serial.printf("Device did not the pass self-test!\n\r"); return FALSE; } } else { //serial.printf("Could not connect to MPU6050: \n\r"); //serial.printf("%#x \n", whoami); return FALSE; } } bool motion_update_data(MPU_data_type *new_data) { if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { mpu6050.readAccelData(accelCount); // Read the x/y/z adc values mpu6050.getAres(); // Now we'll calculate the accleration value into actual g's ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set ay = (float)accelCount[1]*aRes - accelBias[1]; az = (float)accelCount[2]*aRes - accelBias[2]; mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values mpu6050.getGres(); // Calculate the gyro value into actual degrees per second gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; tempCount = mpu6050.readTempData(); // Read the x/y/z adc values temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade Now = t.read_us(); deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update lastUpdate = Now; sum += deltat; sumCount++; if(lastUpdate - firstUpdate > 10000000.0f) { beta = 0.04; // decrease filter gain after stabilized zeta = 0.015; // increase bias drift gain after stabilized } // Pass gyro rate as rad/s mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. // In this coordinate system, the positive z-axis is down toward Earth. // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be // applied in the correct order which for this configuration is yaw, pitch, and then roll. // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); pitch *= 180.0f / PI; yaw *= 180.0f / PI; roll *= 180.0f / PI; /* new_data->ax = (int) (ax / 16384.0f); new_data->ay = (int) (ay / 16384.0f); new_data->az = (int) (az / 16384.0f); new_data->yaw = (int) (yaw / 16384.0f); new_data->pitch = (int) (pitch / 16384.0f); new_data->roll = (int) (roll / 16384.0f); */ new_data->ax = (int) (ax * 1000); new_data->ay = (int) (ay * 1000); new_data->az = (int) (az * 1000); new_data->yaw = (int) (yaw * 10); new_data->pitch = (int) (pitch * 10); new_data->roll = (int) (roll * 10); return TRUE; } else { return FALSE; } } 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; } }