2015_robocon_bteam / Mbed 2 deprecated 2015robot_main_zenkoku

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   PID QEI mbed

Diff: machine_ps3.h

Revision:
33:a4323c20494b
Parent:
32:b8c8ad2eeca7
Child:
35:7b6786193aa2
--- a/machine_ps3.h	Wed Nov 11 06:40:39 2015 +0000
+++ b/machine_ps3.h	Thu Nov 12 07:23:30 2015 +0000
@@ -136,25 +136,36 @@
 {
     velocity_following();
     arg_following();
-    
-    if(flagf==0){
-        if(x2>0.0){
-            Vr = -x1 + x2;
-            Vl = -x1;
+    if(autoflag){
+        if(flagf==0){
+            if(x2>0.0){
+                Vr = -x1 + x2;
+                Vl = -x1;
+            }
+            else{
+                Vr = -x1;
+                Vl = -x1 - x2;
+            }
         }
-        else{
-            Vr = -x1;
-            Vl = -x1 - x2;
+        else if(flagf==1){
+            if(x2>0.0){
+                Vr = x1;
+                Vl = x1 - x2;
+            }
+            else{
+                Vr = x1 + x2;
+                Vl = x1;
+            }
         }
     }
-    else if(flagf==1){
-        if(x2>0.0){
-            Vr = x1;
-            Vl = x1 - x2;
+    if(!autoflag){
+        if(flagf==0){
+            Vr = ((-x1) + x2)/2.0;
+            Vl = ((-x1) - x2)/2.0;
         }
-        else{
-            Vr = x1 + x2;
-            Vl = x1;
+        else if(flagf==1){
+            Vr = (x1 + x2)/2.0;
+            Vl = (x1 - x2)/2.0;
         }
     }
     Move_r( ( float ) Vr );
@@ -162,29 +173,37 @@
 }
 
 /***The function reset state.***/
-void resetState(short d){
+void resetState(int d){
 #ifdef BLUE
     x=0.0;
     y=0.0;
-    if(d){
+    if(d==1){
         sita=PI/4.0;
         targ_sita=PI/4.0;
     }
-    else{
+    else if(d==0){
         sita=3.0*PI/4.0;
         targ_sita=3.0*PI/4.0;
     }
+    else if(d==2){
+        sita=0.0;
+        targ_sita=0.0;
+    }
 #else
     x=0.0;
     y=0.0;
-    if(d){
+    if(d==1){
         sita=-PI/4.0;
         targ_sita=-PI/4.0;
     }
-    else{
+    else if(d==0){
         sita=-3.0*PI/4.0;
         targ_sita=-3.0*PI/4.0;
     }
+    else if(d==2){
+        sita=0.0;
+        targ_sita=0.0;
+    }
 #endif
     targ_velocity=0.0;
     Move_r_sense.reset();