Ran Katz / Mbed 2 deprecated LightSaber

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@4:7e4bb0c29d3b, 2016-03-26 (annotated)

Committer:
nightmechanic
Date:
Sat Mar 26 21:36:24 2016 +0000
Revision:
4:7e4bb0c29d3b
Parent:
3:0c2d9355ed8c 
Added source info to the MPU6050 files

Who changed what in which revision?

UserRevisionLine numberNew 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(&current_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