Antoine Laurens
added
Dependencies: BNO055_fusion PowerControl mbed
Fork of
TEAM_G_FLOW_RIDA
by 
Edwin Cho
Revision 26:5ae5ef623b72, committed 2016-04-06
- Comitter:
- alaurens
- Date:
- Wed Apr 06 03:54:29 2016 +0000
- Parent:
- 25:f3bf8351bbd4
- Commit message:
- aa
Changed in this revision
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();