diff --git a/include/positionsys_zone.h b/include/positionsys_zone.h index 945a5ee..c7e1c3e 100644 --- a/include/positionsys_zone.h +++ b/include/positionsys_zone.h @@ -55,7 +55,8 @@ #define SOUTH_CENTER 8 #define SOUTH_EAST 9 -#define CAMERA_CENTER 3 +#define CAMERA_CENTER_X 3 +#define CAMERA_CENTER_Y 1 class PositionSysZone : public PositionSystem{ public: diff --git a/src/positionsys_zone.cpp b/src/positionsys_zone.cpp index 26dafad..0ab9559 100644 --- a/src/positionsys_zone.cpp +++ b/src/positionsys_zone.cpp @@ -373,16 +373,15 @@ void PositionSysZone::testLogicZone(){ void PositionSysZone::goCenter() { - /* if((camera->true_yb + camera->true_yy) >= 3 && (camera->true_yb + camera->true_yy) <= -3) { - //dx o sx - if((camera->true_yb + camera->true_yy) < 0) drive->prepareDrive(0, 200, 0); - else if ((camera->true_yb + camera->true_yy) > 0) drive->prepareDrive(270, 200, 0); - } */ - - if(camera->true_xb < -CAMERA_CENTER || camera->true_xy < -CAMERA_CENTER) drive->prepareDrive(90, 75, 0); - else if(camera->true_xb > CAMERA_CENTER || camera->true_xy > CAMERA_CENTER) drive->prepareDrive(270, 75, 0); + if((camera->true_yb + camera->true_yy) >= -CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0); + else if ((camera->true_yb + camera->true_yy) <= CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); else drive->prepareDrive(0, 0, 0); - /* if (zoneIndex == 8) +/* if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0); + else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0); + else drive->prepareDrive(0, 0, 0); */ + /* + PREVIOUS + if (zoneIndex == 8) drive->prepareDrive(330, GOCENTER_VEL); if (zoneIndex == 7) drive->prepareDrive(0, GOCENTER_VEL);