Diff: LOCOMOTION.h

Revision:

26:0ea6a550a99d

Parent:

25:f3bf8351bbd4

Child:

27:fb1298d35bd1

--- a/LOCOMOTION.h	Tue Apr 05 02:30:40 2016 +0000
+++ b/LOCOMOTION.h	Wed Apr 06 04:04:00 2016 +0000
@@ -1,29 +1,31 @@
 #ifndef LOCOMOTION_H
 #define LOCOMOTION_H
-
+ 
 #include "mbed.h"
 #include "LOCALIZE.h"
-
+ 
 #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
-
+#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,
     ANGLE_BIAS  = 1,
 };
-
+ 
 class LOCOMOTION
 {
 public:
@@ -40,15 +42,16 @@
     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);
-
+ 
 protected:
     float s;
     void setMotors(float x);