diff --git a/include/positionsys_zone.h b/include/positionsys_zone.h index c7e1c3e..1dc99f0 100644 --- a/include/positionsys_zone.h +++ b/include/positionsys_zone.h @@ -56,7 +56,7 @@ #define SOUTH_EAST 9 #define CAMERA_CENTER_X 3 -#define CAMERA_CENTER_Y 1 +#define CAMERA_CENTER_Y 10 class PositionSysZone : public PositionSystem{ public: diff --git a/include/vars.h b/include/vars.h index 560cb30..e8fec13 100755 --- a/include/vars.h +++ b/include/vars.h @@ -1,5 +1,5 @@ #pragma once -#define DEBUG Serial3 +#define DEBUG Serial #define LED_R 20 #define LED_Y 17 diff --git a/src/main.cpp b/src/main.cpp index 0a39d4d..88fb1e9 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -23,6 +23,9 @@ void loop() { goalie->play(role==1); keeper->play(role==0); + camera->test(); + + // Last thing to do: movement and update status vector drive->drivePrepared(); diff --git a/src/positionsys_zone.cpp b/src/positionsys_zone.cpp index 0ab9559..84c0baf 100644 --- a/src/positionsys_zone.cpp +++ b/src/positionsys_zone.cpp @@ -373,8 +373,8 @@ void PositionSysZone::testLogicZone(){ void PositionSysZone::goCenter() { - 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); + if((camera->true_yb + camera->true_yy) >= 0) 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(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);