Revision 17:2f89826b5679, committed 2016-03-29

Comitter:

12104404

Date:

Tue Mar 29 01:11:09 2016 +0000

Parent:

16:d6f15a13c3aa

Child:

18:f9012e93edb8

Commit message:

working

Changed in this revision

--- a/LOCOMOTION.cpp	Sun Mar 27 02:39:07 2016 +0000
+++ b/LOCOMOTION.cpp	Tue Mar 29 01:11:09 2016 +0000
@@ -11,9 +11,66 @@
     _m2dir=0;
 }
 
+bool LOCOMOTION::setXPos(int target, int current, int error)
+{
+    //s = 0;
+    if(abs(current-target)<=error)
+        s=0.07;
+    else
+        s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07;
+    if(current>target+error) {
+        _m1dir=1;
+        _m2dir=1;
+        _m1f=s;
+        _m1b=s;
+        _m2f=s;
+        _m2b=s;
+    } else if(current<target-error) {
+        _m1dir=0;
+        _m2dir=0;
+        _m1f=s;
+        _m1b=s;
+        _m2f=s;
+        _m2b=s;
+    } else {
+        s=0;
+        _m1f=s;
+        _m1b=s;
+        _m2f=s;
+        _m2b=s;
+        return true;
+    }
+    return false;
+}
+
+bool LOCOMOTION::setYPos(int target, int current, int error)
+{
+    //float s = 0;
+    if(abs(current-target)<=error)
+        s=0.50;
+    else
+        s=((1-0.50)*abs(current-target)/FRAME_H)+0.50;
+    if(current>target+error) {
+        //_m1dir=1;
+        //_m2dir=1;
+        _m1f=_m1f*(1+s);
+        _m1b=_m1b*(1+s);
+    } else if(current<target-error) {
+        //_m1dir=0;
+        //_m2dir=0;
+        _m2f=_m2f*(1+s);
+        _m2b=_m2b*(1+s);
+    } else {
+        s=0;
+        
+        return true;
+    }
+    return false;
+}
+
 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
 {
-    float s = 0;
+    s = 0;
     int diff = 0;
     diff = 180-wrap(target);
     if(abs(wrap(current+diff)-180)<=error)
@@ -55,30 +112,4 @@
 int LOCOMOTION::wrap(int num)
 {
     return num%360;
-}
-/*
-void setAngle(int angle)
-{
-    float s = 0;
-    int diff = 0;
-    diff = 180-wrap(angle);
-    if(abs(wrap(xya.a+diff)-180)<=5)
-        s=SPEED_TURN_MIN;
-    else
-        s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*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

--- a/LOCOMOTION.h	Sun Mar 27 02:39:07 2016 +0000
+++ b/LOCOMOTION.h	Tue Mar 29 01:11:09 2016 +0000
@@ -2,6 +2,7 @@
 #define LOCOMOTION_H
 
 #include "mbed.h"
+#include "LOCALIZE.h"
 
 #define SPEED_TURN_MIN  0.20
 #define SPEED_TURN_MAX  0.65
@@ -21,10 +22,12 @@
     PwmOut _m2b;
     DigitalOut _m1dir;
     DigitalOut _m2dir;
+    bool setXPos(int target, int current, int error);
+    bool setYPos(int target, int current, int error);
     bool setAngle(int target, int current, int error, int mode);
     int wrap(int num);
 
 protected:
-
+    float s;
 };
 #endif
\ No newline at end of file

--- a/main.cpp	Sun Mar 27 02:39:07 2016 +0000
+++ b/main.cpp	Tue Mar 29 01:11:09 2016 +0000
@@ -17,8 +17,13 @@
 LOCOMOTION motion(p21, p22, p23, p24, p15, p16);
 
 Ticker t;
+Ticker tTarget;
 bool flag=false;
+int target=20;
+int angle_error=2;
+bool xGood=false;
 
+void setTarget();
 void send();
 //void setAngle(int angle);
 int wrap(int a);
@@ -29,10 +34,17 @@
     pc.baud(9600);
     //pc.printf("Initialized Localization: %d\n",loc.init());
     t.attach(&send,1);
+    tTarget.attach(&setTarget,7);
     while(1) {
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
-        motion.setAngle(0,xya.a,2,ANGLE_TURN);
+        if(motion.setAngle(0,xya.a,angle_error,ANGLE_TURN)) {
+            xGood = motion.setXPos(target,xya.x,2);
+            if(motion.setYPos(130,xya.y,2) || xGood)
+                angle_error=2;
+            else
+                angle_error=10;
+        }
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
         wdt.kick();
     }
@@ -42,35 +54,8 @@
 {
     pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
 }
-/*
-void setAngle(int angle)
+
+void setTarget()
 {
-    float s = 0;
-    int diff = 0;
-    diff = 180-wrap(angle);
-    if(abs(wrap(xya.a+diff)-180)<=5)
-        s=SPEED_TURN_MIN;
-    else
-        s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*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;
-    }
-}*/
-
-int wrap(int a)
-{
-    return a%360;
-}
+    target=target==20?80:20;
+}
\ No newline at end of file