added

Dependencies:   BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

Files at this revision

API Documentation at this revision

Comitter:
alaurens
Date:
Wed Apr 06 03:54:29 2016 +0000
Parent:
25:f3bf8351bbd4
Commit message:
aa

Changed in this revision

LOCALIZE.cpp Show annotated file Show diff for this revision Revisions of this file
LOCOMOTION.cpp Show annotated file Show diff for this revision Revisions of this file
LOCOMOTION.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r f3bf8351bbd4 -r 5ae5ef623b72 LOCALIZE.cpp
--- a/LOCALIZE.cpp	Tue Apr 05 02:30:40 2016 +0000
+++ b/LOCALIZE.cpp	Wed Apr 06 03:54:29 2016 +0000
@@ -84,30 +84,30 @@
         _rx=_rx_p<_rx_n? _rx_p : FRAME_W-RX_OFF-_rx_n;
         _ry=_ry_p<_ry_n? _ry_p : FRAME_H-RY_OFF-_ry_n;
         if(!_sw1 && !_sw2)
-            _rx=RX_OFF;
+            _rx=0;//RX_OFF;
         else if(!_sw3 && !_sw4)
-            _rx=FRAME_W-RX_OFF;
+            _rx=FRAME_W;//-RX_OFF;
     } else if(abs(_xya.a-270)<R_ERROR) {
         _rx=_ry_p<_ry_n? _ry_p : FRAME_W-RY_OFF-_ry_n;
         _ry=_rx_p<_rx_n? FRAME_H-RX_OFF-_rx_p : _rx_n;
         if(!_sw1 && !_sw2)
-            _ry=FRAME_H-RY_OFF;
+            _ry=FRAME_H;//-RY_OFF;
         else if(!_sw3 && !_sw4)
-            _ry=RY_OFF;
+            _ry=0;//RY_OFF;
     } else if(abs(_xya.a-180)<R_ERROR) {
         _rx=_rx_p<_rx_n? FRAME_W-RX_OFF-_rx_p : _rx_n;
         _ry=_ry_p<_ry_n? FRAME_H-RY_OFF-_ry_p : _ry_n;
         if(!_sw1 && !_sw2)
-            _rx=FRAME_W-RX_OFF;
+            _rx=FRAME_W;//-RX_OFF;
         else if(!_sw3 && !_sw4)
-            _rx=RX_OFF;
+            _rx=0;//RX_OFF;
     } else if(abs(_xya.a-90)<R_ERROR) {
         _rx=_ry_p<_ry_n? FRAME_W-RY_OFF-_ry_p : _ry_n;
         _ry=_rx_p<_rx_n? _rx_p : FRAME_H-RX_OFF-_rx_n;
         if(!_sw1 && !_sw2)
-            _ry=RY_OFF;
+            _ry=0;//RY_OFF;
         else if(!_sw3 && !_sw4)
-            _ry=FRAME_H-RY_OFF;
+            _ry=FRAME_H;//-RY_OFF;
     } else {
         //error last value
         _rx=_xya.x;
diff -r f3bf8351bbd4 -r 5ae5ef623b72 LOCOMOTION.cpp
--- a/LOCOMOTION.cpp	Tue Apr 05 02:30:40 2016 +0000
+++ b/LOCOMOTION.cpp	Wed Apr 06 03:54:29 2016 +0000
@@ -128,24 +128,36 @@
     return false;
 }
 
-void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal)
+void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol)
 {
     if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) {
         if(angleGood && xGood) {
             *xCurrState=X_BACKWARDS;
+            wait(0.5);
 
         }
     }
 
