Program that implements position including rotation
Dependencies: MPU6050IMU Mouse SDFileSystem mbed
main.cpp
- Committer:
- joshwilkins2013
- Date:
- 2015-03-31
- Revision:
- 0:5ed57e0c2298
File content as of revision 0:5ed57e0c2298:
#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; } } } }