From 8fffdfd83bb1be9d9639ce27eb345eb8fef6bfed Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Mon, 13 Jan 2020 17:07:26 +0100 Subject: [PATCH] first test with new code, plays great --- include/game.h | 4 +- include/keeper.h | 2 +- include/linesys_2019.h | 6 +- include/systems.h | 1 + src/keeper.cpp | 7 +- src/linesys_2019.cpp | 185 +++++++++++++++++++-------------------- src/positionsys_zone.cpp | 2 +- 7 files changed, 105 insertions(+), 102 deletions(-) diff --git a/include/game.h b/include/game.h index 7acc2f9..cdbdffc 100755 --- a/include/game.h +++ b/include/game.h @@ -9,9 +9,9 @@ class Game { Game(); Game(LineSystem* ls, PositionSystem* ps); void play(bool condition=true); + LineSystem* ls; + PositionSystem* ps; private: virtual void realPlay() = 0; virtual void init() = 0; - LineSystem* ls; - PositionSystem* ps; }; \ No newline at end of file diff --git a/include/keeper.h b/include/keeper.h index ec7cfb0..ab39d81 100755 --- a/include/keeper.h +++ b/include/keeper.h @@ -35,6 +35,6 @@ class Keeper : public Game{ int defSpeed, defDir; float angle, angleX, angleY; - elapsedMillis t, toh, keeperAttackTimer; + elapsedMillis t, keeperAttackTimer; bool keeper_tookTimer, keeper_backToGoalPost; }; diff --git a/include/linesys_2019.h b/include/linesys_2019.h index 2c15694..754faac 100755 --- a/include/linesys_2019.h +++ b/include/linesys_2019.h @@ -16,6 +16,7 @@ #define LINE_THRESH 90 #define EXTIME 100 +#define LINES_EXIT_SPD 350 class LineSys2019 : public LineSystem{ @@ -33,7 +34,8 @@ class LineSys2019 : public LineSystem{ vector in, out; DataSource* ds; bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow; - int inV, outV, inVOldX, inVOldY, value, linetriggerI[4], linetriggerO[4], i; + int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], i; elapsedMillis exitTimer; - int outDir, outVel; + byte outDir, outVel, linesens; + unsigned long unlockTime; }; \ No newline at end of file diff --git a/include/systems.h b/include/systems.h index 50d7920..84d4e5e 100644 --- a/include/systems.h +++ b/include/systems.h @@ -4,6 +4,7 @@ class LineSystem{ public: virtual void update() = 0; virtual void test() = 0; + bool tookLine; }; class PositionSystem{ diff --git a/src/keeper.cpp b/src/keeper.cpp index c4236d4..a5251f8 100755 --- a/src/keeper.cpp +++ b/src/keeper.cpp @@ -1,13 +1,15 @@ #include "keeper.h" #include "sensors.h" #include "games.h" +#include "linesys_2019.h" #include Keeper::Keeper() : Game() { init(); } -Keeper::Keeper(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_){} +Keeper::Keeper(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_){ +} void Keeper::init(){ defSpeed = 0; @@ -16,7 +18,6 @@ void Keeper::init(){ angleX = 0; angleY = 0; t = 0; - toh= 0; keeperAttackTimer = 0; keeper_tookTimer = false; keeper_backToGoalPost = false; @@ -27,7 +28,7 @@ void Keeper::realPlay() { if(ball->distance > KEEPER_ATTACK_DISTANCE){ // Ball is quite near goalie->play(); - if(!keeper_tookTimer){ + if(!this->ls->tookLine){ keeperAttackTimer = 0; keeper_tookTimer = true; } diff --git a/src/linesys_2019.cpp b/src/linesys_2019.cpp index 25e5bdc..8319276 100755 --- a/src/linesys_2019.cpp +++ b/src/linesys_2019.cpp @@ -13,8 +13,10 @@ LineSys2019::LineSys2019(vector in_, vector out_){ fboundsY = false; slow = false; - inVOldX = 0; - inVOldY = 0; + linesensOldX = 0; + linesensOldY = 0; + + tookLine = false; for(int i = 0; i < 4; i++){ linetriggerI[i] = 0; @@ -27,6 +29,7 @@ LineSys2019::LineSys2019(vector in_, vector out_){ void LineSys2019::update(){ inV = 0; outV = 0; + for(DataSource* d : in) d->readSensor(); for(DataSource* d : out) d->readSensor(); @@ -46,9 +49,9 @@ void LineSys2019::update(){ outV = outV | (linetriggerO[i] << i); } - inV = inV | outV; + linesens |= inV | outV; - if ((inV > 0) || (outV > 0)) { + if ((linesensOldX > 0) || (linesensOldX > 0)) { fboundsOX = true; fboundsOY = true; if(exitTimer > EXTIME) { @@ -62,109 +65,105 @@ void LineSys2019::update(){ } void LineSys2019::outOfBounds(){ - handleExtern(); - handleIntern(); -} - -void LineSys2019::handleIntern(){ if(fboundsX == true) { - if(inV & 0x02) inVOldX = 2; - else if(inV & 0x08) inVOldX = 8; - if(inVOldX != 0) fboundsX = false; + if(linesens & 0x02) linesensOldX = 2; + else if(linesens & 0x08) linesensOldX = 8; + if(linesensOldX != 0) fboundsX = false; } if(fboundsY == true) { - if(inV & 0x01) inVOldY = 1; - else if(inV & 0x04) inVOldY = 4; - if(inVOldY != 0) fboundsY = false; + if(linesens & 0x01) linesensOldY = 1; + else if(linesens & 0x04) linesensOldY = 4; + if(linesensOldY != 0) fboundsY = false; } + if (exitTimer <= EXTIME){ - drive->canUnlock = false; - drive->unlockTime = millis(); - //fase di rientro - if(inV == 15) { - inV = inVOldY | inVOldX; //ZOZZATA MAXIMA - //digitalWrite(Y, HIGH); + if(linesens == 15) linesens = linesensOldY | linesensOldX; //ZOZZATA MAXIMA + unlockTime = millis(); + + if(linesens == 1) outDir = 180; + else if(linesens == 2) outDir = 270; + else if(linesens == 4) outDir = 0; + else if(linesens == 8) outDir = 90; + else if(linesens == 3) outDir = 225; + else if(linesens == 6) outDir = 315; + else if(linesens == 12) outDir = 45; + else if(linesens == 9) outDir = 135; + else if(linesens == 7) outDir = 270; + else if(linesens == 13) outDir = 90; + else if(linesens == 11) outDir = 180; + else if(linesens == 14) outDir = 0; + else if(linesens == 5){ + if(linesensOldX == 2) outDir = 270; + else if(linesensOldY == 8) outDir = 90; + } + else if(linesens == 10){ + if(linesensOldY == 4) outDir = 0; + else if(linesensOldY == 1) outDir = 180; } - switch(inV){ - case 1: - outDir = 180; - break; - case 2: - outDir = 270; - break; - case 4: - outDir = 0; - break; - case 8: - outDir = 90; - break; - case 3: - outDir = 225; - break; - case 6: - outDir = 315; - break; - case 12: - outDir = 45; - break; - case 9: - outDir = 135; - break; - case 7: - outDir = 270; - break; - case 13: - outDir = 90; - break; - case 11: - outDir = 180; - break; - case 14: - outDir = 0; - break; - case 5: - //digitalWrite(R, HIGH); - if(inVOldX == 2) outDir = 270; - if(inVOldX == 8) outDir = 90; - break; - case 10: - if(inVOldY == 4) outDir = 0; - if(inVOldY == 1)outDir = 180; - break; - case 15: - break; - case 0: - default: - //;) - break; - } - - if(exitTimer < 45) outVel = 350; - else outVel = 330; + outVel = LINES_EXIT_SPD; drive->prepareDrive(outDir, outVel, 0); + tookLine = true; // keeper_backToGoalPost = true; // keeper_tookTimer = true; + }else{ - inV = 0; - inVOldX = 0; - inVOldY = 0; - // lineSensByteBak = 30; - drive->canUnlock = true; + //fine rientro + if(linesens == 1) drive->vyp = 1; + else if(linesens == 2) drive->vxp = 1; + else if(linesens == 4) drive->vyn = 1; + else if(linesens == 8) drive->vxn = 1; + else if(linesens == 3) { + drive->vyp = 1; + drive->vxp = 1; + } + else if(linesens == 6){ + drive->vxp = 1; + drive->vyn = 1; + } + else if(linesens == 12) { + drive->vyn = 1; + drive->vxn = 1; + } + else if(linesens == 9) { + drive->vyp = 1; + drive->vxn = 1; + } + else if(linesens == 7) { + drive->vyp = 1; + drive->vyn = 1; + drive->vxp = 1; + } + else if(linesens == 13){ + drive->vxp = 1; + drive->vxn = 1; + drive->vyn = 1; + } + else if(linesens == 11) { + drive->vyp = 1; + drive->vxn = 1; + drive->vxp = 1; + } + else if(linesens == 14) { + drive->vyn = 1; + drive->vxn = 1; + drive->vxp = 1; + } + else if(linesens == 5){ + if(linesensOldX == 2) drive->vyp = 1; + else if(linesensOldY == 8)drive->vyn = 1; + } + else if(linesens == 10){ + if(linesensOldY == 4) drive->vyn = 1; + else if(linesensOldY == 1) drive->vyp = 1; + } + + linesens = 0; + linesensOldY = 0; + linesensOldX = 0; } - -// lineSensByteBak = linesensbyteI; - if(exitTimer == 99) slow = true; - else slow = false; -} - -void LineSys2019::handleExtern(){ - if((outV & 0b00000001) == 1) drive->vyp = 1; // esclusione - if((outV & 0b00000100) == 4) drive->vyn = 1; - if((outV & 0b00000010) == 2) drive->vxp = 1; - if((outV & 0b00001000) == 8) drive->vxn = 1; } void LineSys2019::test(){ diff --git a/src/positionsys_zone.cpp b/src/positionsys_zone.cpp index a76f640..922b197 100644 --- a/src/positionsys_zone.cpp +++ b/src/positionsys_zone.cpp @@ -46,7 +46,7 @@ void PositionSysZone::PositionSysZone::test() { DEBUG.println("------"); for (int i = 0; i < 4; i++) { DEBUG.print("US: "); - usCtrl->test(); + usCtrl->test(); DEBUG.print(" | "); } DEBUG.println();