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@2:59a7d4677474, 2016-03-24 (annotated)
- Committer:
- nightmechanic
- Date:
- Thu Mar 24 22:42:59 2016 +0000
- Revision:
- 2:59a7d4677474
- Parent:
- 0:0bb3687e39da
- Child:
- 3:0c2d9355ed8c
re-arranging the MPU6050 driver: ; separation into h and cpp files; etc.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nightmechanic | 0:0bb3687e39da | 1 | #include "mbed.h" |
nightmechanic | 0:0bb3687e39da | 2 | #include "MPU6050.h" |
nightmechanic | 0:0bb3687e39da | 3 | #ifdef __SERIAL_DEBUG__ |
nightmechanic | 0:0bb3687e39da | 4 | #include "USBSerial.h" |
nightmechanic | 0:0bb3687e39da | 5 | #include "STM32_USB48MHz.h" |
nightmechanic | 0:0bb3687e39da | 6 | #endif |
nightmechanic | 0:0bb3687e39da | 7 | #include "Watchdog.h" |
nightmechanic | 0:0bb3687e39da | 8 | #include "LightSaber.h" |
nightmechanic | 0:0bb3687e39da | 9 | #include "LightSaber_sounds.h" |
nightmechanic | 0:0bb3687e39da | 10 | |
nightmechanic | 0:0bb3687e39da | 11 | |
nightmechanic | 0:0bb3687e39da | 12 | |
nightmechanic | 0:0bb3687e39da | 13 | // Light Controls: (Timer 3, ch 1-3) |
nightmechanic | 0:0bb3687e39da | 14 | PwmOut Red_LED(PA_6); |
nightmechanic | 0:0bb3687e39da | 15 | PwmOut Green_LED(PA_7); |
nightmechanic | 0:0bb3687e39da | 16 | PwmOut Blue_LED(PB_0); |
nightmechanic | 0:0bb3687e39da | 17 | |
nightmechanic | 0:0bb3687e39da | 18 | //Speaker drive (Timer 2 ch 2) |
nightmechanic | 0:0bb3687e39da | 19 | PwmOut Speaker_OUT(PA_1); |
nightmechanic | 0:0bb3687e39da | 20 | |
nightmechanic | 0:0bb3687e39da | 21 | //Color change button |
nightmechanic | 0:0bb3687e39da | 22 | DigitalIn Color_Button(PA_3, PullUp); |
nightmechanic | 0:0bb3687e39da | 23 | |
nightmechanic | 0:0bb3687e39da | 24 | //startup config IOs |
nightmechanic | 0:0bb3687e39da | 25 | DigitalIn Startup_Config_IN1(PB_5, PullDown); |
nightmechanic | 0:0bb3687e39da | 26 | DigitalIn Startup_Config_IN2(PB_7, PullDown); |
nightmechanic | 0:0bb3687e39da | 27 | DigitalOut Startup_Config_OUT(PB_6, 0); |
nightmechanic | 0:0bb3687e39da | 28 | |
nightmechanic | 0:0bb3687e39da | 29 | //Spare Buttons |
nightmechanic | 0:0bb3687e39da | 30 | //DigitalIn Spare1_Button(PB_12); |
nightmechanic | 0:0bb3687e39da | 31 | //DigitalIn Spare2_Button(PB_13); |
nightmechanic | 0:0bb3687e39da | 32 | //DigitalIn Spare3_Button(PB_14); |
nightmechanic | 0:0bb3687e39da | 33 | |
nightmechanic | 0:0bb3687e39da | 34 | //Watchdog |
nightmechanic | 0:0bb3687e39da | 35 | //Watchdog wd; |
nightmechanic | 0:0bb3687e39da | 36 | |
nightmechanic | 0:0bb3687e39da | 37 | bool color_button_released = TRUE; |
nightmechanic | 0:0bb3687e39da | 38 | volatile int Color_Button_Count = 0; |
nightmechanic | 0:0bb3687e39da | 39 | |
nightmechanic | 0:0bb3687e39da | 40 | //sounds |
nightmechanic | 0:0bb3687e39da | 41 | int sound_to_play; |
nightmechanic | 0:0bb3687e39da | 42 | volatile int sound_count = 1; |
nightmechanic | 0:0bb3687e39da | 43 | volatile int sound_line = 0; |
nightmechanic | 0:0bb3687e39da | 44 | volatile int next_sound_time = 0; |
nightmechanic | 0:0bb3687e39da | 45 | volatile bool sound_is_playing_flag = FALSE; |
nightmechanic | 0:0bb3687e39da | 46 | |
nightmechanic | 0:0bb3687e39da | 47 | //Battery voltage monitor |
nightmechanic | 0:0bb3687e39da | 48 | AnalogIn V_bat(PA_2); |
nightmechanic | 0:0bb3687e39da | 49 | |
nightmechanic | 0:0bb3687e39da | 50 | //Ticker |
nightmechanic | 0:0bb3687e39da | 51 | Ticker button_ticker, sound_ticker; |
nightmechanic | 0:0bb3687e39da | 52 | |
nightmechanic | 0:0bb3687e39da | 53 | //Set up I2C, (SDA,SCL) |
nightmechanic | 0:0bb3687e39da | 54 | //I2C MPU_i2c(PB_9, PB_8); //defined in MPU6050.h... |
nightmechanic | 0:0bb3687e39da | 55 | |
nightmechanic | 0:0bb3687e39da | 56 | //MPU interrupt line |
nightmechanic | 0:0bb3687e39da | 57 | DigitalIn MPU_Intr(PB_1); |
nightmechanic | 0:0bb3687e39da | 58 | |
nightmechanic | 2:59a7d4677474 | 59 | //MPU6050 data rate |
nightmechanic | 2:59a7d4677474 | 60 | int delt_t = 0; // used to control display output rate |
nightmechanic | 2:59a7d4677474 | 61 | |
nightmechanic | 2:59a7d4677474 | 62 | extern float sum; |
nightmechanic | 2:59a7d4677474 | 63 | extern uint32_t sumCount; |
nightmechanic | 2:59a7d4677474 | 64 | |
nightmechanic | 0:0bb3687e39da | 65 | MPU6050 mpu6050; |
nightmechanic | 0:0bb3687e39da | 66 | |
nightmechanic | 0:0bb3687e39da | 67 | Timer t; |
nightmechanic | 0:0bb3687e39da | 68 | |
nightmechanic | 0:0bb3687e39da | 69 | void button_inth() |
nightmechanic | 0:0bb3687e39da | 70 | { |
nightmechanic | 0:0bb3687e39da | 71 | int Color_Button_state; |
nightmechanic | 0:0bb3687e39da | 72 | |
nightmechanic | 0:0bb3687e39da | 73 | Color_Button_state = Color_Button.read(); //pressed = 0, released = 1 |
nightmechanic | 0:0bb3687e39da | 74 | |
nightmechanic | 0:0bb3687e39da | 75 | if (color_button_released) { |
nightmechanic | 0:0bb3687e39da | 76 | Color_Button_state = (~Color_Button_state) & 0x01; |
nightmechanic | 0:0bb3687e39da | 77 | } |
nightmechanic | 0:0bb3687e39da | 78 | Color_Button_Count = Color_Button_state*(Color_Button_Count + 1); |
nightmechanic | 0:0bb3687e39da | 79 | |
nightmechanic | 0:0bb3687e39da | 80 | } |
nightmechanic | 0:0bb3687e39da | 81 | |
nightmechanic | 0:0bb3687e39da | 82 | bool sound_player(const int sound_table[][6], int table_lines) |
nightmechanic | 0:0bb3687e39da | 83 | { |
nightmechanic | 0:0bb3687e39da | 84 | int sound_period; |
nightmechanic | 0:0bb3687e39da | 85 | int sound_pulse_width_us; |
nightmechanic | 0:0bb3687e39da | 86 | if (sound_count > sound_table[sound_line][NUM_STEPS_IDX]) |
nightmechanic | 0:0bb3687e39da | 87 | { |
nightmechanic | 0:0bb3687e39da | 88 | sound_count = 1; |
nightmechanic | 0:0bb3687e39da | 89 | sound_line++; |
nightmechanic | 0:0bb3687e39da | 90 | if (sound_line >= table_lines) |
nightmechanic | 0:0bb3687e39da | 91 | { |
nightmechanic | 0:0bb3687e39da | 92 | return FALSE; |
nightmechanic | 0:0bb3687e39da | 93 | } |
nightmechanic | 0:0bb3687e39da | 94 | } |
nightmechanic | 0:0bb3687e39da | 95 | |
nightmechanic | 0:0bb3687e39da | 96 | sound_period = sound_table[sound_line][INIT_PERIOD_IDX] + ((sound_count-1) * sound_table[sound_line][STEP_PERIOD_IDX]); |
nightmechanic | 0:0bb3687e39da | 97 | sound_pulse_width_us = ( sound_period * (sound_table[sound_line][INIT_VOL_IDX] + ((sound_count-1) * sound_table[sound_line][STEP_VOL_IDX])) )/ 200; |
nightmechanic | 0:0bb3687e39da | 98 | |
nightmechanic | 0:0bb3687e39da | 99 | // there are no checks for 0/negative values of the above - need to make sure that tables are valid. |
nightmechanic | 0:0bb3687e39da | 100 | // set PWM parameters for current step |
nightmechanic | 0:0bb3687e39da | 101 | |
nightmechanic | 0:0bb3687e39da | 102 | Speaker_OUT.period_us(sound_period); |
nightmechanic | 0:0bb3687e39da | 103 | Speaker_OUT.pulsewidth_us(sound_pulse_width_us); |
nightmechanic | 0:0bb3687e39da | 104 | |
nightmechanic | 0:0bb3687e39da | 105 | //update next_sound_time, step count |
nightmechanic | 0:0bb3687e39da | 106 | |
nightmechanic | 0:0bb3687e39da | 107 | next_sound_time = t.read_ms() + sound_table[sound_line][STEP_DUR_IDX]; |
nightmechanic | 0:0bb3687e39da | 108 | sound_count++; |
nightmechanic | 0:0bb3687e39da | 109 | |
nightmechanic | 0:0bb3687e39da | 110 | return TRUE; |
nightmechanic | 0:0bb3687e39da | 111 | } |
nightmechanic | 0:0bb3687e39da | 112 | |
nightmechanic | 0:0bb3687e39da | 113 | void sound_inth() |
nightmechanic | 0:0bb3687e39da | 114 | { |
nightmechanic | 0:0bb3687e39da | 115 | if ((t.read_ms() >= next_sound_time) && sound_is_playing_flag ) |
nightmechanic | 0:0bb3687e39da | 116 | { |
nightmechanic | 0:0bb3687e39da | 117 | switch (sound_to_play) { |
nightmechanic | 0:0bb3687e39da | 118 | |
nightmechanic | 0:0bb3687e39da | 119 | case STARTUP_SOUND: |
nightmechanic | 0:0bb3687e39da | 120 | sound_is_playing_flag = sound_player(startup_sound_table, STARTUP_TBL_N_LINES); |
nightmechanic | 0:0bb3687e39da | 121 | break; |
nightmechanic | 0:0bb3687e39da | 122 | case MOVEMENT_SOUND: |
nightmechanic | 0:0bb3687e39da | 123 | sound_is_playing_flag = sound_player(movement_sound_table, MOVEMENT_TBL_N_LINES); |
nightmechanic | 0:0bb3687e39da | 124 | break; |
nightmechanic | 0:0bb3687e39da | 125 | case CLASH_SOUND: |
nightmechanic | 0:0bb3687e39da | 126 | sound_is_playing_flag = sound_player(clash_sound_table, CLASH_TBL_N_LINES); |
nightmechanic | 0:0bb3687e39da | 127 | break; |
nightmechanic | 0:0bb3687e39da | 128 | default: |
nightmechanic | 0:0bb3687e39da | 129 | break; |
nightmechanic | 0:0bb3687e39da | 130 | } |
nightmechanic | 0:0bb3687e39da | 131 | } |
nightmechanic | 0:0bb3687e39da | 132 | } |
nightmechanic | 0:0bb3687e39da | 133 | int main() |
nightmechanic | 0:0bb3687e39da | 134 | { |
nightmechanic | 0:0bb3687e39da | 135 | bool motion_is_init = FALSE; |
nightmechanic | 0:0bb3687e39da | 136 | bool new_data = FALSE; |
nightmechanic | 0:0bb3687e39da | 137 | |
nightmechanic | 0:0bb3687e39da | 138 | int current_color; |
nightmechanic | 0:0bb3687e39da | 139 | int light_intensity = NORM_LIGHT_INTENS; |
nightmechanic | 0:0bb3687e39da | 140 | |
nightmechanic | 0:0bb3687e39da | 141 | int v_bat_avg_mem[VBAT_AVG_LEN]; |
nightmechanic | 0:0bb3687e39da | 142 | int v_bat_avg_value; |
nightmechanic | 0:0bb3687e39da | 143 | int v_bat_avg_pointer = 0; |
nightmechanic | 0:0bb3687e39da | 144 | bool v_bat_ready = FALSE; |
nightmechanic | 0:0bb3687e39da | 145 | bool vbat_low_flag = FALSE; |
nightmechanic | 0:0bb3687e39da | 146 | int t_vbat_count = 0; |
nightmechanic | 0:0bb3687e39da | 147 | int t_vbat_start = 0; |
nightmechanic | 0:0bb3687e39da | 148 | |
nightmechanic | 0:0bb3687e39da | 149 | MPU_data_type MPU_data[MPU_BUFFER_LEN]; |
nightmechanic | 0:0bb3687e39da | 150 | MPU_data_type MPU_avg_data; |
nightmechanic | 0:0bb3687e39da | 151 | int mpu_pointer = 0; |
nightmechanic | 0:0bb3687e39da | 152 | int mpu_data_count = 0; |
nightmechanic | 0:0bb3687e39da | 153 | bool accel_thr_crossed = FALSE; |
nightmechanic | 0:0bb3687e39da | 154 | bool ypr_thr_crossed = FALSE; |
nightmechanic | 0:0bb3687e39da | 155 | int count = 0; |
nightmechanic | 0:0bb3687e39da | 156 | |
nightmechanic | 0:0bb3687e39da | 157 | int temp_diff; |
nightmechanic | 0:0bb3687e39da | 158 | |
nightmechanic | 0:0bb3687e39da | 159 | int i = 0; |
nightmechanic | 0:0bb3687e39da | 160 | #ifdef __SERIAL_DEBUG__ |
nightmechanic | 0:0bb3687e39da | 161 | STM32_HSI_USB48MHz(); // HSI,USB48MHz,SYSCLK32MHz |
nightmechanic | 0:0bb3687e39da | 162 | |
nightmechanic | 0:0bb3687e39da | 163 | // serial port for debug etc. |
nightmechanic | 0:0bb3687e39da | 164 | USBSerial serial; |
nightmechanic | 0:0bb3687e39da | 165 | |
nightmechanic | 0:0bb3687e39da | 166 | //USBSerial serial; |
nightmechanic | 0:0bb3687e39da | 167 | |
nightmechanic | 0:0bb3687e39da | 168 | serial.printf("Initializing main"); |
nightmechanic | 0:0bb3687e39da | 169 | #endif |
nightmechanic | 0:0bb3687e39da | 170 | |
nightmechanic | 0:0bb3687e39da | 171 | //initialize LEDs : set rate, intensity, and startup color (according to jumper settings) |
nightmechanic | 0:0bb3687e39da | 172 | |
nightmechanic | 0:0bb3687e39da | 173 | init_color(¤t_color,light_intensity); |
nightmechanic | 0:0bb3687e39da | 174 | |
nightmechanic | 0:0bb3687e39da | 175 | //Set up I2C |
nightmechanic | 2:59a7d4677474 | 176 | MPU6050_set_I2C_freq(300000); // use fast (400 kHz) I2C |
nightmechanic | 0:0bb3687e39da | 177 | |
nightmechanic | 0:0bb3687e39da | 178 | // start "clock" |
nightmechanic | 0:0bb3687e39da | 179 | t.start(); |
nightmechanic | 0:0bb3687e39da | 180 | |
nightmechanic | 0:0bb3687e39da | 181 | // attach and set sound interrupt handler (every 10msec) |
nightmechanic | 0:0bb3687e39da | 182 | sound_ticker.attach(&sound_inth, 0.01); |
nightmechanic | 0:0bb3687e39da | 183 | // play startup sound |
nightmechanic | 0:0bb3687e39da | 184 | sound_to_play = STARTUP_SOUND; |
nightmechanic | 0:0bb3687e39da | 185 | sound_line = 0; |
nightmechanic | 0:0bb3687e39da | 186 | sound_count = 1; |
nightmechanic | 0:0bb3687e39da | 187 | next_sound_time = t.read_ms(); |
nightmechanic | 0:0bb3687e39da | 188 | sound_is_playing_flag = TRUE; |
nightmechanic | 0:0bb3687e39da | 189 | |
nightmechanic | 0:0bb3687e39da | 190 | // initialize the motion sensor |
nightmechanic | 2:59a7d4677474 | 191 | motion_is_init = mpu6050.motion_sensor_init(); |
nightmechanic | 0:0bb3687e39da | 192 | |
nightmechanic | 0:0bb3687e39da | 193 | button_ticker.attach(&button_inth, 0.025); |
nightmechanic | 0:0bb3687e39da | 194 | |
nightmechanic | 0:0bb3687e39da | 195 | wait_ms(1000); //from MPU example |
nightmechanic | 0:0bb3687e39da | 196 | |
nightmechanic | 0:0bb3687e39da | 197 | //Initialize watchdog with a 2 second interval |
nightmechanic | 0:0bb3687e39da | 198 | // wd.Configure(2.0); |
nightmechanic | 0:0bb3687e39da | 199 | |
nightmechanic | 0:0bb3687e39da | 200 | //main loop |
nightmechanic | 0:0bb3687e39da | 201 | while(1) { |
nightmechanic | 0:0bb3687e39da | 202 | |
nightmechanic | 0:0bb3687e39da | 203 | |
nightmechanic | 0:0bb3687e39da | 204 | |
nightmechanic | 0:0bb3687e39da | 205 | if (vbat_low_flag) //low battery |
nightmechanic | 0:0bb3687e39da | 206 | { |
nightmechanic | 0:0bb3687e39da | 207 | light_intensity = LOW_LIGHT_INTENS; //lower light intensity save some battery |
nightmechanic | 0:0bb3687e39da | 208 | if (t_vbat_start == 0) //starting a new cycle |
nightmechanic | 0:0bb3687e39da | 209 | { |
nightmechanic | 0:0bb3687e39da | 210 | t_vbat_start = t.read_ms(); |
nightmechanic | 0:0bb3687e39da | 211 | if (t_vbat_count < 3) //flash only in first 3 cycles |
nightmechanic | 0:0bb3687e39da | 212 | { |
nightmechanic | 0:0bb3687e39da | 213 | change_color(RED , light_intensity); |
nightmechanic | 0:0bb3687e39da | 214 | } |
nightmechanic | 0:0bb3687e39da | 215 | } else if ( ((t.read_ms() - t_vbat_start) > 1000) && (t_vbat_count < 3)) // dark period and flash only in first 3 cycles |
nightmechanic | 0:0bb3687e39da | 216 | { |
nightmechanic | 0:0bb3687e39da | 217 | change_color(RED, 0); |
nightmechanic | 0:0bb3687e39da | 218 | } else // after first 3 cycles return to normal operation (with lower light intensity) |
nightmechanic | 0:0bb3687e39da | 219 | { |
nightmechanic | 0:0bb3687e39da | 220 | change_color((color_type) current_color, light_intensity); |
nightmechanic | 0:0bb3687e39da | 221 | } |
nightmechanic | 0:0bb3687e39da | 222 | if ( (t.read_ms() - t_vbat_start) > 2000) //end of cycle |
nightmechanic | 0:0bb3687e39da | 223 | { |
nightmechanic | 0:0bb3687e39da | 224 | t_vbat_start = 0; |
nightmechanic | 0:0bb3687e39da | 225 | t_vbat_count ++; |
nightmechanic | 0:0bb3687e39da | 226 | } |
nightmechanic | 0:0bb3687e39da | 227 | if (t_vbat_count > 30) // once a minute, starting over |
nightmechanic | 0:0bb3687e39da | 228 | { |
nightmechanic | 0:0bb3687e39da | 229 | t_vbat_count = 0; |
nightmechanic | 0:0bb3687e39da | 230 | } |
nightmechanic | 0:0bb3687e39da | 231 | |
nightmechanic | 0:0bb3687e39da | 232 | } |
nightmechanic | 0:0bb3687e39da | 233 | //battery ok or between low bat flashes, check if button was pressed or released |
nightmechanic | 0:0bb3687e39da | 234 | if (!vbat_low_flag || (t_vbat_count >=3)) |
nightmechanic | 0:0bb3687e39da | 235 | { |
nightmechanic | 0:0bb3687e39da | 236 | if ( (Color_Button_Count >= BUTTON_PRESS_THR) && color_button_released ) |
nightmechanic | 0:0bb3687e39da | 237 | { |
nightmechanic | 0:0bb3687e39da | 238 | __disable_irq(); |
nightmechanic | 0:0bb3687e39da | 239 | color_button_released = FALSE; |
nightmechanic | 0:0bb3687e39da | 240 | Color_Button_Count = 0; |
nightmechanic | 0:0bb3687e39da | 241 | __enable_irq(); |
nightmechanic | 0:0bb3687e39da | 242 | current_color++; |
nightmechanic | 0:0bb3687e39da | 243 | if (current_color >= NUM_OF_COLORS) |
nightmechanic | 0:0bb3687e39da | 244 | { |
nightmechanic | 0:0bb3687e39da | 245 | current_color = BLUE; |
nightmechanic | 0:0bb3687e39da | 246 | } |
nightmechanic | 0:0bb3687e39da | 247 | #ifdef __SERIAL_DEBUG__ |
nightmechanic | 0:0bb3687e39da | 248 | serial.printf("Color Button pressed"); |
nightmechanic | 0:0bb3687e39da | 249 | #endif |
nightmechanic | 0:0bb3687e39da | 250 | change_color((color_type) current_color, light_intensity); |
nightmechanic | 0:0bb3687e39da | 251 | } |
nightmechanic | 0:0bb3687e39da | 252 | |
nightmechanic | 0:0bb3687e39da | 253 | if ((Color_Button_Count >= BUTTON_RELEASE_THR) && !color_button_released) |
nightmechanic | 0:0bb3687e39da | 254 | { |
nightmechanic | 0:0bb3687e39da | 255 | __disable_irq(); |
nightmechanic | 0:0bb3687e39da | 256 | color_button_released = TRUE; |
nightmechanic | 0:0bb3687e39da | 257 | Color_Button_Count = 0; |
nightmechanic | 0:0bb3687e39da | 258 | __enable_irq(); |
nightmechanic | 0:0bb3687e39da | 259 | #ifdef __SERIAL_DEBUG__ |
nightmechanic | 0:0bb3687e39da | 260 | serial.printf("Color Button released"); |
nightmechanic | 0:0bb3687e39da | 261 | #endif |
nightmechanic | 0:0bb3687e39da | 262 | } |
nightmechanic | 0:0bb3687e39da | 263 | } |
nightmechanic | 0:0bb3687e39da | 264 | |
nightmechanic | 0:0bb3687e39da | 265 | |
nightmechanic | 0:0bb3687e39da | 266 | |
nightmechanic | 0:0bb3687e39da | 267 | if (motion_is_init) |
nightmechanic | 0:0bb3687e39da | 268 | { |
nightmechanic | 2:59a7d4677474 | 269 | new_data = mpu6050.motion_update_data(&MPU_data[mpu_pointer], t.read_us()); |
nightmechanic | 0:0bb3687e39da | 270 | } |
nightmechanic | 0:0bb3687e39da | 271 | |
nightmechanic | 0:0bb3687e39da | 272 | if (new_data) |
nightmechanic | 0:0bb3687e39da | 273 | { |
nightmechanic | 0:0bb3687e39da | 274 | |
nightmechanic | 0:0bb3687e39da | 275 | if ((mpu_data_count > MPU_DATA_STABLE) && !sound_is_playing_flag ) |
nightmechanic | 0:0bb3687e39da | 276 | { |
nightmechanic | 0:0bb3687e39da | 277 | //calculate new average |
nightmechanic | 0:0bb3687e39da | 278 | mpu_calc_avg(MPU_data, mpu_pointer, &MPU_avg_data, MPU_AVG_LEN); |
nightmechanic | 0:0bb3687e39da | 279 | |
nightmechanic | 0:0bb3687e39da | 280 | //check if any thresholds are crossed |
nightmechanic | 0:0bb3687e39da | 281 | //acceleration |
nightmechanic | 0:0bb3687e39da | 282 | if ((abs(MPU_avg_data.ax - MPU_data[mpu_pointer].ax) > ACCEL_THR) |
nightmechanic | 0:0bb3687e39da | 283 | || (abs(MPU_avg_data.ay - MPU_data[mpu_pointer].ay) > ACCEL_THR) |
nightmechanic | 0:0bb3687e39da | 284 | || (abs(MPU_avg_data.az - MPU_data[mpu_pointer].az) > ACCEL_THR)) |
nightmechanic | 0:0bb3687e39da | 285 | { |
nightmechanic | 0:0bb3687e39da | 286 | accel_thr_crossed = TRUE; |
nightmechanic | 0:0bb3687e39da | 287 | } |
nightmechanic | 0:0bb3687e39da | 288 | // YPR |
nightmechanic | 0:0bb3687e39da | 289 | temp_diff = abs(MPU_avg_data.yaw - MPU_data[mpu_pointer].yaw); |
nightmechanic | 0:0bb3687e39da | 290 | if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR))) |
nightmechanic | 0:0bb3687e39da | 291 | { |
nightmechanic | 0:0bb3687e39da | 292 | ypr_thr_crossed = TRUE; |
nightmechanic | 0:0bb3687e39da | 293 | } |
nightmechanic | 0:0bb3687e39da | 294 | |
nightmechanic | 0:0bb3687e39da | 295 | temp_diff = abs(MPU_avg_data.pitch - MPU_data[mpu_pointer].pitch); |
nightmechanic | 0:0bb3687e39da | 296 | if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR))) |
nightmechanic | 0:0bb3687e39da | 297 | { |
nightmechanic | 0:0bb3687e39da | 298 | ypr_thr_crossed = TRUE; |
nightmechanic | 0:0bb3687e39da | 299 | } |
nightmechanic | 0:0bb3687e39da | 300 | |
nightmechanic | 0:0bb3687e39da | 301 | temp_diff = abs(MPU_avg_data.roll - MPU_data[mpu_pointer].roll); |
nightmechanic | 0:0bb3687e39da | 302 | if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR))) |
nightmechanic | 0:0bb3687e39da | 303 | { |
nightmechanic | 0:0bb3687e39da | 304 | ypr_thr_crossed = TRUE; |
nightmechanic | 0:0bb3687e39da | 305 | } |
nightmechanic | 0:0bb3687e39da | 306 | |
nightmechanic | 0:0bb3687e39da | 307 | if (accel_thr_crossed) |
nightmechanic | 0:0bb3687e39da | 308 | { |
nightmechanic | 0:0bb3687e39da | 309 | //play clash sound |
nightmechanic | 0:0bb3687e39da | 310 | sound_to_play = CLASH_SOUND; |
nightmechanic | 0:0bb3687e39da | 311 | sound_line = 0; |
nightmechanic | 0:0bb3687e39da | 312 | sound_count = 1; |
nightmechanic | 0:0bb3687e39da | 313 | next_sound_time = t.read_ms(); |
nightmechanic | 0:0bb3687e39da | 314 | sound_is_playing_flag = TRUE; |
nightmechanic | 0:0bb3687e39da | 315 | |
nightmechanic | 0:0bb3687e39da | 316 | } |
nightmechanic | 0:0bb3687e39da | 317 | if (ypr_thr_crossed && !accel_thr_crossed) |
nightmechanic | 0:0bb3687e39da | 318 | { |
nightmechanic | 0:0bb3687e39da | 319 | //play movement sound |
nightmechanic | 0:0bb3687e39da | 320 | sound_to_play = MOVEMENT_SOUND; |
nightmechanic | 0:0bb3687e39da | 321 | sound_line = 0; |
nightmechanic | 0:0bb3687e39da | 322 | sound_count = 1; |
nightmechanic | 0:0bb3687e39da | 323 | next_sound_time = t.read_ms(); |
nightmechanic | 0:0bb3687e39da | 324 | sound_is_playing_flag = TRUE; |
nightmechanic | 0:0bb3687e39da | 325 | } |
nightmechanic | 0:0bb3687e39da | 326 | |
nightmechanic | 0:0bb3687e39da | 327 | accel_thr_crossed = FALSE; |
nightmechanic | 0:0bb3687e39da | 328 | ypr_thr_crossed = FALSE; |
nightmechanic | 0:0bb3687e39da | 329 | |
nightmechanic | 0:0bb3687e39da | 330 | } else { |
nightmechanic | 0:0bb3687e39da | 331 | mpu_data_count++; |
nightmechanic | 0:0bb3687e39da | 332 | } |
nightmechanic | 0:0bb3687e39da | 333 | |
nightmechanic | 0:0bb3687e39da | 334 | mpu_pointer++; |
nightmechanic | 0:0bb3687e39da | 335 | |
nightmechanic | 0:0bb3687e39da | 336 | if (mpu_pointer >= MPU_BUFFER_LEN) |
nightmechanic | 0:0bb3687e39da | 337 | { |
nightmechanic | 0:0bb3687e39da | 338 | mpu_pointer = 0; |
nightmechanic | 0:0bb3687e39da | 339 | } |
nightmechanic | 0:0bb3687e39da | 340 | |
nightmechanic | 0:0bb3687e39da | 341 | new_data = FALSE; |
nightmechanic | 0:0bb3687e39da | 342 | } |
nightmechanic | 0:0bb3687e39da | 343 | |
nightmechanic | 0:0bb3687e39da | 344 | delt_t = t.read_ms() - count; |
nightmechanic | 0:0bb3687e39da | 345 | if (delt_t > 1000) |
nightmechanic | 0:0bb3687e39da | 346 | { // update serial port once per second independent of read rate |
nightmechanic | 0:0bb3687e39da | 347 | |
nightmechanic | 0:0bb3687e39da | 348 | #ifdef __SERIAL_DEBUG__ |
nightmechanic | 0:0bb3687e39da | 349 | //serial.printf("Yaw, Pitch, Roll: %f %f %f\n\r", (float) (yaw / 16384.0f), (float) (pitch/16384.0f), (float) (roll/16384.0f)); |
nightmechanic | 0:0bb3687e39da | 350 | serial.printf("Yaw, Pitch, Roll: %i %i %i\n\r", (int)(yaw * 10), (int)(pitch * 10), (int)(roll * 10)); |
nightmechanic | 0:0bb3687e39da | 351 | serial.printf("Yaw, Pitch, Roll (average): %i %i %i\n\r", MPU_avg_data.yaw, MPU_avg_data.pitch, MPU_avg_data.roll); |
nightmechanic | 0:0bb3687e39da | 352 | |
nightmechanic | 0:0bb3687e39da | 353 | serial.printf("Ax, Ay, Az : %i %i %i\n\r", (int)(ax * 1000), (int)(ay * 1000), (int)(az * 1000)); |
nightmechanic | 0:0bb3687e39da | 354 | serial.printf("Ax, Ay, Az (average): %i %i %i\n\r", MPU_avg_data.ax , MPU_avg_data.ay, MPU_avg_data.az); |
nightmechanic | 0:0bb3687e39da | 355 | |
nightmechanic | 0:0bb3687e39da | 356 | serial.printf("average rate = %f\n\r", (float) sumCount/sum); |
nightmechanic | 0:0bb3687e39da | 357 | |
nightmechanic | 0:0bb3687e39da | 358 | if (sound_is_playing_flag) |
nightmechanic | 0:0bb3687e39da | 359 | { |
nightmechanic | 0:0bb3687e39da | 360 | serial.printf("Sound is playing! \n\r", (float) sumCount/sum); |
nightmechanic | 0:0bb3687e39da | 361 | } |
nightmechanic | 0:0bb3687e39da | 362 | #endif |
nightmechanic | 0:0bb3687e39da | 363 | count = t.read_ms(); |
nightmechanic | 0:0bb3687e39da | 364 | sum = 0; |
nightmechanic | 0:0bb3687e39da | 365 | sumCount = 0; |
nightmechanic | 0:0bb3687e39da | 366 | |
nightmechanic | 0:0bb3687e39da | 367 | // Battery voltage monitor |
nightmechanic | 0:0bb3687e39da | 368 | v_bat_avg_mem[v_bat_avg_pointer] = V_bat.read_u16(); |
nightmechanic | 0:0bb3687e39da | 369 | v_bat_avg_pointer++; |
nightmechanic | 0:0bb3687e39da | 370 | if (v_bat_avg_pointer >= VBAT_AVG_LEN) |
nightmechanic | 0:0bb3687e39da | 371 | { |
nightmechanic | 0:0bb3687e39da | 372 | v_bat_avg_pointer = 0; |
nightmechanic | 0:0bb3687e39da | 373 | v_bat_ready = TRUE; |
nightmechanic | 0:0bb3687e39da | 374 | } |
nightmechanic | 0:0bb3687e39da | 375 | if (v_bat_ready) |
nightmechanic | 0:0bb3687e39da | 376 | { |
nightmechanic | 0:0bb3687e39da | 377 | //calculate the average |
nightmechanic | 0:0bb3687e39da | 378 | v_bat_avg_value = 0; |
nightmechanic | 0:0bb3687e39da | 379 | for (i = 0 ; i < VBAT_AVG_LEN ; i++) |
nightmechanic | 0:0bb3687e39da | 380 | { |
nightmechanic | 0:0bb3687e39da | 381 | v_bat_avg_value += v_bat_avg_mem[i]; |
nightmechanic | 0:0bb3687e39da | 382 | } |
nightmechanic | 0:0bb3687e39da | 383 | |
nightmechanic | 0:0bb3687e39da | 384 | v_bat_avg_value = v_bat_avg_value/VBAT_AVG_LEN; |
nightmechanic | 0:0bb3687e39da | 385 | #ifdef __SERIAL_DEBUG__ |
nightmechanic | 0:0bb3687e39da | 386 | serial.printf("Battery average voltage: %i \n\r", v_bat_avg_value); |
nightmechanic | 0:0bb3687e39da | 387 | #endif |
nightmechanic | 0:0bb3687e39da | 388 | //check voltage |
nightmechanic | 0:0bb3687e39da | 389 | if (v_bat_avg_value < VBAT_THR) |
nightmechanic | 0:0bb3687e39da | 390 | { |
nightmechanic | 0:0bb3687e39da | 391 | vbat_low_flag = TRUE; |
nightmechanic | 0:0bb3687e39da | 392 | } else if ((vbat_low_flag == TRUE) && (v_bat_avg_value > (VBAT_THR+VBAT_HYST))) |
nightmechanic | 0:0bb3687e39da | 393 | { |
nightmechanic | 0:0bb3687e39da | 394 | vbat_low_flag = FALSE; |
nightmechanic | 0:0bb3687e39da | 395 | } |
nightmechanic | 0:0bb3687e39da | 396 | } |
nightmechanic | 0:0bb3687e39da | 397 | |
nightmechanic | 0:0bb3687e39da | 398 | |
nightmechanic | 0:0bb3687e39da | 399 | } |
nightmechanic | 0:0bb3687e39da | 400 | //handle timer overflow - even if a sound is playing there are 5 minutes before overflow |
nightmechanic | 0:0bb3687e39da | 401 | if ((t.read_ms() > (30*60*1000)) && !sound_is_playing_flag) |
nightmechanic | 0:0bb3687e39da | 402 | { |
nightmechanic | 0:0bb3687e39da | 403 | t.reset(); |
nightmechanic | 0:0bb3687e39da | 404 | // reset mpu statistics as well |
nightmechanic | 0:0bb3687e39da | 405 | count = t.read_ms(); |
nightmechanic | 0:0bb3687e39da | 406 | sum = 0; |
nightmechanic | 0:0bb3687e39da | 407 | sumCount = 0; |
nightmechanic | 0:0bb3687e39da | 408 | //reset the low battery variables |
nightmechanic | 0:0bb3687e39da | 409 | t_vbat_start = 0; |
nightmechanic | 0:0bb3687e39da | 410 | t_vbat_count = 0; |
nightmechanic | 0:0bb3687e39da | 411 | } |
nightmechanic | 0:0bb3687e39da | 412 | // kick the dog before the timeout |
nightmechanic | 0:0bb3687e39da | 413 | // wd.Service(); |
nightmechanic | 0:0bb3687e39da | 414 | } |
nightmechanic | 0:0bb3687e39da | 415 | } |
nightmechanic | 0:0bb3687e39da | 416 | |
nightmechanic | 0:0bb3687e39da | 417 | |
nightmechanic | 0:0bb3687e39da | 418 | |
nightmechanic | 0:0bb3687e39da | 419 | |
nightmechanic | 0:0bb3687e39da | 420 | void mpu_calc_avg(MPU_data_type * MPU_data,int mpu_pointer, MPU_data_type * MPU_avg_data, int avg_len) |
nightmechanic | 0:0bb3687e39da | 421 | { |
nightmechanic | 0:0bb3687e39da | 422 | int i = 0; |
nightmechanic | 0:0bb3687e39da | 423 | //MPU_data_type MPU_avg_data; |
nightmechanic | 0:0bb3687e39da | 424 | |
nightmechanic | 0:0bb3687e39da | 425 | MPU_avg_data->ax = 0; |
nightmechanic | 0:0bb3687e39da | 426 | MPU_avg_data->ay = 0; |
nightmechanic | 0:0bb3687e39da | 427 | MPU_avg_data->az = 0; |
nightmechanic | 0:0bb3687e39da | 428 | MPU_avg_data->yaw = 0; |
nightmechanic | 0:0bb3687e39da | 429 | MPU_avg_data->pitch = 0; |
nightmechanic | 0:0bb3687e39da | 430 | MPU_avg_data->roll = 0; |
nightmechanic | 0:0bb3687e39da | 431 | |
nightmechanic | 0:0bb3687e39da | 432 | for (i=0 ; i < avg_len ; i++) |
nightmechanic | 0:0bb3687e39da | 433 | { |
nightmechanic | 0:0bb3687e39da | 434 | mpu_pointer++; |
nightmechanic | 0:0bb3687e39da | 435 | if (mpu_pointer >= MPU_BUFFER_LEN) |
nightmechanic | 0:0bb3687e39da | 436 | { |
nightmechanic | 0:0bb3687e39da | 437 | mpu_pointer = 0; |
nightmechanic | 0:0bb3687e39da | 438 | } |
nightmechanic | 0:0bb3687e39da | 439 | MPU_avg_data->ax += MPU_data[mpu_pointer].ax; |
nightmechanic | 0:0bb3687e39da | 440 | MPU_avg_data->ay += MPU_data[mpu_pointer].ay; |
nightmechanic | 0:0bb3687e39da | 441 | MPU_avg_data->az += MPU_data[mpu_pointer].az; |
nightmechanic | 0:0bb3687e39da | 442 | MPU_avg_data->yaw += MPU_data[mpu_pointer].yaw; |
nightmechanic | 0:0bb3687e39da | 443 | MPU_avg_data->pitch += MPU_data[mpu_pointer].pitch; |
nightmechanic | 0:0bb3687e39da | 444 | MPU_avg_data->roll += MPU_data[mpu_pointer].roll; |
nightmechanic | 0:0bb3687e39da | 445 | } |
nightmechanic | 0:0bb3687e39da | 446 | |
nightmechanic | 0:0bb3687e39da | 447 | MPU_avg_data->ax = MPU_avg_data->ax / avg_len; |
nightmechanic | 0:0bb3687e39da | 448 | MPU_avg_data->ay = MPU_avg_data->ay / avg_len; |
nightmechanic | 0:0bb3687e39da | 449 | MPU_avg_data->az = MPU_avg_data->az / avg_len; |
nightmechanic | 0:0bb3687e39da | 450 | MPU_avg_data->yaw = MPU_avg_data->yaw / avg_len; |
nightmechanic | 0:0bb3687e39da | 451 | MPU_avg_data->pitch = MPU_avg_data->pitch / avg_len; |
nightmechanic | 0:0bb3687e39da | 452 | MPU_avg_data->roll = MPU_avg_data->roll / avg_len; |
nightmechanic | 0:0bb3687e39da | 453 | |
nightmechanic | 0:0bb3687e39da | 454 | //return MPU_avg_data; |
nightmechanic | 0:0bb3687e39da | 455 | } |
nightmechanic | 0:0bb3687e39da | 456 | |
nightmechanic | 0:0bb3687e39da | 457 | void init_color(int *color, int pulsewidth) |
nightmechanic | 0:0bb3687e39da | 458 | { |
nightmechanic | 0:0bb3687e39da | 459 | Startup_Config_OUT.write(1); |
nightmechanic | 0:0bb3687e39da | 460 | wait(1); |
nightmechanic | 0:0bb3687e39da | 461 | if (Startup_Config_IN1.read() == 1) |
nightmechanic | 0:0bb3687e39da | 462 | { |
nightmechanic | 0:0bb3687e39da | 463 | *color = GREEN; |
nightmechanic | 0:0bb3687e39da | 464 | }else if (Startup_Config_IN2.read() == 1) |
nightmechanic | 0:0bb3687e39da | 465 | { |
nightmechanic | 0:0bb3687e39da | 466 | *color = PURPLE; |
nightmechanic | 0:0bb3687e39da | 467 | }else |
nightmechanic | 0:0bb3687e39da | 468 | { |
nightmechanic | 0:0bb3687e39da | 469 | |
nightmechanic | 0:0bb3687e39da | 470 | *color = BLUE; |
nightmechanic | 0:0bb3687e39da | 471 | } |
nightmechanic | 0:0bb3687e39da | 472 | Startup_Config_OUT.write(0); |
nightmechanic | 0:0bb3687e39da | 473 | |
nightmechanic | 0:0bb3687e39da | 474 | Blue_LED.period_ms(LED_PWM_PERIOD); |
nightmechanic | 0:0bb3687e39da | 475 | |
nightmechanic | 0:0bb3687e39da | 476 | change_color((color_type)*color, pulsewidth); |
nightmechanic | 0:0bb3687e39da | 477 | } |
nightmechanic | 0:0bb3687e39da | 478 | |
nightmechanic | 0:0bb3687e39da | 479 | |
nightmechanic | 0:0bb3687e39da | 480 | void change_color(color_type new_color, int new_pulsewidth) |
nightmechanic | 0:0bb3687e39da | 481 | { |
nightmechanic | 0:0bb3687e39da | 482 | Blue_LED.pulsewidth_us(0); |
nightmechanic | 0:0bb3687e39da | 483 | Green_LED.pulsewidth_us(0); |
nightmechanic | 0:0bb3687e39da | 484 | Red_LED.pulsewidth_us(0); |
nightmechanic | 0:0bb3687e39da | 485 | |
nightmechanic | 0:0bb3687e39da | 486 | switch (new_color) { |
nightmechanic | 0:0bb3687e39da | 487 | case BLUE: |
nightmechanic | 0:0bb3687e39da | 488 | Blue_LED.pulsewidth_us(new_pulsewidth); |
nightmechanic | 0:0bb3687e39da | 489 | break; |
nightmechanic | 0:0bb3687e39da | 490 | |
nightmechanic | 0:0bb3687e39da | 491 | case GREEN: |
nightmechanic | 0:0bb3687e39da | 492 | Green_LED.pulsewidth_us(new_pulsewidth); |
nightmechanic | 0:0bb3687e39da | 493 | break; |
nightmechanic | 0:0bb3687e39da | 494 | |
nightmechanic | 0:0bb3687e39da | 495 | case PURPLE: |
nightmechanic | 0:0bb3687e39da | 496 | Blue_LED.pulsewidth_us(new_pulsewidth); |
nightmechanic | 0:0bb3687e39da | 497 | Red_LED.pulsewidth_us(new_pulsewidth); |
nightmechanic | 0:0bb3687e39da | 498 | break; |
nightmechanic | 0:0bb3687e39da | 499 | |
nightmechanic | 0:0bb3687e39da | 500 | case RED: |
nightmechanic | 0:0bb3687e39da | 501 | Red_LED.pulsewidth_us(new_pulsewidth); |
nightmechanic | 0:0bb3687e39da | 502 | break; |
nightmechanic | 0:0bb3687e39da | 503 | |
nightmechanic | 0:0bb3687e39da | 504 | default: |
nightmechanic | 0:0bb3687e39da | 505 | break; |
nightmechanic | 0:0bb3687e39da | 506 | } |
nightmechanic | 0:0bb3687e39da | 507 | } |
nightmechanic | 0:0bb3687e39da | 508 |