gokmen ascioglu
f
Dependencies: hakan SDFileSystem mbed-src
Diff: main.cpp
- Revision:
- 0:f7e6527f4d8a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Sep 19 21:35:10 2016 +0000
@@ -0,0 +1,335 @@
+#include "mbed.h"
+#include "MPU6050.h"
+
+DigitalOut led(LED1);
+DigitalOut led1(LED2);
+Serial pc(USBTX, USBRX);
+MPU6050 mpu;
+Timer timer;
+Timer timer1;
+Timer timer2;
+Timer timer3;
+
+int16_t ax, ay, az;
+int16_t gx, gy, gz, buff_x[1000],buff_y[1000],buff_z[1000];
+int16_t i=1,ym=5;
+int32_t toplam_x=0,toplam_y=0,toplam_z=0;
+float x,y,z,k,l,m,k1,l1,x1,y1;
+float begin, end,end1,end2, ve, s,t,u,s1,t1,u1;
+
+void accelerometer(void)
+{
+ mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
+ ax=(ax-toplam_x);
+ ay=ay-toplam_y;
+ az=az+toplam_z;
+
+ ax=ax-toplam_x;
+ ay=ay-toplam_y;
+ az=az-toplam_z;
+
+ x=ax/16383.00;
+ y=ay/16383.00;
+ z=az/16383.00;
+ x1=x;
+ y1=y;
+
+}
+
+void accelerometer1(void)
+{
+ mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
+ ax=(ax-toplam_x);
+ ay=ay-toplam_y;
+ az=az+toplam_z;
+
+ ax=ax-toplam_x;
+ ay=ay-toplam_y;
+ az=az-toplam_z;
+
+ s=ax/16383.00;
+ t=ay/16383.00;
+ u=az/16383.00;
+
+}
+
+void accelerometer2(void)
+{
+ mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
+ ax=(ax-toplam_x);
+ ay=ay-toplam_y;
+ az=az+toplam_z;
+
+ ax=ax-toplam_x;
+ ay=ay-toplam_y;
+ az=az-toplam_z;
+
+ s1=ax/16383.00;
+ t1=ay/16383.00;
+ u1=az/16383.00;
+
+}
+
+int main()
+{
+ pc.printf("MPU6050 testConnection\n");
+
+ bool mpu6050TestResult = mpu.testConnection();
+
+ if(mpu6050TestResult) {
+ pc.printf("MPU6050 test passed \n");
+ } else {
+ pc.printf("MPU6050 test failed \n");
+ }
+
+ mpu.initialize();
+ mpu.setDLPFMode(1);
+ pc.printf("DLPF=%d\n\r",mpu.getDLPFMode());
+
+
+ for(i=0; i<500; i++) {
+ mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+ buff_x[i]=ax;
+ toplam_x=toplam_x+buff_x[i];
+ buff_y[i]=ay;
+ toplam_y=toplam_y+buff_y[i];
+ buff_z[i]=az;
+ toplam_z=toplam_z+buff_z[i];
+ }
+
+ toplam_x=toplam_x/500;
+ toplam_y=toplam_y/500;
+ toplam_z=toplam_z/500;
+ toplam_z=16383-toplam_z;
+
+ ax=(ax-toplam_x);
+ ay=ay-toplam_y;
+ az=az+toplam_z;
+
+ ax=ax-toplam_x;
+ ay=ay-toplam_y;
+ az=az-toplam_z;
+
+ k=ax/16383.00;
+ l=ay/16383.00;
+ m=az/16383.00;
+
+ i=0;
+ led=1;
+
+ k1=k;
+ l1=l;
+
+ while(1) {
+ k=k1;
+ l=l1;
+ timer.start();
+ begin = timer.read();
+
+ accelerometer();
+ wait_ms(10);
+ i++;
+ pc.printf("Beklemede\n\r");
+
+ /* HEAD FRONT MOVEMENT */
+ timer1.reset();
+ if ((y+2) < (l+1.7)) {
+
+ timer1.start();
+ accelerometer1();
+ wait_ms(10);
+
+ while((t+2)<(l+1.85)) {
+
+ end=timer1.read();
+ accelerometer1();
+ wait_ms(10);
+
+ if (end>1.5) {
+ while (t<-0.15) { accelerometer1(); }
+ y=5.0;
+ t=7.0;
+ end=0;
+ timer1.reset();
+ break;
+ }
+
+ }
+
+ if ( (y+2)< (l+1.7) ) {
+
+ accelerometer2();
+ timer2.reset();
+ timer2.start();
+ while((t1+2)>(l+1.85)) {
+
+ accelerometer2();
+ wait_ms(10);
+ end1=timer2.read();
+ ym=1;
+ if (end1>1.5) {
+ y=5;
+ ym=5;
+ l=5;
+ break;
+ }
+ }
+
+ }
+ }
+
+
+ /* HEAD back MOVEMENT */
+ if ((y1+1.7) > (l+2)) {
+ timer1.reset();
+ timer1.start();
+ accelerometer1();
+ wait_ms(10);
+
+ while((t+1.85)>(l+2)) {
+ end=timer1.read();
+ accelerometer1();
+ wait_ms(10);
+
+ if (end>1.5) {
+ while (t>0.3) { accelerometer1(); }
+ y1=-5.0;
+ t=-7.0;
+ end=0;
+ timer1.reset();
+ break;
+ }
+
+ }
+
+
+ if ( (y1+1.7)>(l+2.0) ) {
+
+ accelerometer2();
+ timer2.reset();
+ timer2.start();
+
+ while((t1+1.85)<(l+2)) {
+ accelerometer2();
+ wait_ms(10);
+ end1=timer2.read();
+ ym=2;
+ if (end1>1.5) {
+ y1=-5;
+ ym=5;
+ t1=5;
+ break;
+ }
+ }
+
+ }
+ }
+
+ /* HEAD left MOVEMENT */
+ if ((x1+1.7) > (k+2)) {
+ timer1.reset();
+ timer1.start();
+ accelerometer1();
+ wait_ms(10);
+
+ while((s+1.85)>(k+2)) {
+ end=timer1.read();
+ accelerometer1();
+ wait_ms(10);
+
+ if (end>1.5) {
+ while (s>0.3) { accelerometer1(); }
+ x1=-5.0;
+ s=-7.0;
+ end=0;
+ timer1.reset();
+ break;
+ }
+
+ }
+
+
+ if ( (x1+1.7)>(k+2.0) ) {
+
+ accelerometer2();
+ timer2.reset();
+ timer2.start();
+ while((s1+1.85)<(k+2)) {
+ accelerometer2();
+ wait_ms(10);
+ end1=timer2.read();
+ ym=4;
+ if (end1>1.5) {
+ x1=-5;
+ ym=5;
+ s1=5;
+ break;
+ }
+ }
+
+ }
+ }
+
+ /* HEAD right MOVEMENT */
+ timer1.reset();
+ if ((x+2) < (k+1.7)) {
+
+ timer1.start();
+ accelerometer1();
+ wait_ms(10);
+
+ while((s+2)<(k+1.85)) {
+ end=timer1.read();
+ accelerometer1();
+ wait_ms(10);
+ if (end>1.5) {
+ while (s<-0.15) { accelerometer1(); }
+ x=5.0;
+ s=7.0;
+ end=0;
+ timer1.reset();
+ break;
+ }
+
+ }
+
+ if ( (x+2)< (k+1.7) ) {
+
+ accelerometer2();
+ timer2.reset();
+ timer2.start();
+
+ while((s1+2)>(k+1.85)) {
+ accelerometer2();
+ wait_ms(10);
+ end1=timer2.read();
+ ym=3;
+ if (end1>1.5) {
+ x=5;
+ ym=5;
+ k=5;
+ break;
+ }
+ }
+
+ }
+ }
+
+ if (ym==1){ pc.printf("head infront\n\r"); ym=5; wait(2.0); }
+ if (ym==2){ pc.printf("head back\n\r"); ym=5; wait(2.0); }
+ if (ym==3){ pc.printf("head right\n\r"); ym=5; wait(2.0); }
+ if (ym==4){ pc.printf("head left\n\r"); ym=5; wait(2.0); }
+
+
+ if (i==2000) {
+ led=0;
+ break;
+ }
+
+ }
+
+}
+
\ No newline at end of file