Diff: main.cpp

Revision:

13:1423effc49b2

Parent:

12:12dca00d1bca

Child:

14:54f198b9947b

--- a/main.cpp	Tue Mar 17 00:30:30 2015 +0000
+++ b/main.cpp	Tue Mar 17 23:43:14 2015 +0000
@@ -82,22 +82,22 @@
     check = 1;
     while(1){}
 }
-void omni_cont(int hun,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3)
+void omni_cont(int hun,double aval,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3)
 {   
     
     double md1,md2,md3,t1;
     if (hun == 2)
     {
-        t1 = 0.6;
+        t1 = (fabs(aval)/180)*0.3;
     }else if(hun == 1)
     {
-        t1 = -0.6;
+        t1 = -(fabs(aval)/180)*0.3;
     }else{
         t1 = 0;
     }
-    md1 = -sin(theta)*0.4+t1;
-    md2 = sin(theta-pi*2/3)*0.4+t1;
-    md3 = sin(theta-pi*4/3)*0.4+t1;
+    md1 = sin(theta)*0.7+t1;
+    md2 = sin(theta-pi*2/3)*0.7+t1;
+    md3 = sin(theta-pi*4/3)*0.7+t1;
     if (md1 < 0){
         *m1 = 2;
         *mp1 = -md1;
@@ -124,15 +124,16 @@
 int main() {
     sousin.baud(9600);
     sensor.setOpMode(HMC6352_CONTINUOUS,1,20);
-    int i,ec,motor[3],sen;
-    double asen;
+    int i,ec,motor[3],val;
+    double asen,sen,aval;
     int8_t sw1,hun;
     i = 0;
     asen = sensor.sample()/10.0;
     double sx,sy,motp[3],theta;
     while(1) {
         //pc.printf("START\n\r");
-        
+        sen = sensor.sample()/10.0;
+        //pc.printf("%f\n\r",sen);
         hun = hunter;
         check2 = hun;
         ec = get_serial(&sx,&sy,&sw1);
@@ -149,10 +150,10 @@
             t.reset();
             t.stop();
         }
-        if ((hun == 1)&& (t.read_ms()>(i*200))){
+        if ((hun == 1)&& (t.read_ms()>(i*20))){
             asen = asen+1;
             i++;
-        }else if((hun == 2)&& (t.read_ms()>(i*200))){
+        }else if((hun == 2)&& (t.read_ms()>(i*20))){
             asen = asen-1;
             i++;
         }
@@ -162,13 +163,26 @@
         if (asen < 0){
             asen = 360;
         }
-        if (sen > asen+20){
+        val = sen+540-asen;
+        val %= 360;
+        val -= 180;
+        printf("   :%d\n\r",val);
+        if (val > 10){
+            hun = 2;
+        }else if (val <-10){
             hun = 1;
-        }else if (sen <asen-20){
+        }else{
+            hun = 0;
+        } 
+        aval = val;
+        /*if(sen> asen-180){
             hun = 2;
+        }else if (sen< asen-180){
+            hun = 1;
         }else {
             hun = 0;
         }
+        */
         //pc.printf("asen-->%f",asen);
         ec = 0;
         //pc.printf("sx--->%dsy--->%d\n\r",sw1,sw1);
@@ -178,7 +192,7 @@
             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]);
+                omni_cont(hun,aval,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]);
                 mo1 = motor[0];
                 mo2 = motor[1];
                 mo3 = motor[2];
@@ -187,20 +201,20 @@
                 mop3 = motp[2];
                 
             }else
-            if (hun == 1){
+            if (hun == 2){
                 mo1 = 1;
                 mo2 = 1;
                 mo3 = 1;
-                mop1 = 0.5;
-                mop2 = 0.5;
-                mop3 = 0.5;
-            }else if(hun == 2){
+                mop1 = (fabs(aval)/180);
+                mop2 = (fabs(aval)/180);
+                mop3 = (fabs(aval)/180);
+            }else if(hun == 1){
                 mo1 = 2;
                 mo2 = 2;
                 mo3 = 2;
-                mop1 = 0.5;
-                mop2 = 0.5;
-                mop3 = 0.5;
+                mop1 = (fabs(aval)/180);
+                mop2 = (fabs(aval)/180);
+                mop3 = (fabs(aval)/180);
             }else {
                 mo1 = 0;
                 mo2 = 0;