Revision 18:f9012e93edb8, committed 2016-03-29

Comitter:

12104404

Date:

Tue Mar 29 02:12:08 2016 +0000

Parent:

17:2f89826b5679

Commit message:

ROBOT CLEANS THE PATH;

Changed in this revision

--- a/LOCOMOTION.cpp	Tue Mar 29 01:11:09 2016 +0000
+++ b/LOCOMOTION.cpp	Tue Mar 29 02:12:08 2016 +0000
@@ -11,7 +11,7 @@
     _m2dir=0;
 }
 
-bool LOCOMOTION::setXPos(int target, int current, int error)
+bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
 {
     //s = 0;
     if(abs(current-target)<=error)
@@ -19,15 +19,25 @@
     else
         s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07;
     if(current>target+error) {
-        _m1dir=1;
-        _m2dir=1;
+        if(angle==0) {
+            _m1dir=1;
+            _m2dir=1;
+        } else {
+            _m1dir=0;
+            _m2dir=0;
+        }
         _m1f=s;
         _m1b=s;
         _m2f=s;
         _m2b=s;
     } else if(current<target-error) {
-        _m1dir=0;
-        _m2dir=0;
+        if(angle==0) {
+            _m1dir=0;
+            _m2dir=0;
+        } else {
+            _m1dir=1;
+            _m2dir=1;
+        }
         _m1f=s;
         _m1b=s;
         _m2f=s;
@@ -43,7 +53,7 @@
     return false;
 }
 
-bool LOCOMOTION::setYPos(int target, int current, int error)
+bool LOCOMOTION::setYPos(int target, int current, int error, int angle)
 {
     //float s = 0;
     if(abs(current-target)<=error)
@@ -53,16 +63,26 @@
     if(current>target+error) {
         //_m1dir=1;
         //_m2dir=1;
-        _m1f=_m1f*(1+s);
-        _m1b=_m1b*(1+s);
+        if(angle==0) {
+            _m1f=_m1f*(1+s);
+            _m1b=_m1b*(1+s);
+        } else {
+            _m2f=_m2f*(1+s);
+            _m2b=_m2b*(1+s);
+        }
     } else if(current<target-error) {
         //_m1dir=0;
         //_m2dir=0;
-        _m2f=_m2f*(1+s);
-        _m2b=_m2b*(1+s);
+        if(angle==0) {
+            _m2f=_m2f*(1+s);
+            _m2b=_m2b*(1+s);
+        } else {
+            _m1f=_m1f*(1+s);
+            _m1b=_m1b*(1+s);
+        }
     } else {
         s=0;
-        
+
         return true;
     }
     return false;

--- a/LOCOMOTION.h	Tue Mar 29 01:11:09 2016 +0000
+++ b/LOCOMOTION.h	Tue Mar 29 02:12:08 2016 +0000
@@ -22,8 +22,8 @@
     PwmOut _m2b;
     DigitalOut _m1dir;
     DigitalOut _m2dir;
-    bool setXPos(int target, int current, int error);
-    bool setYPos(int target, int current, int error);
+    bool setXPos(int target, int current, int error, int angle);
+    bool setYPos(int target, int current, int error, int angle);
     bool setAngle(int target, int current, int error, int mode);
     int wrap(int num);
 

--- a/main.cpp	Tue Mar 29 01:11:09 2016 +0000
+++ b/main.cpp	Tue Mar 29 02:12:08 2016 +0000
@@ -22,6 +22,7 @@
 int target=20;
 int angle_error=2;
 bool xGood=false;
+int angleTarget=0;
 
 void setTarget();
 void send();
@@ -34,15 +35,19 @@
     pc.baud(9600);
     //pc.printf("Initialized Localization: %d\n",loc.init());
     t.attach(&send,1);
-    tTarget.attach(&setTarget,7);
+    //tTarget.attach(&setTarget,7);
     while(1) {
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
-        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)
+        if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
+            xGood = motion.setXPos(target,xya.x,2,angleTarget);
+            if(motion.setYPos(130,xya.y,2,angleTarget)) {
                 angle_error=2;
-            else
+            } else if(xGood) {
+                target=target==20?80:20;
+                angleTarget=angleTarget==0?180:0;
+                angle_error=2;
+            } else
                 angle_error=10;
         }
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);