Diff: move.cpp

Revision:

15:403e9c57c1a1

Parent:

14:db58d3b0ecde

Child:

16:140e758346ae

--- a/move.cpp	Sat Sep 10 03:15:19 2016 +0000
+++ b/move.cpp	Sat Sep 10 04:37:04 2016 +0000
@@ -97,7 +97,7 @@
 
 }
 
-void turn_abs_rad(float rad)
+void turnrad(float rad)
 {
     green = 1;
 
@@ -132,6 +132,66 @@
 
 }
 
+
+void turnrad_ccw(float rad)
+{
+    green = 1;
+
+    update();
+
+    move((turnspeed+2), -turnspeed);
+
+    while(1) {
+        update();
+        //pc.printf("t:%f\n\r", coordinateTheta());
+        if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
+            move(0,0);
+            break;
+        }
+    }
+
+
+
+    hosei_turn(0, false, rad);
+
+    wait(0.5);
+
+    hosei_turn(0, false, rad);
+
+
+    wait(0.5);
+    green = 0;
+}
+
+void turnrad_cw(float rad)
+{
+    green = 1;
+
+    update();
+
+    move((-1)*(turnspeed+2), turnspeed);
+
+    while(1) {
+        update();
+        //pc.printf("t:%f\n\r", coordinateTheta());
+        if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
+            move(0,0);
+            break;
+        }
+    }
+
+
+
+    hosei_turn(0, false, rad);
+
+    wait(0.5);
+
+    hosei_turn(0, false, rad);
+
+    wait(0.5);
+    green = 0;
+}
+
 void pmove(int x, int y)
 {
     yellow = 1;
@@ -191,7 +251,7 @@
     
     ptheta += nearPi(coordinateTheta() - ptheta);
     
-    turn_abs_rad(ptheta);
+    turnrad(ptheta);
 
     if(length == 0) return;
     
@@ -261,7 +321,7 @@
 
     if(length == 0) return;
 
-    turn_abs_rad(PI + nearPi(coordinateTheta() - PI));
+    turnrad(PI + nearPi(coordinateTheta() - PI));
 
     while(1) {
         update_np();