diff --git a/include/goalie.h b/include/goalie.h index 2c03032..0524463 100755 --- a/include/goalie.h +++ b/include/goalie.h @@ -16,7 +16,7 @@ #define GOALIE_ATKDIR_PLUSANG2_COR 70 #define GOALIE_ATKDIR_PLUSANG3_COR 70 -class Goalie : public Game{ +class Goalie : public Game, public PositionSysZone{ public: Goalie(); diff --git a/include/positionsys_zone.h b/include/positionsys_zone.h index 24dedff..c7d71d6 100644 --- a/include/positionsys_zone.h +++ b/include/positionsys_zone.h @@ -60,6 +60,7 @@ class PositionSysZone : public PositionSystem{ PositionSysZone(); void update() override; void test() override; + void goCenter(); //returns the zone calculated int getValue(); @@ -110,7 +111,7 @@ class PositionSysZone : public PositionSystem{ void testLogicZone(); //movement - void goCenter(); + //void goCenter(); void goGoalPost(); void centerGoalPost(); void centerGoalPostCamera(bool); diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index 53b9b5c..c06b124 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -31,7 +31,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) setpoint = 0; pid->SetMode(AUTOMATIC); - pid->SetSampleTime(2); + pid->SetSampleTime(5); canUnlock = true; unlockTime = 0; diff --git a/src/goalie.cpp b/src/goalie.cpp index 0b0c7b7..437a620 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -1,6 +1,7 @@ #include "goalie.h" #include "sensors.h" #include "vars.h" +#include "positionsys_zone.h" Goalie::Goalie() : Game() { init(); @@ -17,7 +18,7 @@ void Goalie::init(){ } void Goalie::realPlay(){ - if(ball->ballSeen) this->goalie(50); + if(ball->ballSeen) this->goCenter(); else drive->prepareDrive(0,0,0); } @@ -36,11 +37,19 @@ void Goalie::goalie(int plusang) { else dir = dir; storcimentoPorta(); +<<<<<<< HEAD if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc); else { drive->prepareDrive(dir, 350, 0); cstorc = 0; } +======= + //if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 350, cstorc); + //else { + drive->prepareDrive(ball->dir, 350, 0); + // cstorc = 0; + //} +>>>>>>> 5a9ce6692fe1b3165fa36a1f5a11c729c8818dd4 } } diff --git a/src/positionsys_zone.cpp b/src/positionsys_zone.cpp index 922b197..6cdc079 100644 --- a/src/positionsys_zone.cpp +++ b/src/positionsys_zone.cpp @@ -373,7 +373,15 @@ void PositionSysZone::testLogicZone(){ void PositionSysZone::goCenter() { - if (zoneIndex == 8) + 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 >= -10 && camera->true_xb <= 10); + else if(camera->true_xb >= -5 && camera->true_xb <= 35) drive->prepareDrive(90, 200, 0); + else if(camera->true_xb >= 5 && camera->true_xb <= -35) drive->prepareDrive(180, 200, 0); + /* if (zoneIndex == 8) drive->prepareDrive(330, GOCENTER_VEL); if (zoneIndex == 7) drive->prepareDrive(0, GOCENTER_VEL); @@ -390,7 +398,7 @@ void PositionSysZone::goCenter() { if (zoneIndex == 1) drive->prepareDrive(180, GOCENTER_VEL); if (zoneIndex == 0) - drive->prepareDrive(135, GOCENTER_VEL); + drive->prepareDrive(135, GOCENTER_VEL); */ } diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 7984d9e..4a16bd5 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -40,7 +40,7 @@ blue_led.on() thresholds = [ (30, 70, -12, 19, 10, 57), # thresholds yellow goal - (0, 31, -2, 39, -68, -25)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + (0, 44, -5, 42, -65, -13)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152) @@ -62,7 +62,7 @@ sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) sensor.set_contrast(+3) sensor.set_saturation(0) -sensor.set_brightness(0) +sensor.set_brightness(-2) sensor.set_quality(0) sensor.set_auto_exposure(False, 10000) sensor.set_auto_gain(True)