Diff: LOCOMOTION.h

Revision:

25:f3bf8351bbd4

Parent:

24:fb1f083ebd62

Child:

26:0ea6a550a99d

--- a/LOCOMOTION.h	Sun Apr 03 07:25:07 2016 +0000
+++ b/LOCOMOTION.h	Tue Apr 05 02:30:40 2016 +0000
@@ -4,8 +4,20 @@
 #include "mbed.h"
 #include "LOCALIZE.h"
 
-#define SPEED_TURN_MIN  0.20
-#define SPEED_TURN_MAX  0.65
+#define SPEED_TURN_MIN  0.15
+#define SPEED_TURN_MAX  0.45
+#define Y_BIAS_MIN      0.50
+#define Y_BIAS_MAX      1.00
+#define SPEED_X_MIN     0.07
+#define SPEED_X_MAX     0.17
+
+#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
 
 enum {
     ANGLE_TURN  = 0,
@@ -29,12 +41,17 @@
     DigitalOut _led4;
     void eStop(void);
     bool setXPos(int target, int current, int error, int angle);
-    bool setYPos(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 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);
 
 protected:
     float s;
-    void stopMotors(void);
+    void setMotors(float x);
     int wrap(int num);
     
 };