Diff: main.cpp

Revision:

15:02d4a5eed16c

Parent:

14:54f198b9947b

Child:

16:2bb07173f8bf

diff -r 54f198b9947b -r 02d4a5eed16c main.cpp
--- a/main.cpp	Wed Mar 18 00:19:43 2015 +0000
+++ b/main.cpp	Wed Mar 18 05:14:45 2015 +0000
@@ -88,10 +88,10 @@
     double md1,md2,md3,t1;
     if (hun == 2)
     {
-        t1 = (fabs(aval)/180)*0.3;
+        t1 = 0.2;
     }else if(hun == 1)
     {
-        t1 = -(fabs(aval)/180)*0.3;
+        t1 =-0.2;
     }else{
         t1 = 0;
     }
@@ -148,13 +148,14 @@
         if(hun == 1||hun == 2){
             t.start();
         }else {
+            i = 0;
             t.reset();
             t.stop();
         }
-        if ((hun == 1)&& (t.read_ms()>(i*10))){
+        if ((hun == 1) && (t.read_ms()>(i*1))){
             asen = asen+2;
             i++;
-        }else if((hun == 2)&& (t.read_ms()>(i*10))){
+        }else if((hun == 2) && (t.read_ms()>(i*10))){
             asen = asen-2;
             i++;
         }
@@ -168,9 +169,9 @@
         val %= 360;
         val -= 180;
         printf("   :%d\n\r",val);
-        if (val > 5){
+        if (val > 1){
             hun = 2;
-        }else if (val <-5){
+        }else if (val <-1){
             hun = 1;
         }else{
             hun = 0;
@@ -191,7 +192,7 @@
         if (ec == 0){
             em.reset();
             theta = get_theta(sx,sy);
-            theta -=asen2;
+            theta -= (asen-asen2)*pi/180;
             //pc.printf("%f\n\r",theta);
             if (sw1 == -1){
                 omni_cont(hun,aval,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]);
@@ -201,7 +202,6 @@
                 mop1 = motp[0];
                 mop2 = motp[1];
                 mop3 = motp[2];
-                
             }else
             if (hun == 2){
                 mo1 = 1;