Diff: main.cpp

Revision:

10:cf44f4387bc7

Parent:

9:4fc5d70c3bab

Child:

11:9518f8285906

--- a/main.cpp	Wed Mar 23 07:53:32 2016 +0000
+++ b/main.cpp	Wed Mar 23 16:20:53 2016 +0000
@@ -2,7 +2,8 @@
 #include "LOCOMOTION.h"
 #include "WATCHDOG.h"
 
-#define SPEED_TURN_MIN  0.15
+
+#define SPEED_TURN_MIN  0.20
 #define SPEED_TURN_MAX  0.35
 #define SPEED_FB_MIN    0.15
 #define SPEED_FB_MAX    0.50
@@ -38,7 +39,8 @@
 bool flag=false;
 
 void send();
-void turn(int angle);
+void setAngle(int angle);
+int wrap(int a);
 
 int main()
 {
@@ -63,8 +65,8 @@
         led4=!sw4;
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
-        //loc.get_raw_xy();
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
+        setAngle(20);
         wdt.kick();
     }
 }
@@ -74,52 +76,34 @@
     pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
 }
 
-void turn(int angle)
+void setAngle(int angle)
 {
     float s = 0;
-    if(angle==180) {
-        if(abs(xya.a-angle)<=5)
-            s=SPEED_TURN_MIN;
-        else
-            s=(SPEED_TURN_MAX*abs(xya.a-angle)/180)+SPEED_TURN_MIN;
-        motor1F=s;
-        motor1B=s;
-        motor2F=s;
-        motor2B=s;
-        if(xya.a>angle+5) {
-            dir1=1;
-            dir2=0;
-        } else if(xya.a<angle-5) {
-            dir1=0;
-            dir2=1;
-        } else {
-            motor1F=0;
-            motor1B=0;
-            motor2F=0;
-            motor2B=0;
-        }
-    } else if(angle==0) {
-        if(xya.a>angle+5 && xya.a<180) {
-            s=(SPEED_TURN_MAX*abs(xya.a-angle)/180)+SPEED_TURN_MIN;
-            motor1F=s;
-            motor1B=s;
-            motor2F=s;
-            motor2B=s;
-            dir1=1;
-            dir2=0;
-        } else if(xya.a>180 && xya.a<355) {
-            s=(SPEED_TURN_MAX*abs(xya.a-359)/180)+SPEED_TURN_MIN;
-            motor1F=s;
-            motor1B=s;
-            motor2F=s;
-            motor2B=s;
-            dir1=0;
-            dir2=1;
-        } else {
-            motor1F=0;
-            motor1B=0;
-            motor2F=0;
-            motor2B=0;
-        }
+    int diff = 0;
+    diff = 180-wrap(angle);
+    if(abs(xya.a-angle)<=5)
+        s=SPEED_TURN_MIN;
+    else
+        s=(SPEED_TURN_MAX*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
+    motor1F=s;
+    motor1B=s;
+    motor2F=s;
+    motor2B=s;
+    if(wrap(xya.a+diff)>180+2) {
+        dir1=1;
+        dir2=0;
+    } else if(wrap(xya.a+diff)<180-2) {
+        dir1=0;
+        dir2=1;
+    } else {
+        motor1F=0;
+        motor1B=0;
+        motor2F=0;
+        motor2B=0;
     }
-}
\ No newline at end of file
+}
+
+int wrap(int a)
+{
+    return a%360;
+}