Diff: main.cpp

Revision:

7:0e9cbd33abfe

Parent:

6:78538ae01ce6

Child:

8:5c54756c501b

--- a/main.cpp	Mon Mar 09 06:05:45 2015 +0000
+++ b/main.cpp	Mon Mar 09 09:01:33 2015 +0000
@@ -1,18 +1,61 @@
 #include "mbed.h"
 #define pi 3.141592653589793
-Serial sousin(p9,p10);
-Serial hunter(p28,p27);
+Serial sousin(p28,p27);
 Serial pc(USBTX,USBRX);
 Timer em;
+Timer t;
+I2C sensor(p9,p10);
 DigitalOut  check (LED1);
 BusOut check2 (LED2,LED3);
-
-BusOut mo1 (p13,p14);
-BusOut mo2 (p15,p16);
-BusOut mo3 (p17,p18);
+BusOut mo1 (p15,p16);
+BusOut mo2 (p17,p18);
+BusOut mo3 (p19,p20);
+BusIn hunter(p13,p14);
 PwmOut mop1 (p21);
 PwmOut mop2 (p22);
 PwmOut mop3 (p23);
+double get_sen(void)
+{
+    double degree;
+    double phi,theta,tp;
+    double val[3];
+    int i = 0;
+    short d[3];
+    char addr = 0xD0;
+    char raddr = 0xD1;
+    char maddr =  0x18;
+    char mraddr = 0x19;
+    sensor.frequency(400000);
+    char put1[] = {0x6b,0x00};
+    char put2[] = {0x37,0x02};
+    char put3[] = {0x0a,0x12};
+    char put4[] = {0x03};
+    char data[6];
+    sensor.write(addr,put1,2);
+    sensor.write(addr,put2,2);
+    sensor.write(maddr,put3,2);
+    sensor.write(maddr,put4,1);
+    sensor.read(mraddr,data,7);
+    while(i<= 2){
+        d[i] = data[2*i] + data[2*i+1]<<8;
+        i++;
+    }
+    val[0] = d[0] *0.15;
+    val[1] = d[1] *0.15;
+    val[2] = d[2] *0.15;
+    tp = sqrt(val[0]*val[0]+val[1]*val[1]+val[2]*val[2]);
+    theta = atan2(val[0],val[1]);
+    phi = acos(val[2]/tp);
+    //printf("theta-->%f phi--->%f\n\r",theta,phi);
+    if(theta>=0)
+    {
+        degree = theta/pi*180;
+    }else{
+        degree = (pi+theta)/pi*180+180;
+    }
+    //printf("degree-->%f\n\r",degree);
+    return degree;
+}
 double get_theta(double stickx,double sticky)
 {
     double x,y,theta;
@@ -62,7 +105,7 @@
         *sty = y/100.0; 
         if (error == x^y^(*sw)){
             em.reset();
-            pc.printf("sw-->%d\n\r",x);
+            //pc.printf("sw-->%d\n\r",x);
             return 0;
         }else{
             return 1;
@@ -79,22 +122,22 @@
     check = 1;
     while(1){}
 }
-void omni_cont(int8_t hun,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3)
+void omni_cont(int hun,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3)
 {   
     
     double md1,md2,md3,t1;
     if (hun == 2)
     {
-        t1 = 0.1;
+        t1 = 0.6;
     }else if(hun == 1)
     {
-        t1 = -0.1;
+        t1 = -0.6;
     }else{
         t1 = 0;
     }
-    md1 = sin(theta)*0.9+t1;
-    md2 = sin(theta-pi*2/3)*0.9+t1;
-    md3 = sin(theta-pi*4/3)*0.9+t1;
+    md1 = sin(theta)*0.4+t1;
+    md2 = sin(theta-pi*2/3)*0.4+t1;
+    md3 = sin(theta-pi*4/3)*0.4+t1;
     if (md1 < 0){
         *m1 = 2;
         *mp1 = -md1;
@@ -117,70 +160,86 @@
         *mp3 = md3;
     }
 } 
-int8_t get_hunter()
-{
-    int8_t s;
-    if (hunter.readable()){
-        if (hunter.getc() == 48){
-            s = hunter.getc();
-        }
-    }
-    if (s == 255)
-    {
-        allstop();
-    }
-    pc.printf("s-->%d\n\r",s);
-    return s;
-}
+
 int main() {
-    hunter.baud(9600);
+    t.start();
     sousin.baud(9600);
-    int ec,motor[3];
+    int i,ec,motor[3],sen;
+    double asen;
+    asen = get_sen();
     int8_t sw1,hun;
+    i = 0;
     double sx,sy,motp[3],theta;
     while(1) {
         //pc.printf("START\n\r");
-        hun = get_hunter();
+        sen = get_sen();
+        hun = hunter;
         check2 = hun;
         ec = get_serial(&sx,&sy,&sw1);
         ec =0;
+        if ((hun == 1)&& (t.read_ms()>(i*200))){
+            asen = asen+1;
+            i++;
+        }else if((hun == 2)&& (t.read_ms()>(i*200))){
+            asen = asen-1;
+            i++;
+        }
+        if (asen > 360){
+            asen = 0;
+        }
+        if (asen < 0){
+            asen = 360;
+        }
+        if (sen > asen+10){
+            hun = 1;
+        }else if (sen <asen-10){
+            hun = 2;
+        }else {
+            hun = 0;
+        }
+        //pc.printf("asen-->%f",asen);
+        ec = 0;
         //pc.printf("sx--->%dsy--->%d\n\r",sw1,sw1);
         //pc.printf("%d\n\r",hun);
         if (ec == 0){
             em.reset();
             theta = get_theta(sx,sy);
+            pc.printf("%f\n\r",theta);
             if (sw1 == -1){
                 omni_cont(hun,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]);
-            }else if(hun ==1){
-                motor[0] = 1;
-                motor[1] = 1;
-                motor[2] = 1;
-                motp[0] = 0.5;
-                motp[1] = 0.5;
-                motp[2] = 0.5;
-            }else if (hun == 2){
-                motor[0] = 2;
-                motor[1] = 2;
-                motor[2] = 2;
-                motp[0] = 0.5;
-                motp[1] = 0.5;
-                motp[2] = 0.5;
-            }else{
-                motor[0] = 0;
-                motor[1] = 0;
-                motor[2] = 0;
-                motp[0] = 0;
-                motp[1] = 0;
-                motp[2] = 0;
+                mo1 = motor[0];
+                mo2 = motor[1];
+                mo3 = motor[2];
+                mop1 = motp[0];
+                mop2 = motp[1];
+                mop3 = motp[2];
+                
+            }else
+            if (hun == 1){
+                mo1 = 1;
+                mo2 = 1;
+                mo3 = 1;
+                mop1 = 1;
+                mop2 = 1;
+                mop3 = 1;
+            }else if(hun == 2){
+                mo1 = 2;
+                mo2 = 2;
+                mo3 = 2;
+                mop1 = 1;
+                mop2 = 1;
+                mop3 = 1;
+            }else {
+                mo1 = 0;
+                mo2 = 0;
+                mo3 = 0;
+                mop1 = 0;
+                mop2 = 0;
+                mop3 = 0;
             }
-            mo1 = motor[0];
-            mo2 = motor[1];
-            mo3 = motor[2];
-            mop1 = motp[0];
-            mop2 = motp[1];
-            mop3 = motp[2];
         }else{
             em.start();
+            
             //if(em.read_ms()>200){
             //    allstop();
             //}