diff --git a/include/goalie.h b/include/goalie.h index 029fe32..a5044a5 100644 --- a/include/goalie.h +++ b/include/goalie.h @@ -2,12 +2,34 @@ #include "game.h" +#define GOALIE_ATKSPD_LAT 255 +#define GOALIE_ATKSPD_BAK 350 +#define GOALIE_ATKSPD_FRT 345 +#define GOALIE_ATKSPD_STRK 355 +#define GOALIE_ATKDIR_PLUSANG1 20 +#define GOALIE_ATKDIR_PLUSANG2 35 +#define GOALIE_ATKDIR_PLUSANG3 40 +#define GOALIE_ATKDIR_PLUSANGBAK 40 +#define GOALIE_ATKDIR_PLUSANG1_COR 60 +#define GOALIE_ATKDIR_PLUSANG2_COR 70 +#define GOALIE_ATKDIR_PLUSANG3_COR 70 + class Goalie{ public: - Goalie(); + void goalie(); + void palla_dietro(); + void palla_dietroP(); + void storcimentoPorta(); + void storcimentoPorta2(); + void storcimentoZone(); + void storcimentoPortaIncr(); + void leaveMeAlone(); + void ballBack(); + int atk_direction, atk_speed; + /* Goalie(); void goalie(); - void ballBack(); + void ballBack(); */ //void rigore(); to be implemented }; diff --git a/src/goalie.cpp b/src/goalie.cpp index d99cf56..3d0cfe6 100644 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -1,11 +1,79 @@ #include "goalie.h" -Goalie::Goalie(){ +/* Goalie::Goalie(){ + digitalWrite(LED_Y, LOW); +} */ -} +/* void Goalie::goalie(){ + digitalWrite(LED_Y, LOW); + if(ball->angle >= 350 || ball->angle <= 10) { + if(ball->distance > 190) atk_direction = 0; + else atk_direction = ball->angle; + atk_speed = GOALIE_ATKSPD_FRT; + } -void Goalie::goalie(){ - digitalWrite(LED_G, HIGH); + if(ball->angle >= 90 && ball->angle <= 270) { + 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 >= 330 || ball->angle <= 30) && ball->distance > 190) { //storcimento + atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito + drive->prepareDrive(atk_direction, atk_speed, 0); + } + else drive->prepareDrive(atk_direction, atk_speed, 0); + /* digitalWrite(LED_G, HIGH); if(ball->ballSeen){ if(ball->angle >= 0 && ball->angle < 45) drive->drive(ball->angle, 75, 0); else if(ball->angle >= 45 && ball->angle <= 60) drive->drive(ball->angle, 75, 0); @@ -14,9 +82,21 @@ void Goalie::goalie(){ } else { drive->prepareDrive(0, 0, 0); drive->drive(0, 0, 0); - } -} + } */ void Goalie::ballBack(){ - + int ballangle2; + int dir; + int plusang; + + if(ball->distance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK; + else plusang = 0; + + if(ball->angle > 180) ballangle2 = ball->angle - 360; + else ballangle2 = ball->angle; + if(ballangle2 > 0) dir = ball->angle + plusang; //45 con 8 ruote + else dir = ball->angle - plusang; //45 con 8 ruote + if(dir < 0) dir = dir + 360; + else dir = dir; + atk_direction = dir; } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 77614af..9324dd1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,6 +1,82 @@ #include #include "game.h" +#include "goalie.h" + +class Trial{ + public: + void trial(){ + digitalWrite(LED_Y, LOW); + if(ball->angle >= 350 || ball->angle <= 10) { + if(ball->distance > 190) goalie->atk_direction = 0; + else goalie->atk_direction = ball->angle; + goalie->atk_speed = GOALIE_ATKSPD_FRT; + } + + if(ball->angle >= 90 && ball->angle <= 270) { + goalie->ballBack(); + goalie->atk_speed = GOALIE_ATKSPD_BAK; + } + if(digitalRead(SWITCH_DX) == 1) { + if(ball->angle > 10 && ball->angle < 30) { + goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle >= 30 && ball->angle < 45) { + goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle >= 45 && ball->angle < 90) { + goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle > 270 && ball->angle <= 315) { + goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle > 315 && ball->angle <= 330) { + goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle > 330 && ball->angle < 350) { + goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + } else { + if(ball->angle > 10 && ball->angle < 30) { + goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1_COR; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle >= 30 && ball->angle < 45) { + goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2_COR; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle >= 45 && ball->angle < 90) { + goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3_COR; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle > 270 && ball->angle <= 315) { + goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle > 315 && ball->angle <= 330) { + goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + if(ball->angle > 330 && ball->angle < 350) { + goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1; + goalie->atk_speed = GOALIE_ATKSPD_LAT; + } + } + + if((ball->angle >= 330 || ball->angle <= 30) && ball->distance > 190) { //storcimento + goalie->atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito + drive->prepareDrive(goalie->atk_direction, goalie->atk_speed, 0); + } + else drive->prepareDrive(goalie->atk_direction, goalie->atk_speed, 0); +} + +}; void setup() { @@ -13,9 +89,10 @@ void setup() { void loop() { updateSensors(); - - drive->prepareDrive(0,0,0); - linesCtrl->update(); + Trial trial; + trial.trial(); + //drive->prepareDrive(0,0,0); + //linesCtrl->update(); drive->drivePrepared(); diff --git a/src/motor.cpp b/src/motor.cpp index 0bf5f13..ccdf0ec 100644 --- a/src/motor.cpp +++ b/src/motor.cpp @@ -11,8 +11,6 @@ Motor::Motor(int a, int b, int pwm, int angle_){ pinMode(pinA, OUTPUT); pinMode(pinB, OUTPUT); pinMode(pinPwm, OUTPUT); - - angle = 0; } Motor::Motor(){ }