diff --git a/include/linesys_2019.h b/include/linesys_2019.h index 754faac..6275203 100755 --- a/include/linesys_2019.h +++ b/include/linesys_2019.h @@ -27,8 +27,6 @@ class LineSys2019 : public LineSystem{ void update() override; void test() override; void outOfBounds(); - void handleIntern(); - void handleExtern(); private: vector in, out; @@ -36,6 +34,7 @@ class LineSys2019 : public LineSystem{ bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow; int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], i; elapsedMillis exitTimer; - byte outDir, outVel, linesens; + int outDir, outVel; + byte linesens; unsigned long unlockTime; }; \ No newline at end of file diff --git a/include/sensors.h b/include/sensors.h index 1ef031f..915c3b7 100755 --- a/include/sensors.h +++ b/include/sensors.h @@ -23,8 +23,6 @@ void initSensors(); void updateSensors(); -s_extr vector lIn; -s_extr vector lOut; s_extr vector dUs; s_extr DataSourceCtrl* usCtrl; diff --git a/src/data_source_camera.cpp b/src/data_source_camera.cpp index 887f841..8b0c3ca 100755 --- a/src/data_source_camera.cpp +++ b/src/data_source_camera.cpp @@ -60,7 +60,7 @@ void DataSourceCamera :: readSensor(){ negateB = false; datavalid ++; } -} + } } void DataSourceCamera :: postProcess(){ @@ -92,7 +92,7 @@ void DataSourceCamera :: postProcess(){ } } -int DataSourceCamera :: getValueAtk(bool fixed){ +int DataSourceCamera::getValueAtk(bool fixed){ //attacco gialla if(digitalRead(SWITCH_DX) == HIGH){ if(fixed) return fixCamIMU(valY); @@ -100,12 +100,12 @@ int DataSourceCamera :: getValueAtk(bool fixed){ } //attacco blu if(digitalRead(SWITCH_DX) == LOW){ - if(fixed) return fixCamIMU(valB); - return valB; + if(fixed) return fixCamIMU(valB); + return valB; } } -int DataSourceCamera :: getValueDef(bool fixed){ +int DataSourceCamera::getValueDef(bool fixed){ //difendo gialla if(digitalRead(SWITCH_DX) == HIGH){ if(fixed) return fixCamIMU(valY); @@ -113,12 +113,12 @@ int DataSourceCamera :: getValueDef(bool fixed){ } //difendo blu if(digitalRead(SWITCH_DX) == LOW){ - if(fixed) return fixCamIMU(valB); - return valB; + if(fixed) return fixCamIMU(valB); + return valB; } } -void DataSourceCamera :: test(){ +void DataSourceCamera::test(){ goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu @@ -133,7 +133,7 @@ void DataSourceCamera :: test(){ delay(100); } -int DataSourceCamera :: fixCamIMU(int d){ +int DataSourceCamera::fixCamIMU(int d){ if(compass->getValue() > 0 && compass->getValue() < 180) imuOff = compass->getValue(); else if (compass->getValue() <= 360 && compass->getValue() >= 180) imuOff = compass->getValue() - 360; imuOff = constrain(imuOff*0.8, -30, 30); diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index af2653d..2539bc5 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -34,7 +34,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) integral = 0; } -void DriveController::prepareDrive(int dir, int speed, int tilt){ +void DriveController::prepareDrive(int dir=0, int speed=0, int tilt=0){ pDir = dir; pSpeed = speed; pTilt = tilt; diff --git a/src/game.cpp b/src/game.cpp index 28529fd..87619f4 100644 --- a/src/game.cpp +++ b/src/game.cpp @@ -8,7 +8,9 @@ Game::Game(LineSystem* ls_, PositionSystem* ps_) { void Game::play(bool condition){ ps->update(); - if(condition) realPlay(); - ls->update(); + if(condition) { + realPlay(); + ls->update(); + } } \ No newline at end of file diff --git a/src/games.cpp b/src/games.cpp index 3a18f44..93a0253 100644 --- a/src/games.cpp +++ b/src/games.cpp @@ -5,6 +5,9 @@ #include "positionsys_zone.h" void initGames(){ - goalie = new Goalie(new LineSys2019(), new PositionSysZone()); - keeper = new Keeper(new LineSys2019(), new PositionSysZone()); + vector lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) }; + vector lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) }; + + goalie = new Goalie(new LineSys2019(lIn, lOut), new PositionSysZone()); + keeper = new Keeper(new LineSys2019(lOut, lOut), new PositionSysZone()); } \ No newline at end of file diff --git a/src/goalie.cpp b/src/goalie.cpp index 807e76a..ea55f9c 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -13,15 +13,15 @@ Goalie::Goalie(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) { void Goalie::init(){ atk_speed = 0; atk_direction = 0; - + cstorc = 0; } void Goalie::realPlay(){ - goalie(); + if(ball->ballSeen) this->goalie(); + else drive->prepareDrive(0,0,0); } void Goalie::goalie() { - compass->readSensor(); if(ball->angle >= 350 || ball->angle <= 10) { if(ball->distance > 190) atk_direction = 0; else atk_direction = ball->angle; @@ -29,61 +29,35 @@ void Goalie::goalie() { } if(ball->angle >= 90 && ball->angle <= 270) { - ballBack(); + this->ballBack(); atk_speed = GOALIE_ATKSPD_BAK; } - if(digitalRead(SWITCH_DX) == 1) { - if(ball->angle > 10 && ball->angle < 30) { - atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle >= 30 && ball->angle < 45) { - atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle >= 45 && ball->angle < 90) { - atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle > 270 && ball->angle <= 315) { - atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle > 315 && ball->angle <= 330) { - atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle > 330 && ball->angle < 350) { - atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - } else { - if(ball->angle > 10 && ball->angle < 30) { - atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle >= 30 && ball->angle < 45) { - atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle >= 45 && ball->angle < 90) { - atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle > 270 && ball->angle <= 315) { - atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle > 315 && ball->angle <= 330) { - atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle > 330 && ball->angle < 350) { - atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1; - atk_speed = GOALIE_ATKSPD_LAT; - } + if(ball->angle > 10 && ball->angle < 30) { + atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1; + atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle >= 30 && ball->angle < 45) { + atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2; + atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle >= 45 && ball->angle < 90) { + atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3; + atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle > 270 && ball->angle <= 315) { + atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR; + atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle > 315 && ball->angle <= 330) { + atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR; + atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle > 330 && ball->angle < 350) { + atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR; + atk_speed = GOALIE_ATKSPD_LAT; } + this->storcimentoPorta(); if((ball->angle >= 330 || ball->angle <= 30) && ball->distance > 190) { //storcimento atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito drive->prepareDrive(atk_direction, atk_speed, cstorc); diff --git a/src/keeper.cpp b/src/keeper.cpp index a5251f8..1b6e8c3 100755 --- a/src/keeper.cpp +++ b/src/keeper.cpp @@ -24,7 +24,12 @@ void Keeper::init(){ } void Keeper::realPlay() { - + if(ball->ballSeen) keeper(); + else drive->prepareDrive(0,0,0); +} + +void Keeper::keeper() { + if(ball->distance > KEEPER_ATTACK_DISTANCE){ // Ball is quite near goalie->play(); diff --git a/src/linesys_2019.cpp b/src/linesys_2019.cpp index 8319276..687cd5a 100755 --- a/src/linesys_2019.cpp +++ b/src/linesys_2019.cpp @@ -7,8 +7,6 @@ LineSys2019::LineSys2019(vector in_, vector out_){ this->in = in_; this->out = out_; - fboundsOX = false; - fboundsOY = false; fboundsX = false; fboundsY = false; slow = false; @@ -24,11 +22,13 @@ LineSys2019::LineSys2019(vector in_, vector out_){ } exitTimer = 0; + linesens = 0; } void LineSys2019::update(){ inV = 0; outV = 0; + tookLine = false; for(DataSource* d : in) d->readSensor(); for(DataSource* d : out) d->readSensor(); @@ -49,11 +49,8 @@ void LineSys2019::update(){ outV = outV | (linetriggerO[i] << i); } - linesens |= inV | outV; - if ((linesensOldX > 0) || (linesensOldX > 0)) { - fboundsOX = true; - fboundsOY = true; + if ((inV > 0) || (outV > 0)) { if(exitTimer > EXTIME) { fboundsX = true; fboundsY = true; @@ -61,6 +58,7 @@ void LineSys2019::update(){ exitTimer = 0; } + linesens |= inV | outV; outOfBounds(); } @@ -79,6 +77,7 @@ void LineSys2019::outOfBounds(){ if (exitTimer <= EXTIME){ //fase di rientro + digitalWrite(LED_R, HIGH); if(linesens == 15) linesens = linesensOldY | linesensOldX; //ZOZZATA MAXIMA unlockTime = millis(); @@ -103,12 +102,8 @@ void LineSys2019::outOfBounds(){ else if(linesensOldY == 1) outDir = 180; } - outVel = LINES_EXIT_SPD; - drive->prepareDrive(outDir, outVel, 0); + drive->prepareDrive(outDir, LINES_EXIT_SPD, 0); tookLine = true; - // keeper_backToGoalPost = true; - // keeper_tookTimer = true; - }else{ //fine rientro if(linesens == 1) drive->vyp = 1; diff --git a/src/main.cpp b/src/main.cpp index 4bf6849..f02c5c1 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -20,11 +20,6 @@ void loop() { goalie->play(role==1); keeper->play(role==0); - //Check lines before final movement - linesCtrl->update(); - // Last thing to do: movement drive->drivePrepared(); - -/* compass->test(); */ } diff --git a/src/sensors.cpp b/src/sensors.cpp index 0b46aaa..78cb8ce 100755 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -5,18 +5,18 @@ void initSensors(){ pinMode(SWITCH_DX, INPUT); pinMode(SWITCH_SX, INPUT); + pinMode(LED_R, OUTPUT); + pinMode(LED_Y, OUTPUT); + pinMode(LED_G, OUTPUT); + dUs = { new DataSourceUS(&Wire1, int(112)), new DataSourceUS(&Wire1, int(113)), new DataSourceUS(&Wire1, int(114)), new DataSourceUS(&Wire1, int(115)) }; - lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) }; - lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) }; - drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315)); compass = new DataSourceBNO055(); ball = new DataSourceBall(&Serial4, 57600); camera = new DataSourceCamera(&Serial2, 19200); usCtrl = new DataSourceCtrl(dUs); - linesCtrl = new LineSys2019(lIn, lOut); bt = new DataSourceBT(&Serial3, 115200); }