Diff: main.cpp

Revision:

4:0fc7221e2f79

Parent:

3:0bd9ad37f319

Child:

5:ba883a4bddc3

--- a/main.cpp	Thu Jul 23 08:55:47 2015 +0000
+++ b/main.cpp	Thu Jul 23 09:18:20 2015 +0000
@@ -146,6 +146,44 @@
     
 } 
 
+//ロボットの動き方
+char robot_Action(double robot_angle, double target_angle) {
+    
+    double n, t, r;
+    t = target_angle;
+    r = robot_angle;
+    n = r - t;
+    if(n<0) n *= -1;
+    
+    
+    if(t==r) {
+        //前進
+        return 'f';
+    } else if(n < 4) {
+        if(t > r)
+            t -= 8;
+        else
+            r -= 8; 
+            
+        if(r-t > 0)
+            return 'l';
+        else
+            return 'r';
+    } else if(n >= 4) {
+        if(t > r)
+            t -= 8;
+        else
+            r -= 8; 
+            
+        if(r-t > 0)
+            return 'r';
+        else
+            return 'l';
+    }
+    
+    return 'b';
+}
+
 /******************************
 スタンバイモード
 ******************************/
@@ -164,7 +202,15 @@
 走行モード
 ******************************/
 void running(){
-            
+    double r = 6378.137;
+    double y1 = get_ta
+    double y2 = 
+    double x1 =
+    double x2 =
+    set_target_distance(r*acos(sin);
+    if(get_compass_z < 0) {
+        //ひっくり返っている
+    }    
 }
 
 /******************************