-    if(*xCurrState==X_BACKWARDS) {
-        if(xGood && angleGood) {
-            if(*angleGoal==0) {
-                *xCurrState=X_INCREASE;
-            } else {
-                *xCurrState=X_DECREASE;
-            }
+    else if(xGood && angleGood && *xCurrState==X_BACKWARDS) {
+        if(*angleGoal==0) {
+            *xCurrState=ROTATE_1;
+            wait(0.5);
+        } else {
+            *xCurrState=ROTATE_2;
+            wait(0.5);
         }
     }
+
+    else if (xGood && angleGood) {
+        if (*xCurrState==ROTATE_1) {
+            *xCurrState=X_INCREASE;
+            wait(0.5);
+        } else {
+            *xCurrState=X_DECREASE;
+            wait(0.5);
+        }
+    }
+
     switch(*xCurrState) {
         case X_INCREASE:
             *angleGoal=180;
@@ -157,8 +169,8 @@
         case X_DECREASE:
             *angleGoal=0;
             *xTarget=X_NEAR_GOAL;
-            _m1dir=1;
-            _m2dir=1;
+            //_m1dir=1;
+            //_m2dir=1;
             break;
 
         case X_BACKWARDS:
@@ -167,12 +179,24 @@
             } else {
                 *xTarget=X_FAR_GOAL-BACKOFF;
             }
-            _m1dir=0;
-            _m2dir=0;
+            //_m1dir=0;
+            //_m2dir=0;
             break;
+
+        case ROTATE_1:
+            *xTarget=*xTarget;
+            *angleTol=2;
+            *angleGoal=180;
+            break;
+
+        case ROTATE_2:
+            *xTarget=*xTarget;
+            *angleTol=2;
+            *angleGoal=0;
+            break;
+
     }
 }
-
 void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
 {
     if (xGood||yGood) {
@@ -184,7 +208,7 @@
 
 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
 {
-    if (xGood && angleGood && yGood) {
+    if (yGood) {
         *yTarget+=Y_INCREMENT;
     }
 }
@@ -193,5 +217,10 @@
 {
     *xGood=abs(xya.x-xGoal)<xErr;
     *yGood=abs(xya.y-yGoal)<yErr;
-    *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN);    
+    *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN);
+}
+
+void LOCOMOTION::mStop(void)
+{
+    setMotors(0);
 }
\ No newline at end of file
diff -r f3bf8351bbd4 -r 5ae5ef623b72 LOCOMOTION.h
--- a/LOCOMOTION.h	Tue Apr 05 02:30:40 2016 +0000
+++ b/LOCOMOTION.h	Wed Apr 06 03:54:29 2016 +0000
@@ -14,10 +14,12 @@
 #define X_INCREASE 1
 #define X_DECREASE 2
 #define X_BACKWARDS 3
-#define BACKOFF 30
-#define X_NEAR_GOAL 20
-#define X_FAR_GOAL 80
-#define Y_INCREMENT 20
+#define ROTATE_1 4
+#define ROTATE_2 5
+#define BACKOFF 10
+#define X_NEAR_GOAL 0
+#define X_FAR_GOAL 90
+#define Y_INCREMENT 7
 
 enum {
     ANGLE_TURN  = 0,
@@ -40,11 +42,12 @@
     DigitalOut _led3;
     DigitalOut _led4;
     void eStop(void);
+    void mStop(void);
     bool setXPos(int target, int current, int error, int angle);
     bool setYBias(int target, int current, int error, int angle);
     bool setAngle(int target, int current, int error, int mode);
     
-    void setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal);
+    void setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol );
     void setAngleTol(int *angleTol,bool yGood, bool xGood);
     void setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget);
     void check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr);
diff -r f3bf8351bbd4 -r 5ae5ef623b72 main.cpp
--- a/main.cpp	Tue Apr 05 02:30:40 2016 +0000
+++ b/main.cpp	Wed Apr 06 03:54:29 2016 +0000
@@ -3,7 +3,7 @@
 #include "SAFETY.h"
 
 #define WATCHDOG_TIME   10
-//#define PC_MODE         1
+#define PC_MODE         1
 
 #if defined (PC_MODE)
 Serial pc(USBTX, USBRX);
@@ -35,7 +35,7 @@
 bool angleGood=false;
 int xState=X_INCREASE;
 int angleTarget=0;
-int yTarget=100;
+int yTarget=30;
 
 //bool flag=false;
 //int target=20;
@@ -70,11 +70,16 @@
     led4=0;
     suction.pulsewidth_us(1000);
     while(1) {
+        if (yTarget>120){
+        motion.mStop();
+        safe.kick();
+        continue;
+        }
         //suction.pulsewidth_us(1900);
         loc.get_xy(&xya);
         motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error);
         
-        motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget);
+        motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error);
         motion.setAngleTol(&angle_error,yGood,xGood);
         motion.setYgoal(xGood,angleGood,yGood,&yTarget);
         if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
@@ -96,7 +101,7 @@
 
 
 #if defined(PC_MODE)
-        pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.x,xya.y,xya.a,xGood,angleGood,xState);
+        pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState);
 #endif
         //Feed the dog
         safe.kick();