Diff: main.cpp

Revision:

0:0bb3687e39da

Child:

2:59a7d4677474

diff -r 000000000000 -r 0bb3687e39da main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 22 10:30:16 2016 +0000
@@ -0,0 +1,641 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#ifdef __SERIAL_DEBUG__ 
+#include "USBSerial.h"
+#include "STM32_USB48MHz.h"
+#endif
+#include "Watchdog.h"
+#include "LightSaber.h"
+#include "LightSaber_sounds.h"
+
+
+
+// Light Controls: (Timer 3, ch 1-3)
+PwmOut Red_LED(PA_6);
+PwmOut Green_LED(PA_7);
+PwmOut Blue_LED(PB_0);
+
+//Speaker drive (Timer 2 ch 2)
+PwmOut Speaker_OUT(PA_1);
+
+//Color change button
+DigitalIn Color_Button(PA_3, PullUp);
+
+//startup config IOs
+DigitalIn Startup_Config_IN1(PB_5, PullDown);
+DigitalIn Startup_Config_IN2(PB_7, PullDown);
+DigitalOut Startup_Config_OUT(PB_6, 0);
+
+//Spare Buttons
+//DigitalIn Spare1_Button(PB_12);
+//DigitalIn Spare2_Button(PB_13);
+//DigitalIn Spare3_Button(PB_14);
+
+//Watchdog
+//Watchdog wd;
+
+bool color_button_released = TRUE;
+volatile int Color_Button_Count = 0;
+
+//sounds
+int sound_to_play;
+volatile int sound_count = 1;
+volatile int sound_line = 0;
+volatile int next_sound_time = 0;
+volatile bool sound_is_playing_flag = FALSE;
+
+//Battery voltage monitor
+AnalogIn V_bat(PA_2);
+
+//Ticker
+Ticker button_ticker, sound_ticker;
+
+//MPU 6050
+float sum = 0;
+uint32_t sumCount = 0;
+
+//Set up I2C, (SDA,SCL)
+//I2C MPU_i2c(PB_9, PB_8); //defined in MPU6050.h...
+
+//MPU interrupt line
+DigitalIn MPU_Intr(PB_1);
+
+MPU6050 mpu6050;
+   
+Timer t;
+
+void button_inth()
+{
+    int Color_Button_state;
+    
+    Color_Button_state = Color_Button.read(); //pressed = 0, released = 1
+    
+    if (color_button_released) {
+        Color_Button_state = (~Color_Button_state) & 0x01;
+    }
+    Color_Button_Count = Color_Button_state*(Color_Button_Count + 1);
+    
+}
+
+bool sound_player(const int sound_table[][6], int table_lines)
+{
+    int sound_period;
+    int sound_pulse_width_us;
+    if (sound_count > sound_table[sound_line][NUM_STEPS_IDX])
+    {
+        sound_count = 1;
+        sound_line++;
+        if (sound_line >= table_lines)
+        {
+            return FALSE;
+        }
+    }
+    
+    sound_period = sound_table[sound_line][INIT_PERIOD_IDX] + ((sound_count-1) * sound_table[sound_line][STEP_PERIOD_IDX]);
+    sound_pulse_width_us = ( sound_period * (sound_table[sound_line][INIT_VOL_IDX] + ((sound_count-1) * sound_table[sound_line][STEP_VOL_IDX])) )/ 200;
+    
+    // there are no checks for 0/negative values of the above - need to make sure that tables are valid.
+    // set PWM parameters for current step
+    
+    Speaker_OUT.period_us(sound_period);
+    Speaker_OUT.pulsewidth_us(sound_pulse_width_us);
+        
+    //update next_sound_time, step count        
+    
+    next_sound_time = t.read_ms() + sound_table[sound_line][STEP_DUR_IDX];
+    sound_count++;
+    
+    return TRUE;
+}
+
+void sound_inth()
+{
+    if ((t.read_ms() >= next_sound_time) && sound_is_playing_flag )
+    {
+        switch (sound_to_play) {
+        
+            case STARTUP_SOUND:
+                sound_is_playing_flag = sound_player(startup_sound_table, STARTUP_TBL_N_LINES);
+                break;
+            case MOVEMENT_SOUND:
+                sound_is_playing_flag = sound_player(movement_sound_table, MOVEMENT_TBL_N_LINES);
+                break;
+            case CLASH_SOUND:
+                sound_is_playing_flag = sound_player(clash_sound_table, CLASH_TBL_N_LINES);
+                break;
+            default:
+                break;
+        }       
+    }
+}
+int main()
+{
+    bool motion_is_init = FALSE;
+    bool new_data = FALSE;
+    
+    int current_color;
+    int light_intensity = NORM_LIGHT_INTENS;
+    
+    int v_bat_avg_mem[VBAT_AVG_LEN];
+    int v_bat_avg_value;
+    int v_bat_avg_pointer = 0;
+    bool v_bat_ready = FALSE;
+    bool vbat_low_flag = FALSE;
+    int t_vbat_count = 0;
+    int t_vbat_start = 0;
+    
+    MPU_data_type MPU_data[MPU_BUFFER_LEN];
+    MPU_data_type MPU_avg_data;
+    int mpu_pointer = 0;
+    int mpu_data_count = 0;
+    bool accel_thr_crossed = FALSE;
+    bool ypr_thr_crossed = FALSE;
+    int count = 0;
+    
+    int temp_diff;
+    
+    int i = 0;
+#ifdef __SERIAL_DEBUG__   
+    STM32_HSI_USB48MHz(); // HSI,USB48MHz,SYSCLK32MHz
+    
+    // serial port for debug etc.
+    USBSerial serial;
+
+    //USBSerial serial;
+
+    serial.printf("Initializing main");
+#endif
+
+//initialize LEDs : set rate, intensity, and startup color (according to jumper settings)
+    
+    init_color(&current_color,light_intensity); 
+
+//Set up I2C
+    MPU_i2c.frequency(300000);  // use fast (400 kHz) I2C
+
+// start "clock"
+    t.start();
+    
+// attach and set sound interrupt handler (every 10msec)   
+    sound_ticker.attach(&sound_inth, 0.01);
+// play startup sound    
+    sound_to_play = STARTUP_SOUND;
+    sound_line = 0;
+    sound_count = 1;
+    next_sound_time = t.read_ms();
+    sound_is_playing_flag = TRUE;
+    
+// initialize the motion sensor
+    motion_is_init = motion_sensor_init();
+    
+    button_ticker.attach(&button_inth, 0.025);
+        
+    wait_ms(1000); //from MPU example
+    
+//Initialize watchdog with a 2 second interval
+ //   wd.Configure(2.0);       
+
+//main loop
+    while(1) {
+        
+              
+    
+        if (vbat_low_flag) //low battery
+        {
+            light_intensity = LOW_LIGHT_INTENS; //lower light intensity save some battery
+            if (t_vbat_start == 0) //starting a new cycle
+            {
+                t_vbat_start = t.read_ms();
+                if (t_vbat_count < 3) //flash only in first 3 cycles
+                {
+                    change_color(RED , light_intensity);
+                }
+            } else if ( ((t.read_ms() - t_vbat_start) > 1000) && (t_vbat_count < 3)) // dark period and flash only in first 3 cycles
+            {
+                change_color(RED, 0);
+            } else // after first 3 cycles return to normal operation (with lower light intensity)
+            {
+                change_color((color_type) current_color, light_intensity);
+            }
+            if ( (t.read_ms() - t_vbat_start) > 2000) //end of cycle
+            {
+                t_vbat_start = 0;
+                t_vbat_count ++;
+            }
+            if (t_vbat_count > 30) // once a minute, starting over
+            {
+                t_vbat_count  = 0;
+            }
+            
+        }
+        //battery ok or between low bat flashes, check if button was pressed or released
+        if (!vbat_low_flag || (t_vbat_count >=3))
+        {        
+            if ( (Color_Button_Count >= BUTTON_PRESS_THR) && color_button_released )
+            {
+                __disable_irq();
+                color_button_released = FALSE;
+                Color_Button_Count = 0;
+                __enable_irq(); 
+                current_color++;
+                if (current_color >= NUM_OF_COLORS)
+                {
+                    current_color = BLUE;
+                }
+#ifdef __SERIAL_DEBUG__
+                serial.printf("Color Button pressed");
+#endif
+                change_color((color_type) current_color, light_intensity);
+            }   
+        
+            if ((Color_Button_Count >= BUTTON_RELEASE_THR) && !color_button_released)
+            {
+                __disable_irq();
+                color_button_released = TRUE;
+                Color_Button_Count = 0;
+                __enable_irq(); 
+#ifdef __SERIAL_DEBUG__
+                serial.printf("Color Button released");
+#endif
+            }
+        }
+            
+        
+        
+        if (motion_is_init)
+        {
+            new_data = motion_update_data(&MPU_data[mpu_pointer]);
+        }
+        
+        if (new_data)
+        {
+            
+            if ((mpu_data_count > MPU_DATA_STABLE) && !sound_is_playing_flag )
+            {
+                //calculate new average
+                mpu_calc_avg(MPU_data, mpu_pointer, &MPU_avg_data, MPU_AVG_LEN);
+                
+                //check if any thresholds are crossed
+                //acceleration
+                if ((abs(MPU_avg_data.ax - MPU_data[mpu_pointer].ax) > ACCEL_THR) 
+                        || (abs(MPU_avg_data.ay - MPU_data[mpu_pointer].ay) > ACCEL_THR) 
+                        || (abs(MPU_avg_data.az - MPU_data[mpu_pointer].az) > ACCEL_THR))
+                {
+                    accel_thr_crossed = TRUE;
+                }
+                // YPR
+                temp_diff = abs(MPU_avg_data.yaw - MPU_data[mpu_pointer].yaw);
+                if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
+                {
+                    ypr_thr_crossed = TRUE;
+                }
+                
+                temp_diff = abs(MPU_avg_data.pitch - MPU_data[mpu_pointer].pitch);
+                if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
+                {
+                    ypr_thr_crossed = TRUE;
+                }
+                
+                temp_diff = abs(MPU_avg_data.roll - MPU_data[mpu_pointer].roll);
+                if ((temp_diff > YPR_THR) && (temp_diff < (3600-YPR_THR)))
+                {
+                    ypr_thr_crossed = TRUE;
+                }
+                
+                if (accel_thr_crossed)
+                {
+                    //play clash sound
+                    sound_to_play = CLASH_SOUND;
+                    sound_line = 0;
+                    sound_count = 1;
+                    next_sound_time = t.read_ms();
+                    sound_is_playing_flag = TRUE;
+                
+                }
+                if (ypr_thr_crossed && !accel_thr_crossed)
+                {
+                    //play movement sound
+                    sound_to_play = MOVEMENT_SOUND;
+                    sound_line = 0;
+                    sound_count = 1;
+                    next_sound_time = t.read_ms();
+                    sound_is_playing_flag = TRUE;
+                }
+                
+                accel_thr_crossed = FALSE;
+                ypr_thr_crossed = FALSE;
+                                
+            } else {
+                mpu_data_count++;
+            }
+            
+            mpu_pointer++;
+            
+            if (mpu_pointer >= MPU_BUFFER_LEN)
+            {
+                mpu_pointer = 0;
+            }
+            
+            new_data = FALSE;   
+        }
+        
+        delt_t = t.read_ms() - count;
+        if (delt_t > 1000) 
+        { // update serial port once per second independent of read rate
+            
+#ifdef __SERIAL_DEBUG__
+            //serial.printf("Yaw, Pitch, Roll: %f %f %f\n\r", (float) (yaw / 16384.0f), (float) (pitch/16384.0f), (float) (roll/16384.0f));
+            serial.printf("Yaw, Pitch, Roll: %i %i %i\n\r", (int)(yaw * 10), (int)(pitch * 10), (int)(roll * 10));
+            serial.printf("Yaw, Pitch, Roll (average): %i %i %i\n\r", MPU_avg_data.yaw, MPU_avg_data.pitch, MPU_avg_data.roll);
+            
+            serial.printf("Ax, Ay, Az : %i %i %i\n\r", (int)(ax * 1000), (int)(ay * 1000), (int)(az * 1000));
+            serial.printf("Ax, Ay, Az (average): %i %i %i\n\r", MPU_avg_data.ax , MPU_avg_data.ay, MPU_avg_data.az);
+            
+            serial.printf("average rate = %f\n\r", (float) sumCount/sum);
+            
+            if (sound_is_playing_flag)
+            {
+                serial.printf("Sound is playing!  \n\r", (float) sumCount/sum);
+            }
+#endif           
+            count = t.read_ms();
+            sum = 0;
+            sumCount = 0;
+                      
+// Battery voltage monitor
+            v_bat_avg_mem[v_bat_avg_pointer] = V_bat.read_u16();
+            v_bat_avg_pointer++;
+            if (v_bat_avg_pointer >= VBAT_AVG_LEN)
+            {
+                v_bat_avg_pointer = 0;
+                v_bat_ready = TRUE;
+            }
+            if (v_bat_ready)
+            {
+                //calculate the average
+                v_bat_avg_value = 0;
+                for (i = 0 ; i < VBAT_AVG_LEN ; i++)
+                {
+                    v_bat_avg_value += v_bat_avg_mem[i];
+                }
+                
+                v_bat_avg_value = v_bat_avg_value/VBAT_AVG_LEN;
+#ifdef __SERIAL_DEBUG__                
+                serial.printf("Battery average voltage: %i \n\r", v_bat_avg_value);
+#endif               
+                //check voltage
+                if (v_bat_avg_value < VBAT_THR)
+                {
+                    vbat_low_flag = TRUE;
+                } else if ((vbat_low_flag == TRUE) && (v_bat_avg_value > (VBAT_THR+VBAT_HYST)))
+                {
+                    vbat_low_flag = FALSE;
+                }
+            }
+            
+            
+        }
+//handle timer overflow - even if a sound is playing there are 5 minutes before overflow
+        if ((t.read_ms() > (30*60*1000)) && !sound_is_playing_flag)
+        {
+            t.reset();
+            // reset mpu statistics as well
+            count = t.read_ms();
+            sum = 0;
+            sumCount = 0;
+            //reset the low battery variables
+            t_vbat_start = 0;
+            t_vbat_count = 0;
+        }
+// kick the dog before the timeout       
+//       wd.Service();      
+    }
+}
+
+
+bool motion_sensor_init()
+{
+
+    
+// Read the WHO_AM_I register, this is a good test of communication
+    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
+    //serial.printf("I AM 0x%x\n\r", whoami);
+    //serial.printf("I SHOULD BE 0x68\n\r");
+
+    if (whoami == 0x68) { // WHO_AM_I should always be 0x68
+       // serial.printf("MPU6050 is online...");
+        wait(1);
+
+
+        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+/*
+        serial.printf("x-axis self test: acceleration trim within : ");
+        serial.printf("%f", SelfTest[0]);
+        serial.printf("% of factory value \n\r");
+        serial.printf("y-axis self test: acceleration trim within : ");
+        serial.printf("%f", SelfTest[1]);
+        serial.printf("% of factory value \n\r");
+        serial.printf("z-axis self test: acceleration trim within : ");
+        serial.printf("%f", SelfTest[2]);
+        serial.printf("% of factory value \n\r");
+        serial.printf("x-axis self test: gyration trim within : ");
+        serial.printf("%f", SelfTest[3]);
+        serial.printf("% of factory value \n\r");
+        serial.printf("y-axis self test: gyration trim within : ");
+        serial.printf("%f", SelfTest[4]);
+        serial.printf("% of factory value \n\r");
+        serial.printf("z-axis self test: gyration trim within : ");
+        serial.printf("%f", SelfTest[5]);
+        serial.printf("% of factory value \n\r");
+*/
+        wait(1);
+
+        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) {
+            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+            mpu6050.initMPU6050();
+            //serial.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+
+            return TRUE;
+        } else {
+            //serial.printf("Device did not the pass self-test!\n\r");
+            return FALSE;
+
+        }
+    } else {
+        //serial.printf("Could not connect to MPU6050: \n\r");
+        //serial.printf("%#x \n",  whoami);
+
+        return FALSE;
+    }
+
+
+}
+
+bool motion_update_data(MPU_data_type *new_data)
+{
+    if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {
+        mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
+        mpu6050.getAres();
+
+        // Now we'll calculate the accleration value into actual g's
+        ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+        ay = (float)accelCount[1]*aRes - accelBias[1];
+        az = (float)accelCount[2]*aRes - accelBias[2];
+
+        mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
+        mpu6050.getGres();
+
+        // Calculate the gyro value into actual degrees per second
+        gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
+        gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
+        gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
+
+        tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
+        temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+
+
+        Now = t.read_us();
+        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+        lastUpdate = Now;
+
+        sum += deltat;
+        sumCount++;
+
+        if(lastUpdate - firstUpdate > 10000000.0f) {
+            beta = 0.04;  // decrease filter gain after stabilized
+            zeta = 0.015; // increase bias drift gain after stabilized
+        }
+
+        // Pass gyro rate as rad/s
+        mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+
+
+
+
+        // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
+        // In this coordinate system, the positive z-axis is down toward Earth.
+        // 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.
+        // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
+        // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
+        // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
+        // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
+        // applied in the correct order which for this configuration is yaw, pitch, and then roll.
+        // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
+        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]);
+        pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+        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]);
+        pitch *= 180.0f / PI;
+        yaw   *= 180.0f / PI;
+        roll  *= 180.0f / PI;
+/*
+        new_data->ax = (int) (ax / 16384.0f);
+        new_data->ay = (int) (ay / 16384.0f);
+        new_data->az = (int) (az / 16384.0f);
+        new_data->yaw = (int) (yaw / 16384.0f);
+        new_data->pitch = (int) (pitch / 16384.0f);
+        new_data->roll = (int) (roll / 16384.0f);
+*/
+        new_data->ax = (int) (ax * 1000);
+        new_data->ay = (int) (ay * 1000);
+        new_data->az = (int) (az * 1000);
+        new_data->yaw = (int) (yaw * 10);
+        new_data->pitch = (int) (pitch * 10);
+        new_data->roll = (int) (roll * 10);
+        return TRUE;
+
+    } else {
+        return FALSE;
+    }
+
+}
+
+void mpu_calc_avg(MPU_data_type * MPU_data,int mpu_pointer, MPU_data_type * MPU_avg_data, int avg_len)
+{
+    int i = 0;
+    //MPU_data_type MPU_avg_data;
+    
+    MPU_avg_data->ax = 0;
+    MPU_avg_data->ay = 0;
+    MPU_avg_data->az = 0;
+    MPU_avg_data->yaw = 0;
+    MPU_avg_data->pitch = 0;
+    MPU_avg_data->roll = 0;
+    
+    for (i=0 ; i < avg_len ; i++)
+    {
+        mpu_pointer++;
+        if (mpu_pointer >= MPU_BUFFER_LEN)
+            {
+                mpu_pointer = 0;
+            }
+        MPU_avg_data->ax += MPU_data[mpu_pointer].ax;
+        MPU_avg_data->ay += MPU_data[mpu_pointer].ay;
+        MPU_avg_data->az += MPU_data[mpu_pointer].az;
+        MPU_avg_data->yaw += MPU_data[mpu_pointer].yaw;
+        MPU_avg_data->pitch += MPU_data[mpu_pointer].pitch;
+        MPU_avg_data->roll += MPU_data[mpu_pointer].roll; 
+    }
+    
+    MPU_avg_data->ax = MPU_avg_data->ax / avg_len;
+    MPU_avg_data->ay = MPU_avg_data->ay / avg_len;
+    MPU_avg_data->az = MPU_avg_data->az / avg_len;
+    MPU_avg_data->yaw = MPU_avg_data->yaw / avg_len;
+    MPU_avg_data->pitch = MPU_avg_data->pitch / avg_len;
+    MPU_avg_data->roll = MPU_avg_data->roll / avg_len;
+    
+    //return MPU_avg_data;
+}
+
+void init_color(int *color, int pulsewidth)
+{
+    Startup_Config_OUT.write(1);
+    wait(1);
+    if (Startup_Config_IN1.read() == 1)
+    {
+       *color = GREEN;
+    }else if (Startup_Config_IN2.read() == 1)
+    {
+        *color = PURPLE;
+    }else
+    {
+        
+        *color = BLUE;
+    }
+    Startup_Config_OUT.write(0);
+    
+    Blue_LED.period_ms(LED_PWM_PERIOD);
+    
+    change_color((color_type)*color, pulsewidth);
+}
+        
+
+void change_color(color_type new_color, int new_pulsewidth)
+{
+    Blue_LED.pulsewidth_us(0);
+    Green_LED.pulsewidth_us(0);
+    Red_LED.pulsewidth_us(0);
+
+    switch (new_color) {
+        case BLUE:
+            Blue_LED.pulsewidth_us(new_pulsewidth);
+            break;
+
+        case GREEN:
+            Green_LED.pulsewidth_us(new_pulsewidth);
+            break;
+
+        case PURPLE:
+            Blue_LED.pulsewidth_us(new_pulsewidth);
+            Red_LED.pulsewidth_us(new_pulsewidth);
+            break;
+
+        case RED:
+            Red_LED.pulsewidth_us(new_pulsewidth);
+            break;
+
+        default:
+            break;
+    }
+}
+    
\ No newline at end of file