Program that implements position including rotation
Dependencies: MPU6050IMU Mouse SDFileSystem mbed
Diff: main.cpp
- Revision:
- 0:5ed57e0c2298
--- /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