EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

keeper: initial keeper

keeper
EmaMaker 2022-07-03 13:24:17 +02:00
parent 27d4488de7
commit 95b4cd37b5
4 changed files with 235 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
include/strategy_roles/keeper.h
View File

@ -1,33 +1,65 @@
#pragma once
#include "behaviour_control/complementary_filter.h"
#include "strategy_roles/game.h"
#include "systems/position/positionsys_camera.h"
#include <Arduino.h>
#define KEEPER_3_POINTS
//#define KEEPER_5_POINTS
#ifdef KEEPER_3_POINTS
// #ifdef KEEPER_3_POINTS
#define KEEPER_POINT_LEFT_2 -2
#define KEEPER_POINT_LEFT -1
#define KEEPER_POINT_CENTER 0
#define KEEPER_POINT_RIGHT 1
#define KEEPER_POINT_LEFT_C CAMERA_GOAL_MIN_X
#define KEEPER_POINT_RIGHT_2 2
#define KEEPER_POINT_LEFT_2_C CAMERA_GOAL_MIN_X
#define KEEPER_POINT_LEFT_C (CAMERA_GOAL_X + abs(CAMERA_GOAL_MIN_X) / -2)
#define KEEPER_POINT_CENTER_C CAMERA_GOAL_X
#define KEEPER_POINT_RIGHT_C CAMERA_GOAL_MAX_X
#endif
#define KEEPER_ATTACK_DISTANCE 100
#define KEEPER_POINT_RIGHT_C ((CAMERA_GOAL_X + CAMERA_GOAL_MAX_X) / 2)
#define KEEPER_POINT_RIGHT_2_C CAMERA_GOAL_MAX_X
class Keeper : public Game{
// #define KEEPER_Y_CORNER -45
public:
Keeper();
Keeper(LineSystem*, PositionSystem*);
public:
bool shouldStrike;
// #define KEEPER_PANIC_Y -25
private:
void realPlay() override;
void init() override;
void keeper();
#define KEEPER_VEL MAX_VEL
int point_spacing, ball_x;
// #endif
#define KEEPER_ATTACK_DISTANCE 140
class Keeper : public Game
{
public:
Keeper();
Keeper(LineSystem *, PositionSystem *);
public:
bool shouldStrike = false;
private:
void realPlay() override;
void init() override;
void keeper();
void oscillate();
void kick();
int point_spacing, ball_x;
int x = CAMERA_GOAL_X, y = CAMERA_GOAL_Y;
bool can_change_coords = true;
unsigned long t = millis();
float tilt = 0;
bool b = false;
int kick_status = -1;
int kick_ball_angle = 0;
int kick_start_x = 0;
int kick_start_y = 0;
int goal_angle = 0, goal_distance = 0;
ComplementaryFilter *tiltfilter = new ComplementaryFilter(0.8);
ComplementaryFilter *ballfilter = new ComplementaryFilter(0.65);
};

3
src/main.cpp
View File

@ -50,7 +50,8 @@ void loop() {
drive->resetDrive();
testmenu->testMenu();
striker->play(1);
keeper->play(!role);
striker->play(role || ((Keeper*)keeper)->shouldStrike);
drive->drivePrepared();
updateStatusVector();

200
src/strategy_roles/keeper.cpp
View File

@ -2,8 +2,8 @@
#include "behaviour_control/status_vector.h"
#include "sensors/sensors.h"
#include "strategy_roles/keeper.h"
#include "strategy_roles/games.h"
#include "strategy_roles/keeper.h"
#include "systems/position/positionsys_camera.h"
#include <Arduino.h>
@ -27,30 +27,196 @@ void Keeper::init()
void Keeper::realPlay()
{
if (ball->ballSeen)
keeper();
oscillate();
else
ps->centerGoal();
}
void Keeper::oscillate()
{
if (kick_status == 0)
{
// start kicking, go towards ball (timed event)
drive->prepareDrive(kick_ball_angle, MAX_VEL_HALF, 0);
if (millis() - t >= 300)
{
kick_status++;
t = millis();
}
}
else if (kick_status == 1)
{
if (millis() - t >= 350)
{
kick_status++;
}
else
{
drive->prepareDrive((kick_ball_angle + 180) % 360, MAX_VEL_HALF, 0);
}
}
else
{
// default to follow ball
keeper();
// ball in roughly the same position
if ((CURRENT_DATA_READ.ballAngleFix - getDataAtIndex_backwardsFromCurrent(1).ballAngleFix) / 100 <= 0.1 && CURRENT_DATA_READ.ballDistance <= 110 && (CURRENT_DATA_READ.ballDistance - getDataAtIndex_backwardsFromCurrent(1).ballDistance) / 100 <= 0.1)
{
if (!b)
{
// if not started start timer
t = millis();
b = true;
}
}
else
b = false; // if changed position stop timer
// timer started and 3secs passed
if (b && millis() - t >= 3000)
{
// start kick
kick_status = 0;
b = false;
kick_ball_angle = CURRENT_DATA_READ.ballAngleFix;
kick_start_x = CURRENT_DATA_READ.posx;
kick_start_y = CURRENT_DATA_READ.posy;
// restart the timer since kicking is a timed event
t = millis();
}
}
}
void Keeper::keeper()
{
shouldStrike = false;
shouldStrike = ball->isInFront() && CURRENT_DATA_READ.ballDistance <= KEEPER_ATTACK_DISTANCE;
if (shouldStrike) return;
if(CURRENT_DATA_READ.ballDistance < KEEPER_ATTACK_DISTANCE || (CURRENT_DATA_READ.ballAngle >= 90 && CURRENT_DATA_READ.ballAngle <= 270)){
shouldStrike = true;
return;
}
if(CURRENT_DATA_READ.ballAngle >= 340 || CURRENT_DATA_READ.ballAngle <= 20) currentPosition = currentPosition; //Unneeded, just here for clarity
else if(CURRENT_DATA_READ.ballAngle > 20 && CURRENT_DATA_READ.ballAngle < 90) currentPosition ++;
else if(CURRENT_DATA_READ.ballAngle > 270 && CURRENT_DATA_READ.ballAngle < 340) currentPosition --;
else{
shouldStrike = true;
int speed = MAX_VEL_HALF, dir = 0;
speed = 0;
if (CURRENT_DATA_READ.defSeen)
{
goal_angle = (CURRENT_DATA_READ.angleDefFix + 360) % 360;
goal_distance = CURRENT_DATA_READ.distDef;
}
currentPosition = constrain(currentPosition, KEEPER_POINT_LEFT, KEEPER_POINT_RIGHT);
if (CURRENT_DATA_READ.ballAngleFix >= MOUTH_MAX_ANGLE && CURRENT_DATA_READ.ballAngleFix <= 180)
{
if (goal_angle <= 220)
{
dir = 90;
speed = KEEPER_VEL;
tilt = tiltfilter->calculate(0);
}
else
{
dir = 0;
speed = 0;
tilt = tiltfilter->calculate(tilt + 0.5);
}
}
else if (CURRENT_DATA_READ.ballAngleFix <= MOUTH_MIN_ANGLE && CURRENT_DATA_READ.ballAngleFix > 180)
{
if (goal_angle >= 160)
{
dir = 270;
speed = KEEPER_VEL;
tilt = tiltfilter->calculate(0);
}
else
{
dir = 0;
speed = 0;
tilt = tiltfilter->calculate(tilt - 0.5);
}
}
if(currentPosition == KEEPER_POINT_LEFT) ((PositionSysCamera*)ps)->setMoveSetpoints(KEEPER_POINT_LEFT_C, CAMERA_GOAL_Y);
else if(currentPosition == KEEPER_POINT_CENTER) ((PositionSysCamera*)ps)->setMoveSetpoints(KEEPER_POINT_CENTER_C, CAMERA_GOAL_Y);
else if(currentPosition == KEEPER_POINT_RIGHT) ((PositionSysCamera*)ps)->setMoveSetpoints(KEEPER_POINT_RIGHT_C, CAMERA_GOAL_Y);
tilt = constrain(tilt, -25, 25);
// if ((angle >= 220 || angle <= 160) && ball->isInFront())
// {
if ((CURRENT_INPUT_READ.lineByte & 16) == 16)
{ // 16 OUT 1 IN front
dir -= 15 * sins[dir];
if (speed == 0) speed = MAX_VEL_EIGTH;
}
else if ((CURRENT_INPUT_READ.lineByte & 64) == 64)
{ // 64 OUT 4 IN back
dir += 30 * sins[dir];
if (speed == 0) speed = MAX_VEL_EIGTH;
}
// }
if (CURRENT_INPUT_READ.lineByte == 0)
{
speed = MAX_VEL_QUARTER;
if (goal_distance <= 21 || goal_distance >= 24)
if (goal_distance <= 23)
dir = (goal_angle + 360 + 180) % 360;
else
dir = (goal_angle + 360) % 360;
}
int tmp = ((int)tilt + 360) % 360;
dir = dir - tmp;
if (dir < 0) dir += 360;
drive->prepareDrive(dir, speed, tilt);
}
// shouldStrike = false;
// if (can_change_coords)
// {
// if (CURRENT_DATA_READ.ballAngleFix >= 340 || CURRENT_DATA_READ.ballAngleFix <= 20)
// currentPosition = currentPosition; // Unneeded, just here for clarity
// else if (CURRENT_DATA_READ.ballAngleFix > 20 && CURRENT_DATA_READ.ballAngleFix < 90)
// {
// currentPosition++;
// }
// else if (CURRENT_DATA_READ.ballAngleFix > 270 && CURRENT_DATA_READ.ballAngleFix < 340)
// {
// currentPosition--;
// }
// else
// {
// shouldStrike = true;
// }
// can_change_coords = false;
// }
// currentPosition = constrain(currentPosition, KEEPER_POINT_LEFT_2, KEEPER_POINT_RIGHT_2);
// switch (currentPosition)
// {
// case KEEPER_POINT_LEFT_2:
// x = KEEPER_POINT_LEFT_2_C;
// y = KEEPER_Y_CORNER;
// break;
// case KEEPER_POINT_LEFT:
// x = KEEPER_POINT_LEFT_C;
// y = CAMERA_GOAL_Y;
// break;
// case KEEPER_POINT_CENTER:
// x = KEEPER_POINT_CENTER_C;
// y = CAMERA_GOAL_Y;
// break;
// case KEEPER_POINT_RIGHT:
// x = KEEPER_POINT_RIGHT_C;
// y = CAMERA_GOAL_Y;
// break;
// case KEEPER_POINT_RIGHT_2:
// x = KEEPER_POINT_RIGHT_2_C;
// y = KEEPER_Y_CORNER;
// break;
// }
// if (((PositionSysCamera *)ps)->isInTheVicinityOf(x, y) || millis() - t >= 250)
// {
// can_change_coords = true;
// t = millis();
// }
// ps->setMoveSetpoints(x, y);

2
src/test_menu.cpp
View File

@ -76,7 +76,7 @@ void TestMenu::testMenu()
delay(100);
break;
case '7':
DEBUG.println("LineSensors Test");
DEBUG.println("<LineSensors Test");
CURRENT_DATA_READ.game->ls->test();
delay(200);
break;