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@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?

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