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@0:0bb3687e39da, 2016-03-22 (annotated)

Committer:
nightmechanic
Date:
Tue Mar 22 10:30:16 2016 +0000
Revision:
0:0bb3687e39da
Child:
2:59a7d4677474
First and not so stable version

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 //MPU 6050
nightmechanic 0:0bb3687e39da 54 float sum = 0;
nightmechanic 0:0bb3687e39da 55 uint32_t sumCount = 0;
nightmechanic 0:0bb3687e39da 56
nightmechanic 0:0bb3687e39da 57 //Set up I2C, (SDA,SCL)
nightmechanic 0:0bb3687e39da 58 //I2C MPU_i2c(PB_9, PB_8); //defined in MPU6050.h...
nightmechanic 0:0bb3687e39da 59
nightmechanic 0:0bb3687e39da 60 //MPU interrupt line
nightmechanic 0:0bb3687e39da 61 DigitalIn MPU_Intr(PB_1);
nightmechanic 0:0bb3687e39da 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 bool sound_player(const int sound_table[][6], int table_lines)
nightmechanic 0:0bb3687e39da 81 {
nightmechanic 0:0bb3687e39da 82 int sound_period;
nightmechanic 0:0bb3687e39da 83 int sound_pulse_width_us;
nightmechanic 0:0bb3687e39da 84 if (sound_count > sound_table[sound_line][NUM_STEPS_IDX])
nightmechanic 0:0bb3687e39da 85 {
nightmechanic 0:0bb3687e39da 86 sound_count = 1;
nightmechanic 0:0bb3687e39da 87 sound_line++;
nightmechanic 0:0bb3687e39da 88 if (sound_line >= table_lines)
nightmechanic 0:0bb3687e39da 89 {
nightmechanic 0:0bb3687e39da 90 return FALSE;
nightmechanic 0:0bb3687e39da 91 }
nightmechanic 0:0bb3687e39da 92 }
nightmechanic 0:0bb3687e39da 93
nightmechanic 0:0bb3687e39da 94 sound_period = sound_table[sound_line][INIT_PERIOD_IDX] + ((sound_count-1) * sound_table[sound_line][STEP_PERIOD_IDX]);
nightmechanic 0:0bb3687e39da 95 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 96
nightmechanic 0:0bb3687e39da 97 // there are no checks for 0/negative values of the above - need to make sure that tables are valid.
nightmechanic 0:0bb3687e39da 98 // set PWM parameters for current step
nightmechanic 0:0bb3687e39da 99
nightmechanic 0:0bb3687e39da 100 Speaker_OUT.period_us(sound_period);
nightmechanic 0:0bb3687e39da 101 Speaker_OUT.pulsewidth_us(sound_pulse_width_us);
nightmechanic 0:0bb3687e39da 102
nightmechanic 0:0bb3687e39da 103 //update next_sound_time, step count
nightmechanic 0:0bb3687e39da 104
nightmechanic 0:0bb3687e39da 105 next_sound_time = t.read_ms() + sound_table[sound_line][STEP_DUR_IDX];
nightmechanic 0:0bb3687e39da 106 sound_count++;
nightmechanic 0:0bb3687e39da 107
nightmechanic 0:0bb3687e39da 108 return TRUE;
nightmechanic 0:0bb3687e39da 109 }
nightmechanic 0:0bb3687e39da 110
nightmechanic 0:0bb3687e39da 111 void sound_inth()
nightmechanic 0:0bb3687e39da 112 {
nightmechanic 0:0bb3687e39da 113 if ((t.read_ms() >= next_sound_time) && sound_is_playing_flag )
nightmechanic 0:0bb3687e39da 114 {
nightmechanic 0:0bb3687e39da 115 switch (sound_to_play) {
nightmechanic 0:0bb3687e39da 116
nightmechanic 0:0bb3687e39da 117 case STARTUP_SOUND:
nightmechanic 0:0bb3687e39da 118 sound_is_playing_flag = sound_player(startup_sound_table, STARTUP_TBL_N_LINES);
nightmechanic 0:0bb3687e39da 119 break;
nightmechanic 0:0bb3687e39da 120 case MOVEMENT_SOUND:
nightmechanic 0:0bb3687e39da 121 sound_is_playing_flag = sound_player(movement_sound_table, MOVEMENT_TBL_N_LINES);
nightmechanic 0:0bb3687e39da 122 break;
nightmechanic 0:0bb3687e39da 123 case CLASH_SOUND:
nightmechanic 0:0bb3687e39da 124 sound_is_playing_flag = sound_player(clash_sound_table, CLASH_TBL_N_LINES);
nightmechanic 0:0bb3687e39da 125 break;
nightmechanic 0:0bb3687e39da 126 default:
nightmechanic 0:0bb3687e39da 127 break;
nightmechanic 0:0bb3687e39da 128 }
nightmechanic 0:0bb3687e39da 129 }
nightmechanic 0:0bb3687e39da 130 }
nightmechanic 0:0bb3687e39da 131 int main()
nightmechanic 0:0bb3687e39da 132 {
nightmechanic 0:0bb3687e39da 133 bool motion_is_init = FALSE;
nightmechanic 0:0bb3687e39da 134 bool new_data = FALSE;
nightmechanic 0:0bb3687e39da 135
nightmechanic 0:0bb3687e39da 136 int current_color;
nightmechanic 0:0bb3687e39da 137 int light_intensity = NORM_LIGHT_INTENS;
nightmechanic 0:0bb3687e39da 138
nightmechanic 0:0bb3687e39da 139 int v_bat_avg_mem[VBAT_AVG_LEN];
nightmechanic 0:0bb3687e39da 140 int v_bat_avg_value;
nightmechanic 0:0bb3687e39da 141 int v_bat_avg_pointer = 0;
nightmechanic 0:0bb3687e39da 142 bool v_bat_ready = FALSE;
nightmechanic 0:0bb3687e39da 143 bool vbat_low_flag = FALSE;
nightmechanic 0:0bb3687e39da 144 int t_vbat_count = 0;
nightmechanic 0:0bb3687e39da 145 int t_vbat_start = 0;
nightmechanic 0:0bb3687e39da 146
nightmechanic 0:0bb3687e39da 147 MPU_data_type MPU_data[MPU_BUFFER_LEN];
nightmechanic 0:0bb3687e39da 148 MPU_data_type MPU_avg_data;
nightmechanic 0:0bb3687e39da 149 int mpu_pointer = 0;
nightmechanic 0:0bb3687e39da 150 int mpu_data_count = 0;
nightmechanic 0:0bb3687e39da 151 bool accel_thr_crossed = FALSE;
nightmechanic 0:0bb3687e39da 152 bool ypr_thr_crossed = FALSE;
nightmechanic 0:0bb3687e39da 153 int count = 0;
nightmechanic 0:0bb3687e39da 154
nightmechanic 0:0bb3687e39da 155 int temp_diff;
nightmechanic 0:0bb3687e39da 156
nightmechanic 0:0bb3687e39da 157 int i = 0;
nightmechanic 0:0bb3687e39da 158 #ifdef __SERIAL_DEBUG__
nightmechanic 0:0bb3687e39da 159 STM32_HSI_USB48MHz(); // HSI,USB48MHz,SYSCLK32MHz
nightmechanic 0:0bb3687e39da 160
nightmechanic 0:0bb3687e39da 161 // serial port for debug etc.
nightmechanic 0:0bb3687e39da 162 USBSerial serial;
nightmechanic 0:0bb3687e39da 163
nightmechanic 0:0bb3687e39da 164 //USBSerial serial;
nightmechanic 0:0bb3687e39da 165
nightmechanic 0:0bb3687e39da 166 serial.printf("Initializing main");
nightmechanic 0:0bb3687e39da 167 #endif
nightmechanic 0:0bb3687e39da 168
nightmechanic 0:0bb3687e39da 169 //initialize LEDs : set rate, intensity, and startup color (according to jumper settings)
nightmechanic 0:0bb3687e39da 170
nightmechanic 0:0bb3687e39da 171 init_color(&current_color,light_intensity);
nightmechanic 0:0bb3687e39da 172
nightmechanic 0:0bb3687e39da 173 //Set up I2C
nightmechanic 0:0bb3687e39da 174 MPU_i2c.frequency(300000); // use fast (400 kHz) I2C
nightmechanic 0:0bb3687e39da 175
nightmechanic 0:0bb3687e39da 176 // start "clock"
nightmechanic 0:0bb3687e39da 177 t.start();
nightmechanic 0:0bb3687e39da 178
nightmechanic 0:0bb3687e39da 179 // attach and set sound interrupt handler (every 10msec)
nightmechanic 0:0bb3687e39da 180 sound_ticker.attach(&sound_inth, 0.01);
nightmechanic 0:0bb3687e39da 181 // play startup sound
nightmechanic 0:0bb3687e39da 182 sound_to_play = STARTUP_SOUND;
nightmechanic 0:0bb3687e39da 183 sound_line = 0;
nightmechanic 0:0bb3687e39da 184 sound_count = 1;
nightmechanic 0:0bb3687e39da 185 next_sound_time = t.read_ms();
nightmechanic 0:0bb3687e39da 186 sound_is_playing_flag = TRUE;
nightmechanic 0:0bb3687e39da 187
nightmechanic 0:0bb3687e39da 188 // initialize the motion sensor
nightmechanic 0:0bb3687e39da 189 motion_is_init = motion_sensor_init();
nightmechanic 0:0bb3687e39da 190
nightmechanic 0:0bb3687e39da 191 button_ticker.attach(&button_inth, 0.025);
nightmechanic 0:0bb3687e39da 192
nightmechanic 0:0bb3687e39da 193 wait_ms(1000); //from MPU example
nightmechanic 0:0bb3687e39da 194
nightmechanic 0:0bb3687e39da 195 //Initialize watchdog with a 2 second interval
nightmechanic 0:0bb3687e39da 196 // wd.Configure(2.0);
nightmechanic 0:0bb3687e39da 197
nightmechanic 0:0bb3687e39da 198 //main loop
nightmechanic 0:0bb3687e39da 199 while(1) {
nightmechanic 0:0bb3687e39da 200
nightmechanic 0:0bb3687e39da 201
nightmechanic 0:0bb3687e39da 202
nightmechanic 0:0bb3687e39da 203 if (vbat_low_flag) //low battery
nightmechanic 0:0bb3687e39da 204 {
nightmechanic 0:0bb3687e39da 205 light_intensity = LOW_LIGHT_INTENS; //lower light intensity save some battery
nightmechanic 0:0bb3687e39da 206 if (t_vbat_start == 0) //starting a new cycle
nightmechanic 0:0bb3687e39da 207 {
nightmechanic 0:0bb3687e39da 208 t_vbat_start = t.read_ms();
nightmechanic 0:0bb3687e39da 209 if (t_vbat_count < 3) //flash only in first 3 cycles
nightmechanic 0:0bb3687e39da 210 {
nightmechanic 0:0bb3687e39da 211 change_color(RED , light_intensity);
nightmechanic 0:0bb3687e39da 212 }
nightmechanic 0:0bb3687e39da 213 } 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 214 {
nightmechanic 0:0bb3687e39da 215 change_color(RED, 0);
nightmechanic 0:0bb3687e39da 216 } else // after first 3 cycles return to normal operation (with lower light intensity)
nightmechanic 0:0bb3687e39da 217 {
nightmechanic 0:0bb3687e39da 218 change_color((color_type) current_color, light_intensity);
nightmechanic 0:0bb3687e39da 219 }
nightmechanic 0:0bb3687e39da 220 if ( (t.read_ms() - t_vbat_start) > 2000) //end of cycle
nightmechanic 0:0bb3687e39da 221 {
nightmechanic 0:0bb3687e39da 222 t_vbat_start = 0;
nightmechanic 0:0bb3687e39da 223 t_vbat_count ++;
nightmechanic 0:0bb3687e39da 224 }
nightmechanic 0:0bb3687e39da 225 if (t_vbat_count > 30) // once a minute, starting over
nightmechanic 0:0bb3687e39da 226 {
nightmechanic 0:0bb3687e39da 227 t_vbat_count = 0;
nightmechanic 0:0bb3687e39da 228 }
nightmechanic 0:0bb3687e39da 229
nightmechanic 0:0bb3687e39da 230 }
nightmechanic 0:0bb3687e39da 231 //battery ok or between low bat flashes, check if button was pressed or released
nightmechanic 0:0bb3687e39da 232 if (!vbat_low_flag || (t_vbat_count >=3))
nightmechanic 0:0bb3687e39da 233 {
nightmechanic 0:0bb3687e39da 234 if ( (Color_Button_Count >= BUTTON_PRESS_THR) && color_button_released )
nightmechanic 0:0bb3687e39da 235 {
nightmechanic 0:0bb3687e39da 236 __disable_irq();
nightmechanic 0:0bb3687e39da 237 color_button_released = FALSE;
nightmechanic 0:0bb3687e39da 238 Color_Button_Count = 0;
nightmechanic 0:0bb3687e39da 239 __enable_irq();
nightmechanic 0:0bb3687e39da 240 current_color++;
nightmechanic 0:0bb3687e39da 241 if (current_color >= NUM_OF_COLORS)
nightmechanic 0:0bb3687e39da 242 {
nightmechanic 0:0bb3687e39da 243 current_color = BLUE;
nightmechanic 0:0bb3687e39da 244 }
nightmechanic 0:0bb3687e39da 245 #ifdef __SERIAL_DEBUG__
nightmechanic 0:0bb3687e39da 246 serial.printf("Color Button pressed");
nightmechanic 0:0bb3687e39da 247 #endif
nightmechanic 0:0bb3687e39da 248 change_color((color_type) current_color, light_intensity);
nightmechanic 0:0bb3687e39da 249 }
nightmechanic 0:0bb3687e39da 250
nightmechanic 0:0bb3687e39da 251 if ((Color_Button_Count >= BUTTON_RELEASE_THR) && !color_button_released)
nightmechanic 0:0bb3687e39da 252 {
nightmechanic 0:0bb3687e39da 253 __disable_irq();
nightmechanic 0:0bb3687e39da 254 color_button_released = TRUE;
nightmechanic 0:0bb3687e39da 255 Color_Button_Count = 0;
nightmechanic 0:0bb3687e39da 256 __enable_irq();
nightmechanic 0:0bb3687e39da 257 #ifdef __SERIAL_DEBUG__
nightmechanic 0:0bb3687e39da 258 serial.printf("Color Button released");
nightmechanic 0:0bb3687e39da 259 #endif
nightmechanic 0:0bb3687e39da 260 }
nightmechanic 0:0bb3687e39da 261 }
nightmechanic 0:0bb3687e39da 262
nightmechanic 0:0bb3687e39da 263
nightmechanic 0:0bb3687e39da 264
nightmechanic 0:0bb3687e39da 265 if (motion_is_init)
nightmechanic 0:0bb3687e39da 266 {
nightmechanic 0:0bb3687e39da 267 new_data = motion_update_data(&MPU_data[mpu_pointer]);
nightmechanic 0:0bb3687e39da 268 }
nightmechanic 0:0bb3687e39da 269
nightmechanic 0:0bb3687e39da 270 if (new_data)
nightmechanic 0:0bb3687e39da 271 {
nightmechanic 0:0bb3687e39da 272
nightmechanic 0:0bb3687e39da 273 if ((mpu_data_count > MPU_DATA_STABLE) && !sound_is_playing_flag )
nightmechanic 0:0bb3687e39da 274 {
nightmechanic 0:0bb3687e39da 275 //calculate new average
nightmechanic 0:0bb3687e39da 276 mpu_calc_avg(MPU_data, mpu_pointer, &MPU_avg_data, MPU_AVG_LEN);
nightmechanic 0:0bb3687e39da 277
nightmechanic 0:0bb3687e39da 278 //check if any thresholds are crossed
nightmechanic 0:0bb3687e39da 279 //acceleration
nightmechanic 0:0bb3687e39da 280 if ((abs(MPU_avg_data.ax - MPU_data[mpu_pointer].ax) > ACCEL_THR)
nightmechanic 0:0bb3687e39da 281 || (abs(MPU_avg_data.ay - MPU_data[mpu_pointer].ay) > ACCEL_THR)
nightmechanic 0:0bb3687e39da 282 || (abs(MPU_avg_data.az - MPU_data[mpu_pointer].az) > ACCEL_THR))
nightmechanic 0:0bb3687e39da 283 {
nightmechanic 0:0bb3687e39da 284 accel_thr_crossed = TRUE;
nightmechanic 0:0bb3687e39da 285 }
nightmechanic 0:0bb3687e39da 286 // YPR
nightmechanic 0:0bb3687e39da 287 temp_diff = abs(MPU_avg_data.yaw - MPU_data[mpu_pointer].yaw);
nightmechanic 0:0bb3687e39da 288 if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
nightmechanic 0:0bb3687e39da 289 {
nightmechanic 0:0bb3687e39da 290 ypr_thr_crossed = TRUE;
nightmechanic 0:0bb3687e39da 291 }
nightmechanic 0:0bb3687e39da 292
nightmechanic 0:0bb3687e39da 293 temp_diff = abs(MPU_avg_data.pitch - MPU_data[mpu_pointer].pitch);
nightmechanic 0:0bb3687e39da 294 if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
nightmechanic 0:0bb3687e39da 295 {
nightmechanic 0:0bb3687e39da 296 ypr_thr_crossed = TRUE;
nightmechanic 0:0bb3687e39da 297 }
nightmechanic 0:0bb3687e39da 298
nightmechanic 0:0bb3687e39da 299 temp_diff = abs(MPU_avg_data.roll - MPU_data[mpu_pointer].roll);
nightmechanic 0:0bb3687e39da 300 if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
nightmechanic 0:0bb3687e39da 301 {
nightmechanic 0:0bb3687e39da 302 ypr_thr_crossed = TRUE;
nightmechanic 0:0bb3687e39da 303 }
nightmechanic 0:0bb3687e39da 304
nightmechanic 0:0bb3687e39da 305 if (accel_thr_crossed)
nightmechanic 0:0bb3687e39da 306 {
nightmechanic 0:0bb3687e39da 307 //play clash sound
nightmechanic 0:0bb3687e39da 308 sound_to_play = CLASH_SOUND;
nightmechanic 0:0bb3687e39da 309 sound_line = 0;
nightmechanic 0:0bb3687e39da 310 sound_count = 1;
nightmechanic 0:0bb3687e39da 311 next_sound_time = t.read_ms();
nightmechanic 0:0bb3687e39da 312 sound_is_playing_flag = TRUE;
nightmechanic 0:0bb3687e39da 313
nightmechanic 0:0bb3687e39da 314 }
nightmechanic 0:0bb3687e39da 315 if (ypr_thr_crossed && !accel_thr_crossed)
nightmechanic 0:0bb3687e39da 316 {
nightmechanic 0:0bb3687e39da 317 //play movement sound
nightmechanic 0:0bb3687e39da 318 sound_to_play = MOVEMENT_SOUND;
nightmechanic 0:0bb3687e39da 319 sound_line = 0;
nightmechanic 0:0bb3687e39da 320 sound_count = 1;
nightmechanic 0:0bb3687e39da 321 next_sound_time = t.read_ms();
nightmechanic 0:0bb3687e39da 322 sound_is_playing_flag = TRUE;
nightmechanic 0:0bb3687e39da 323 }
nightmechanic 0:0bb3687e39da 324
nightmechanic 0:0bb3687e39da 325 accel_thr_crossed = FALSE;
nightmechanic 0:0bb3687e39da 326 ypr_thr_crossed = FALSE;
nightmechanic 0:0bb3687e39da 327
nightmechanic 0:0bb3687e39da 328 } else {
nightmechanic 0:0bb3687e39da 329 mpu_data_count++;
nightmechanic 0:0bb3687e39da 330 }
nightmechanic 0:0bb3687e39da 331
nightmechanic 0:0bb3687e39da 332 mpu_pointer++;
nightmechanic 0:0bb3687e39da 333
nightmechanic 0:0bb3687e39da 334 if (mpu_pointer >= MPU_BUFFER_LEN)
nightmechanic 0:0bb3687e39da 335 {
nightmechanic 0:0bb3687e39da 336 mpu_pointer = 0;
nightmechanic 0:0bb3687e39da 337 }
nightmechanic 0:0bb3687e39da 338
nightmechanic 0:0bb3687e39da 339 new_data = FALSE;
nightmechanic 0:0bb3687e39da 340 }
nightmechanic 0:0bb3687e39da 341
nightmechanic 0:0bb3687e39da 342 delt_t = t.read_ms() - count;
nightmechanic 0:0bb3687e39da 343 if (delt_t > 1000)
nightmechanic 0:0bb3687e39da 344 { // update serial port once per second independent of read rate
nightmechanic 0:0bb3687e39da 345
nightmechanic 0:0bb3687e39da 346 #ifdef __SERIAL_DEBUG__
nightmechanic 0:0bb3687e39da 347 //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 348 serial.printf("Yaw, Pitch, Roll: %i %i %i\n\r", (int)(yaw * 10), (int)(pitch * 10), (int)(roll * 10));
nightmechanic 0:0bb3687e39da 349 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 350
nightmechanic 0:0bb3687e39da 351 serial.printf("Ax, Ay, Az : %i %i %i\n\r", (int)(ax * 1000), (int)(ay * 1000), (int)(az * 1000));
nightmechanic 0:0bb3687e39da 352 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 353
nightmechanic 0:0bb3687e39da 354 serial.printf("average rate = %f\n\r", (float) sumCount/sum);
nightmechanic 0:0bb3687e39da 355
nightmechanic 0:0bb3687e39da 356 if (sound_is_playing_flag)
nightmechanic 0:0bb3687e39da 357 {
nightmechanic 0:0bb3687e39da 358 serial.printf("Sound is playing! \n\r", (float) sumCount/sum);
nightmechanic 0:0bb3687e39da 359 }
nightmechanic 0:0bb3687e39da 360 #endif
nightmechanic 0:0bb3687e39da 361 count = t.read_ms();
nightmechanic 0:0bb3687e39da 362 sum = 0;
nightmechanic 0:0bb3687e39da 363 sumCount = 0;
nightmechanic 0:0bb3687e39da 364
nightmechanic 0:0bb3687e39da 365 // Battery voltage monitor
nightmechanic 0:0bb3687e39da 366 v_bat_avg_mem[v_bat_avg_pointer] = V_bat.read_u16();
nightmechanic 0:0bb3687e39da 367 v_bat_avg_pointer++;
nightmechanic 0:0bb3687e39da 368 if (v_bat_avg_pointer >= VBAT_AVG_LEN)
nightmechanic 0:0bb3687e39da 369 {
nightmechanic 0:0bb3687e39da 370 v_bat_avg_pointer = 0;
nightmechanic 0:0bb3687e39da 371 v_bat_ready = TRUE;
nightmechanic 0:0bb3687e39da 372 }
nightmechanic 0:0bb3687e39da 373 if (v_bat_ready)
nightmechanic 0:0bb3687e39da 374 {
nightmechanic 0:0bb3687e39da 375 //calculate the average
nightmechanic 0:0bb3687e39da 376 v_bat_avg_value = 0;
nightmechanic 0:0bb3687e39da 377 for (i = 0 ; i < VBAT_AVG_LEN ; i++)
nightmechanic 0:0bb3687e39da 378 {
nightmechanic 0:0bb3687e39da 379 v_bat_avg_value += v_bat_avg_mem[i];
nightmechanic 0:0bb3687e39da 380 }
nightmechanic 0:0bb3687e39da 381
nightmechanic 0:0bb3687e39da 382 v_bat_avg_value = v_bat_avg_value/VBAT_AVG_LEN;
nightmechanic 0:0bb3687e39da 383 #ifdef __SERIAL_DEBUG__
nightmechanic 0:0bb3687e39da 384 serial.printf("Battery average voltage: %i \n\r", v_bat_avg_value);
nightmechanic 0:0bb3687e39da 385 #endif
nightmechanic 0:0bb3687e39da 386 //check voltage
nightmechanic 0:0bb3687e39da 387 if (v_bat_avg_value < VBAT_THR)
nightmechanic 0:0bb3687e39da 388 {
nightmechanic 0:0bb3687e39da 389 vbat_low_flag = TRUE;
nightmechanic 0:0bb3687e39da 390 } else if ((vbat_low_flag == TRUE) && (v_bat_avg_value > (VBAT_THR+VBAT_HYST)))
nightmechanic 0:0bb3687e39da 391 {
nightmechanic 0:0bb3687e39da 392 vbat_low_flag = FALSE;
nightmechanic 0:0bb3687e39da 393 }
nightmechanic 0:0bb3687e39da 394 }
nightmechanic 0:0bb3687e39da 395
nightmechanic 0:0bb3687e39da 396
nightmechanic 0:0bb3687e39da 397 }
nightmechanic 0:0bb3687e39da 398 //handle timer overflow - even if a sound is playing there are 5 minutes before overflow
nightmechanic 0:0bb3687e39da 399 if ((t.read_ms() > (30*60*1000)) && !sound_is_playing_flag)
nightmechanic 0:0bb3687e39da 400 {
nightmechanic 0:0bb3687e39da 401 t.reset();
nightmechanic 0:0bb3687e39da 402 // reset mpu statistics as well
nightmechanic 0:0bb3687e39da 403 count = t.read_ms();
nightmechanic 0:0bb3687e39da 404 sum = 0;
nightmechanic 0:0bb3687e39da 405 sumCount = 0;
nightmechanic 0:0bb3687e39da 406 //reset the low battery variables
nightmechanic 0:0bb3687e39da 407 t_vbat_start = 0;
nightmechanic 0:0bb3687e39da 408 t_vbat_count = 0;
nightmechanic 0:0bb3687e39da 409 }
nightmechanic 0:0bb3687e39da 410 // kick the dog before the timeout
nightmechanic 0:0bb3687e39da 411 // wd.Service();
nightmechanic 0:0bb3687e39da 412 }
nightmechanic 0:0bb3687e39da 413 }
nightmechanic 0:0bb3687e39da 414
nightmechanic 0:0bb3687e39da 415
nightmechanic 0:0bb3687e39da 416 bool motion_sensor_init()
nightmechanic 0:0bb3687e39da 417 {
nightmechanic 0:0bb3687e39da 418
nightmechanic 0:0bb3687e39da 419
nightmechanic 0:0bb3687e39da 420 // Read the WHO_AM_I register, this is a good test of communication
nightmechanic 0:0bb3687e39da 421 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
nightmechanic 0:0bb3687e39da 422 //serial.printf("I AM 0x%x\n\r", whoami);
nightmechanic 0:0bb3687e39da 423 //serial.printf("I SHOULD BE 0x68\n\r");
nightmechanic 0:0bb3687e39da 424
nightmechanic 0:0bb3687e39da 425 if (whoami == 0x68) { // WHO_AM_I should always be 0x68
nightmechanic 0:0bb3687e39da 426 // serial.printf("MPU6050 is online...");
nightmechanic 0:0bb3687e39da 427 wait(1);
nightmechanic 0:0bb3687e39da 428
nightmechanic 0:0bb3687e39da 429
nightmechanic 0:0bb3687e39da 430 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
nightmechanic 0:0bb3687e39da 431 /*
nightmechanic 0:0bb3687e39da 432 serial.printf("x-axis self test: acceleration trim within : ");
nightmechanic 0:0bb3687e39da 433 serial.printf("%f", SelfTest[0]);
nightmechanic 0:0bb3687e39da 434 serial.printf("% of factory value \n\r");
nightmechanic 0:0bb3687e39da 435 serial.printf("y-axis self test: acceleration trim within : ");
nightmechanic 0:0bb3687e39da 436 serial.printf("%f", SelfTest[1]);
nightmechanic 0:0bb3687e39da 437 serial.printf("% of factory value \n\r");
nightmechanic 0:0bb3687e39da 438 serial.printf("z-axis self test: acceleration trim within : ");
nightmechanic 0:0bb3687e39da 439 serial.printf("%f", SelfTest[2]);
nightmechanic 0:0bb3687e39da 440 serial.printf("% of factory value \n\r");
nightmechanic 0:0bb3687e39da 441 serial.printf("x-axis self test: gyration trim within : ");
nightmechanic 0:0bb3687e39da 442 serial.printf("%f", SelfTest[3]);
nightmechanic 0:0bb3687e39da 443 serial.printf("% of factory value \n\r");
nightmechanic 0:0bb3687e39da 444 serial.printf("y-axis self test: gyration trim within : ");
nightmechanic 0:0bb3687e39da 445 serial.printf("%f", SelfTest[4]);
nightmechanic 0:0bb3687e39da 446 serial.printf("% of factory value \n\r");
nightmechanic 0:0bb3687e39da 447 serial.printf("z-axis self test: gyration trim within : ");
nightmechanic 0:0bb3687e39da 448 serial.printf("%f", SelfTest[5]);
nightmechanic 0:0bb3687e39da 449 serial.printf("% of factory value \n\r");
nightmechanic 0:0bb3687e39da 450 */
nightmechanic 0:0bb3687e39da 451 wait(1);
nightmechanic 0:0bb3687e39da 452
nightmechanic 0:0bb3687e39da 453 if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
nightmechanic 0:0bb3687e39da 454 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
nightmechanic 0:0bb3687e39da 455 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
nightmechanic 0:0bb3687e39da 456 mpu6050.initMPU6050();
nightmechanic 0:0bb3687e39da 457 //serial.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
nightmechanic 0:0bb3687e39da 458
nightmechanic 0:0bb3687e39da 459 return TRUE;
nightmechanic 0:0bb3687e39da 460 } else {
nightmechanic 0:0bb3687e39da 461 //serial.printf("Device did not the pass self-test!\n\r");
nightmechanic 0:0bb3687e39da 462 return FALSE;
nightmechanic 0:0bb3687e39da 463
nightmechanic 0:0bb3687e39da 464 }
nightmechanic 0:0bb3687e39da 465 } else {
nightmechanic 0:0bb3687e39da 466 //serial.printf("Could not connect to MPU6050: \n\r");
nightmechanic 0:0bb3687e39da 467 //serial.printf("%#x \n", whoami);
nightmechanic 0:0bb3687e39da 468
nightmechanic 0:0bb3687e39da 469 return FALSE;
nightmechanic 0:0bb3687e39da 470 }
nightmechanic 0:0bb3687e39da 471
nightmechanic 0:0bb3687e39da 472
nightmechanic 0:0bb3687e39da 473 }
nightmechanic 0:0bb3687e39da 474
nightmechanic 0:0bb3687e39da 475 bool motion_update_data(MPU_data_type *new_data)
nightmechanic 0:0bb3687e39da 476 {
nightmechanic 0:0bb3687e39da 477 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {
nightmechanic 0:0bb3687e39da 478 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
nightmechanic 0:0bb3687e39da 479 mpu6050.getAres();
nightmechanic 0:0bb3687e39da 480
nightmechanic 0:0bb3687e39da 481 // Now we'll calculate the accleration value into actual g's
nightmechanic 0:0bb3687e39da 482 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
nightmechanic 0:0bb3687e39da 483 ay = (float)accelCount[1]*aRes - accelBias[1];
nightmechanic 0:0bb3687e39da 484 az = (float)accelCount[2]*aRes - accelBias[2];
nightmechanic 0:0bb3687e39da 485
nightmechanic 0:0bb3687e39da 486 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
nightmechanic 0:0bb3687e39da 487 mpu6050.getGres();
nightmechanic 0:0bb3687e39da 488
nightmechanic 0:0bb3687e39da 489 // Calculate the gyro value into actual degrees per second
nightmechanic 0:0bb3687e39da 490 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
nightmechanic 0:0bb3687e39da 491 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
nightmechanic 0:0bb3687e39da 492 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
nightmechanic 0:0bb3687e39da 493
nightmechanic 0:0bb3687e39da 494 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
nightmechanic 0:0bb3687e39da 495 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
nightmechanic 0:0bb3687e39da 496
nightmechanic 0:0bb3687e39da 497
nightmechanic 0:0bb3687e39da 498 Now = t.read_us();
nightmechanic 0:0bb3687e39da 499 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
nightmechanic 0:0bb3687e39da 500 lastUpdate = Now;
nightmechanic 0:0bb3687e39da 501
nightmechanic 0:0bb3687e39da 502 sum += deltat;
nightmechanic 0:0bb3687e39da 503 sumCount++;
nightmechanic 0:0bb3687e39da 504
nightmechanic 0:0bb3687e39da 505 if(lastUpdate - firstUpdate > 10000000.0f) {
nightmechanic 0:0bb3687e39da 506 beta = 0.04; // decrease filter gain after stabilized
nightmechanic 0:0bb3687e39da 507 zeta = 0.015; // increase bias drift gain after stabilized
nightmechanic 0:0bb3687e39da 508 }
nightmechanic 0:0bb3687e39da 509
nightmechanic 0:0bb3687e39da 510 // Pass gyro rate as rad/s
nightmechanic 0:0bb3687e39da 511 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
nightmechanic 0:0bb3687e39da 512
nightmechanic 0:0bb3687e39da 513
nightmechanic 0:0bb3687e39da 514
nightmechanic 0:0bb3687e39da 515
nightmechanic 0:0bb3687e39da 516 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
nightmechanic 0:0bb3687e39da 517 // In this coordinate system, the positive z-axis is down toward Earth.
nightmechanic 0:0bb3687e39da 518 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
nightmechanic 0:0bb3687e39da 519 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
nightmechanic 0:0bb3687e39da 520 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
nightmechanic 0:0bb3687e39da 521 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
nightmechanic 0:0bb3687e39da 522 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
nightmechanic 0:0bb3687e39da 523 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
nightmechanic 0:0bb3687e39da 524 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
nightmechanic 0:0bb3687e39da 525 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
nightmechanic 0:0bb3687e39da 526 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
nightmechanic 0:0bb3687e39da 527 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
nightmechanic 0:0bb3687e39da 528 pitch *= 180.0f / PI;
nightmechanic 0:0bb3687e39da 529 yaw *= 180.0f / PI;
nightmechanic 0:0bb3687e39da 530 roll *= 180.0f / PI;
nightmechanic 0:0bb3687e39da 531 /*
nightmechanic 0:0bb3687e39da 532 new_data->ax = (int) (ax / 16384.0f);
nightmechanic 0:0bb3687e39da 533 new_data->ay = (int) (ay / 16384.0f);
nightmechanic 0:0bb3687e39da 534 new_data->az = (int) (az / 16384.0f);
nightmechanic 0:0bb3687e39da 535 new_data->yaw = (int) (yaw / 16384.0f);
nightmechanic 0:0bb3687e39da 536 new_data->pitch = (int) (pitch / 16384.0f);
nightmechanic 0:0bb3687e39da 537 new_data->roll = (int) (roll / 16384.0f);
nightmechanic 0:0bb3687e39da 538 */
nightmechanic 0:0bb3687e39da 539 new_data->ax = (int) (ax * 1000);
nightmechanic 0:0bb3687e39da 540 new_data->ay = (int) (ay * 1000);
nightmechanic 0:0bb3687e39da 541 new_data->az = (int) (az * 1000);
nightmechanic 0:0bb3687e39da 542 new_data->yaw = (int) (yaw * 10);
nightmechanic 0:0bb3687e39da 543 new_data->pitch = (int) (pitch * 10);
nightmechanic 0:0bb3687e39da 544 new_data->roll = (int) (roll * 10);
nightmechanic 0:0bb3687e39da 545 return TRUE;
nightmechanic 0:0bb3687e39da 546
nightmechanic 0:0bb3687e39da 547 } else {
nightmechanic 0:0bb3687e39da 548 return FALSE;
nightmechanic 0:0bb3687e39da 549 }
nightmechanic 0:0bb3687e39da 550
nightmechanic 0:0bb3687e39da 551 }
nightmechanic 0:0bb3687e39da 552
nightmechanic 0:0bb3687e39da 553 void mpu_calc_avg(MPU_data_type * MPU_data,int mpu_pointer, MPU_data_type * MPU_avg_data, int avg_len)
nightmechanic 0:0bb3687e39da 554 {
nightmechanic 0:0bb3687e39da 555 int i = 0;
nightmechanic 0:0bb3687e39da 556 //MPU_data_type MPU_avg_data;
nightmechanic 0:0bb3687e39da 557
nightmechanic 0:0bb3687e39da 558 MPU_avg_data->ax = 0;
nightmechanic 0:0bb3687e39da 559 MPU_avg_data->ay = 0;
nightmechanic 0:0bb3687e39da 560 MPU_avg_data->az = 0;
nightmechanic 0:0bb3687e39da 561 MPU_avg_data->yaw = 0;
nightmechanic 0:0bb3687e39da 562 MPU_avg_data->pitch = 0;
nightmechanic 0:0bb3687e39da 563 MPU_avg_data->roll = 0;
nightmechanic 0:0bb3687e39da 564
nightmechanic 0:0bb3687e39da 565 for (i=0 ; i < avg_len ; i++)
nightmechanic 0:0bb3687e39da 566 {
nightmechanic 0:0bb3687e39da 567 mpu_pointer++;
nightmechanic 0:0bb3687e39da 568 if (mpu_pointer >= MPU_BUFFER_LEN)
nightmechanic 0:0bb3687e39da 569 {
nightmechanic 0:0bb3687e39da 570 mpu_pointer = 0;
nightmechanic 0:0bb3687e39da 571 }
nightmechanic 0:0bb3687e39da 572 MPU_avg_data->ax += MPU_data[mpu_pointer].ax;
nightmechanic 0:0bb3687e39da 573 MPU_avg_data->ay += MPU_data[mpu_pointer].ay;
nightmechanic 0:0bb3687e39da 574 MPU_avg_data->az += MPU_data[mpu_pointer].az;
nightmechanic 0:0bb3687e39da 575 MPU_avg_data->yaw += MPU_data[mpu_pointer].yaw;
nightmechanic 0:0bb3687e39da 576 MPU_avg_data->pitch += MPU_data[mpu_pointer].pitch;
nightmechanic 0:0bb3687e39da 577 MPU_avg_data->roll += MPU_data[mpu_pointer].roll;
nightmechanic 0:0bb3687e39da 578 }
nightmechanic 0:0bb3687e39da 579
nightmechanic 0:0bb3687e39da 580 MPU_avg_data->ax = MPU_avg_data->ax / avg_len;
nightmechanic 0:0bb3687e39da 581 MPU_avg_data->ay = MPU_avg_data->ay / avg_len;
nightmechanic 0:0bb3687e39da 582 MPU_avg_data->az = MPU_avg_data->az / avg_len;
nightmechanic 0:0bb3687e39da 583 MPU_avg_data->yaw = MPU_avg_data->yaw / avg_len;
nightmechanic 0:0bb3687e39da 584 MPU_avg_data->pitch = MPU_avg_data->pitch / avg_len;
nightmechanic 0:0bb3687e39da 585 MPU_avg_data->roll = MPU_avg_data->roll / avg_len;
nightmechanic 0:0bb3687e39da 586
nightmechanic 0:0bb3687e39da 587 //return MPU_avg_data;
nightmechanic 0:0bb3687e39da 588 }
nightmechanic 0:0bb3687e39da 589
nightmechanic 0:0bb3687e39da 590 void init_color(int *color, int pulsewidth)
nightmechanic 0:0bb3687e39da 591 {
nightmechanic 0:0bb3687e39da 592 Startup_Config_OUT.write(1);
nightmechanic 0:0bb3687e39da 593 wait(1);
nightmechanic 0:0bb3687e39da 594 if (Startup_Config_IN1.read() == 1)
nightmechanic 0:0bb3687e39da 595 {
nightmechanic 0:0bb3687e39da 596 *color = GREEN;
nightmechanic 0:0bb3687e39da 597 }else if (Startup_Config_IN2.read() == 1)
nightmechanic 0:0bb3687e39da 598 {
nightmechanic 0:0bb3687e39da 599 *color = PURPLE;
nightmechanic 0:0bb3687e39da 600 }else
nightmechanic 0:0bb3687e39da 601 {
nightmechanic 0:0bb3687e39da 602
nightmechanic 0:0bb3687e39da 603 *color = BLUE;
nightmechanic 0:0bb3687e39da 604 }
nightmechanic 0:0bb3687e39da 605 Startup_Config_OUT.write(0);
nightmechanic 0:0bb3687e39da 606
nightmechanic 0:0bb3687e39da 607 Blue_LED.period_ms(LED_PWM_PERIOD);
nightmechanic 0:0bb3687e39da 608
nightmechanic 0:0bb3687e39da 609 change_color((color_type)*color, pulsewidth);
nightmechanic 0:0bb3687e39da 610 }
nightmechanic 0:0bb3687e39da 611
nightmechanic 0:0bb3687e39da 612
nightmechanic 0:0bb3687e39da 613 void change_color(color_type new_color, int new_pulsewidth)
nightmechanic 0:0bb3687e39da 614 {
nightmechanic 0:0bb3687e39da 615 Blue_LED.pulsewidth_us(0);
nightmechanic 0:0bb3687e39da 616 Green_LED.pulsewidth_us(0);
nightmechanic 0:0bb3687e39da 617 Red_LED.pulsewidth_us(0);
nightmechanic 0:0bb3687e39da 618
nightmechanic 0:0bb3687e39da 619 switch (new_color) {
nightmechanic 0:0bb3687e39da 620 case BLUE:
nightmechanic 0:0bb3687e39da 621 Blue_LED.pulsewidth_us(new_pulsewidth);
nightmechanic 0:0bb3687e39da 622 break;
nightmechanic 0:0bb3687e39da 623
nightmechanic 0:0bb3687e39da 624 case GREEN:
nightmechanic 0:0bb3687e39da 625 Green_LED.pulsewidth_us(new_pulsewidth);
nightmechanic 0:0bb3687e39da 626 break;
nightmechanic 0:0bb3687e39da 627
nightmechanic 0:0bb3687e39da 628 case PURPLE:
nightmechanic 0:0bb3687e39da 629 Blue_LED.pulsewidth_us(new_pulsewidth);
nightmechanic 0:0bb3687e39da 630 Red_LED.pulsewidth_us(new_pulsewidth);
nightmechanic 0:0bb3687e39da 631 break;
nightmechanic 0:0bb3687e39da 632
nightmechanic 0:0bb3687e39da 633 case RED:
nightmechanic 0:0bb3687e39da 634 Red_LED.pulsewidth_us(new_pulsewidth);
nightmechanic 0:0bb3687e39da 635 break;
nightmechanic 0:0bb3687e39da 636
nightmechanic 0:0bb3687e39da 637 default:
nightmechanic 0:0bb3687e39da 638 break;
nightmechanic 0:0bb3687e39da 639 }
nightmechanic 0:0bb3687e39da 640 }
nightmechanic 0:0bb3687e39da 641