The Professional Perfectionists
Program that implements position including rotation
Dependencies: MPU6050IMU Mouse SDFileSystem mbed
Revision 0:5ed57e0c2298, committed 2015-03-31
- Comitter:
- joshwilkins2013
- Date:
- Tue Mar 31 22:36:45 2015 +0000
- Commit message:
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050IMU.lib Tue Mar 31 22:36:45 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/The-A-Team/code/MPU6050IMU/#5835b443885d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Mouse.lib Tue Mar 31 22:36:45 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/The-A-Team/code/Mouse/#eb8581df1f5e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Tue Mar 31 22:36:45 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/SDFileSystem/#7b35d1709458
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Mar 31 22:36:45 2015 +0000
@@ -0,0 +1,154 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "SDFileSystem.h"
+#include "Mouse.h"
+
+SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd"); // MOSI, MISO, SCK, CS
+
+Serial pc(USBTX, USBRX);
+
+FILE *Data; // Creates name of file
+char buffer[5000000]; // Buffer size (# of characters) for SD card
+
+MPU6050 mpu6050;
+MOUSE mouse;
+Timer t;
+
+DigitalIn button(PTB9);
+
+signed long mx, my;
+float d,dx,dy;
+double x,xx,y,yy;
+double psi,lpsi=0; // psi - current orientation, lpsi - last orientation, theta - current angle around circle
+double gyroZ,lgyroZ;
+double dt, timer;
+
+bool closed = 1; // Flag for sd card, to open and close with switch
+
+
+//================================================================================
+//============= Sensor Initialization =============
+//================================================================================
+
+void gyro_init(){
+ i2c.frequency(400000);
+
+ uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
+ pc.printf("I AM 0x%x\n\r", whoami);
+ pc.printf("I SHOULD BE 0x68\n\r");
+
+ if (whoami == 0x68){
+ pc.printf("MPU6050 is online...");
+ wait(1);
+
+ mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+ 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();
+ pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+
+ pc.printf("\nMPU6050 passed self test... Initializing");
+ wait(2); //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ } else pc.printf("\nDevice did not the pass self-test!\n\r");
+ } else {
+ pc.printf("Could not connect to MPU6050: \n\r");
+ pc.printf("%#x \n", whoami);
+ while(1) ; // Loop forever if communication doesn't happen
+ }
+}
+
+
+//================================================================================
+//============= Get Inputs =============
+//================================================================================
+
+void get_Rates(){
+ if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
+ mpu6050.readGyroData(gyroCount); // Read the x/y/z adc gyro values
+ mpu6050.getGres();
+ gyroZ = gyroCount[2]*gRes; // - gyroBias[2];
+ }
+}
+
+
+//================================================================================
+//============= Main Program Start =============
+//================================================================================
+
+int main(){
+ pc.baud(115200);
+
+ Data = fopen("/sd/Data.txt", "w");
+ closed = 0;
+
+ gyro_init();
+ mouse.init();
+
+ t.start();
+ timer = t.read_us();
+
+ while(1) {
+ if(button) {
+
+ if (closed){
+ Data = fopen("/sd/Data.txt", "w");
+ closed = 0;
+ }
+
+ DigitalOut myled(LED1);
+
+ get_Rates(); // Gyro rates & Mouse position
+
+ dt = (t.read_us() - timer) / 1000000.00;
+ timer = t.read_us();
+
+
+//================================================================================
+//============= Char Conversion =============
+//================================================================================
+
+ psi += ((lgyroZ+gyroZ)/2)*dt; // Current orientation/pose
+ if (psi>360) psi -= 360; // resets current orientation to keep between 0 & 360
+ if (psi<-360) psi += 360; // resets current orientation to keep between 0 & -360
+ lgyroZ = gyroZ;
+
+ mx = mouse.getDx();
+ my = mouse.getDy();
+ dx = (mx/231.25); // Converts char into float in inches
+ dy = (my/231.25); // Experimentally obtained 231.25 (1850/8in)
+
+ double a1 = psi;
+ a1 *= (PI/180);
+ double a2 = lpsi;
+ a2 *= (PI/180);
+ double a3 = a1-a2;
+
+ double a = dx*tan(a3);
+ double b = dx/cos(a3);
+ double c = (dy+a)*cos(a2)-(b*sin(a1));
+ double d = sqrt((dx*dx)+(dy*dy));
+ double e = sqrt((d*d)-(c*c));
+ double f = (dy+a)*cos(a3);
+ double g = c*cos(a1);
+ double h = (f-g)/(sin(a1));
+
+ yy -= c;
+ xx += h;
+
+ x = xx + 3;
+ y = yy - 9.5;
+
+ lpsi = psi;
+
+ if (Data == NULL) pc.printf("Unable to write the file \n"); // This chunk for writing to file on SD card
+ else fprintf(Data, "%f %f %f %f\r\n",psi,x,y,timer); // can read float or double not int (only checked these three types)
+
+ }else{
+ if(!closed){
+ fclose(Data);
+ closed = 1;
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Mar 31 22:36:45 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/487b796308b0 \ No newline at end of file