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
Diff: main.cpp
- Revision:
- 2:59a7d4677474
- Parent:
- 0:0bb3687e39da
- Child:
- 3:0c2d9355ed8c
diff -r 8143972a0587 -r 59a7d4677474 main.cpp --- a/main.cpp Thu Mar 24 21:53:12 2016 +0000 +++ b/main.cpp Thu Mar 24 22:42:59 2016 +0000 @@ -50,16 +50,18 @@ //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 data rate +int delt_t = 0; // used to control display output rate + +extern float sum; +extern uint32_t sumCount; + MPU6050 mpu6050; Timer t; @@ -171,7 +173,7 @@ init_color(¤t_color,light_intensity); //Set up I2C - MPU_i2c.frequency(300000); // use fast (400 kHz) I2C + MPU6050_set_I2C_freq(300000); // use fast (400 kHz) I2C // start "clock" t.start(); @@ -186,7 +188,7 @@ sound_is_playing_flag = TRUE; // initialize the motion sensor - motion_is_init = motion_sensor_init(); + motion_is_init = mpu6050.motion_sensor_init(); button_ticker.attach(&button_inth, 0.025); @@ -264,7 +266,7 @@ if (motion_is_init) { - new_data = motion_update_data(&MPU_data[mpu_pointer]); + new_data = mpu6050.motion_update_data(&MPU_data[mpu_pointer], t.read_us()); } if (new_data) @@ -413,142 +415,7 @@ } -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) {