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