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

remove robocup stuff

pull/1/head
emamaker 2022-05-16 20:34:35 +02:00
parent 7a1583128a
commit 954809ab6e
24 changed files with 7 additions and 1820 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
include/strategy_roles/corner_kick_1.h
View File

@ -1,29 +0,0 @@
#pragma once
#include "systems/position/positionsys_camera.h"
#include "systems/lines/linesys_camera.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#include "strategy_roles/games.h"
#include "behaviour_control/status_vector.h"
#include "sensors/data_source_ball.h"
#include "strategy_roles/striker.h"
#include "sensors/data_source_ball_presence.h"
#include "sensors/data_source_bt.h"
#include "vars.h"
class CornerKick : public Game{
public:
CornerKick();
CornerKick(LineSystem* ls, PositionSystem* ps);
void init() override;
void realPlay() override;
void kick();
unsigned long kicktimer = 0, debounce_timer = 0;
int kick_state = 99;
bool kick_flag = false;
int old_btn = 0, new_val = 0;
ComplementaryFilter* ballAngleFilter = new ComplementaryFilter(0.85);
};

43
include/strategy_roles/corner_kick_2.h
View File

@ -1,43 +0,0 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define CK2_SPINNER_OVERHEAD 10
#define CK2_KICK_LIMIT_TILT1 200
#define CK2_KICK_LIMIT_MAX 315
#define CK2_KICK_LIMIT_MIN 45
class CornerKick2 : public Game{
public:
CornerKick2();
CornerKick2(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void catchBall();
void spinner(int);
unsigned long timer = 0;
int state = 0;
int ball_catch_state = 0;
int ball_catch_timer = 0;
bool ball_catch_flag = false;
float ball_catch_tilt = 0;
int goal_coords = 0;
ComplementaryFilter* ballAngleFilter = new ComplementaryFilter(0.85);
int limitx = 0, spinner_state = 0;
bool spinner_flag = false;
float tilt1 = 0, tilt2 = 0, spinner_tilt = 0;
unsigned long spinner_timer = 0;
};

12
include/strategy_roles/games.h
View File

@ -9,15 +9,7 @@
#include <Arduino.h>
#include "strategy_roles/game.h"
#include "strategy_roles/striker.h"
#include "strategy_roles/striker_roller.h"
#include "strategy_roles/precision_shooter.h"
#include "strategy_roles/pass_and_shoot.h"
#include "strategy_roles/spot_finder.h"
#include "strategy_roles/the_spinner.h"
#include "strategy_roles/round_robin.h"
#include "strategy_roles/corner_kick_2.h"
#include "strategy_roles/corner_kick_1.h"
// #include "strategy_roles/keeper.h"
#include "strategy_roles/keeper.h"
void initGames();
@ -25,7 +17,7 @@ g_extr Game* striker;
g_extr Game* striker_roller;
g_extr Game* precision_shooter;
g_extr Game* pass_and_shoot;
// g_extr Game* keeper;
g_extr Game* keeper;
g_extr Game* tc1;
g_extr Game* tc2;

28
include/strategy_roles/pass_and_shoot.h
View File

@ -1,28 +0,0 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define PAS_ATTACK_DISTANCE 110
#define PAS_TILT_STOP_DISTANCE 140
#define PAS_PLUSANG 55
#define PAS_PLUSANG_VISIONCONE 10
class PassAndShoot : public Game{
public:
PassAndShoot();
PassAndShoot(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void striker();
int tilt();
int atk_speed, atk_direction, atk_tilt;
float cstorc;
unsigned long pass_counter;
bool gotta_tilt;
};

50
include/strategy_roles/precision_shooter.h
View File

@ -1,50 +0,0 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "behaviour_control/complementary_filter.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define PS_ATTACK_DISTANCE 110
#define PS_TILT_STOP_DISTANCE 140
#define PS_PLUSANG 55
#define PS_PLUSANG_VISIONCONE 10
// There needs to be a little bit of space between the target point and the spot to be in
#define PS_SPINNER_OVERHEAD 7
#define PS_KICK_LIMIT_TILT1 200
#define PS_KICK_LIMIT_MAX 315
#define PS_KICK_LIMIT_MIN 45
class PrecisionShooter : public Game{
public:
PrecisionShooter();
PrecisionShooter(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void catchBall();
void spinner(int);
int tilt();
private:
int atk_speed, atk_direction, atk_tilt;
float cstorc;
bool gotta_tilt;
ComplementaryFilter* ballAngleFilter;
float tilt1 = 0;
float tilt2 = 0;
int spinner_state = 0;
bool spinner_flag = false;
unsigned long spinner_timer = 0;
float spinner_tilt = 0;
int ball_catch_state = 0;
bool ball_catch_flag = false;
unsigned long ball_catch_timer = 0;
float ball_catch_tilt = 0;
int limitx = 0;
};

57
include/strategy_roles/round_robin.h
View File

@ -1,57 +0,0 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "behaviour_control/complementary_filter.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define RR_ATTACK_DISTANCE 110
#define RR_TILT_STOP_DISTANCE 140
#define RR_PLUSANG 55
#define RR_PLUSANG_VISIONCONE 10
// There needs to be a little bit of space between the target point and the spot to be in
#define RR_SPINNER_OVERHEAD 7
#define RR_KICK_LIMIT_TILT1 200
#define RR_KICK_LIMIT_MAX 315
#define RR_KICK_LIMIT_MIN 45
#define ROUND_ROBIN_VEL 30
#define RR_YCOORD -5
#define RR_ROLLER_SPD 1500
class RoundRobin : public Game{
public:
RoundRobin();
RoundRobin(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void catchBall();
void spinner(int);
void push();
void spinnerStep();
private:
int atk_speed, atk_direction, atk_tilt;
float cstorc;
bool gotta_tilt;
ComplementaryFilter* ballAngleFilter;
int flip = 0;
float tilt1 = 0;
float tilt2 = 0;
int spinner_state = 0;
bool spinner_flag = false;
unsigned long spinner_timer = 0;
float spinner_tilt = 0;
int ball_catch_state = 0;
bool ball_catch_flag = false;
unsigned long ball_catch_timer = 0;
float ball_catch_tilt = 0;
int limitx = 0;
};

21
include/strategy_roles/spot_finder.h
View File

@ -1,21 +0,0 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define X_COORD 10
#define Y_COORD 15
class SpotFinder : public Game{
public:
SpotFinder();
SpotFinder(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
unsigned long t = 0;
};

53
include/strategy_roles/striker_roller.h
View File

@ -1,53 +0,0 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "behaviour_control/complementary_filter.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define STRL_ATTACK_DISTANCE 110
#define STRL_TILT_STOP_DISTANCE 140
#define STRL_PLUSANG 55
#define STRL_PLUSANG_VISIONCONE 10
// There needs to be a little bit of space between the target point and the spot to be in
#define STRL_SPINNER_OVERHEAD 7
#define STRL_KICK_LIMIT_TILT1 180
#define STRL_KICK_LIMIT_MAX 300
#define STRL_KICK_LIMIT_MIN 60
class StrikerRoller : public Game{
public:
StrikerRoller();
StrikerRoller(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void catchBall();
void spinner(int);
int tilt();
private:
int atk_speed, atk_direction, atk_tilt;
float cstorc;
bool gotta_tilt;
ComplementaryFilter* ballAngleFilter;
float tilt1 = 0;
float tilt2 = 0;
int spinner_state = 0;
bool spinner_flag = false;
unsigned long spinner_timer = 0;
float spinner_tilt = 0;
int ball_catch_state = 0;
bool ball_catch_flag = false;
unsigned long ball_catch_timer = 0;
float ball_catch_tilt = 0;
int limitx = 0;
unsigned long spinner_end_time = 0;
bool spinner_end_flag = false;
};

56
include/strategy_roles/the_spinner.h
View File

@ -1,56 +0,0 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define X_COORD 10
#define Y_COORD 15
class Spinner : public Game{
public:
Spinner();
Spinner(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void circle();
bool doingCircle = false;
bool firstCircle = true;
bool flag = false;
unsigned long t =0;
int step = 0;
typedef struct v{
v(){
x = 0;
y = 0;
}
v(int x_, int y_){
x = x_;
y = y_;
}
int x, y;
} spot;
/*
spot(13, 15), //top right spot
spot(-13, 15), //top left spot
spot(-13, -15) //bottom left spot
spot(13, -15), //bottom right spot
*/
vector<spot> spots = {
/*START: centre*/
spot(0,0),
// /*1ST BOTTLE*/
spot(20, 15),
};
int current_spot_i = 0;
spot current_spot;
};

49
include/systems/lines/linesys_camera_recovery.h
View File

@ -1,49 +0,0 @@
#pragma once
#include <Arduino.h>
#include "behaviour_control/ds_ctrl.h"
#include "systems/systems.h"
#include "vars.h"
#define S1O A7
#define S1I A6
#define S2O A2
#define S2I A3
#define S3I A9
#define S3O A8
#define S4I A0
#define S4O A1
#define LINE_THRESH_CAM_REC 350
#define EXIT_TIME_REC 300
#define X_LIMIT 4
#define Y_LIMIT 5
class LineSysCameraRecovery : public LineSystem{
public:
LineSysCameraRecovery();
LineSysCameraRecovery(vector<DataSource*> in_, vector<DataSource*> out);
void update() override;
void test() override;
void outOfBounds();
void checkLineSensors();
void striker();
bool isInLimitX();
bool isInLimitY();
private:
vector<DataSource*> in, out;
DataSource* ds;
bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow;
int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], linepins[4], i;
unsigned long exitTimer;
int outDir, outVel;
byte linesens;
int outX, outY;
};

41
include/systems/lines/linesys_camera_roundrobin.h
View File

@ -1,41 +0,0 @@
#pragma once
#include <Arduino.h>
#include "behaviour_control/ds_ctrl.h"
#include "systems/systems.h"
#include "vars.h"
#define S1O A7
#define S1I A6
#define S2O A2
#define S2I A3
#define S3I A9
#define S3O A8
#define S4I A0
#define S4O A1
#define LINE_THRESH_CAM 350
#define EXIT_TIME 300
class LineSysCameraRR : public LineSystem{
public:
LineSysCameraRR();
LineSysCameraRR(vector<DataSource*> in_, vector<DataSource*> out);
void update() override;
void test() override;
void outOfBounds();
void checkLineSensors();
public:
bool tookLine = false, flag = false;
int linetriggerI[4], linetriggerO[4];
private:
vector<DataSource*> in, out;
DataSource* ds;
int inV, outV, linesensOldX, linesensOldY, value, linepins[4], i;
};

6
src/sensors/data_source_bt.cpp
View File

@ -50,9 +50,9 @@ void DataSourceBT::send(){
void DataSourceBT::update(){
// if(!bt_bombarded && can_bombard) connect();
receive();
send();
if(comrade)Serial2.write(0b00000100);
// receive();
// send();
// if(comrade)Serial2.write(0b00000100);
}
void DataSourceBT :: test(){

82
src/strategy_roles/corner_kick_1.cpp
View File

@ -1,82 +0,0 @@
#include "strategy_roles/corner_kick_1.h"
#include <Bounce2.h>
CornerKick::CornerKick() : Game(){
init();
}
CornerKick::CornerKick(LineSystem *ls_, PositionSystem *ps_) : Game(ls_,ps_){
init();
}
Bounce b = Bounce(); // Instantiate a Bounce object
void CornerKick::init(){
b.attach(37, INPUT);
b.interval(25);
}
void CornerKick::realPlay(){
b.update();
if(CURRENT_DATA_READ.ballSeen){
if(b.fell()) kick_state = 0;
this->kick();
}
else drive->prepareDrive(0,0,0);
}
void CornerKick::kick(){
//DEBUG.println(kick_state);
if(kick_state == 0){
kicktimer = millis();
kick_state++;
kick_flag = false;
} else if(kick_state == 1){
drive->prepareDrive(0, 0, CURRENT_DATA_READ.ballAngleFix);
if((CURRENT_DATA_READ.ballAngle >= 350 || CURRENT_DATA_READ.ballAngle <= 10) && millis() - kicktimer > 1000) kick_state ++;
} else if(kick_state==2){
drive->prepareDrive(0, 100, 350);
if(ball->isInMouth()){
if(!kick_flag) {
kick_flag = true;
kicktimer = millis();
}else{
if(kick_flag && millis() - kicktimer > 425 ){
kick_flag = false;
kick_state++;
kicktimer = millis();
}
}
}
} else if(kick_state==3){
drive->prepareDrive(270, 60,0);
if(millis()-kicktimer > 300){
kick_state++;
}
} else if(kick_state==4){
if(((PositionSysCamera*)ps)->isAtDistanceFrom(1, -28, 2)) {
kick_state++;
kicktimer = millis();
}else (((PositionSysCamera*)ps)->setMoveSetpoints(1, -28));
} else if(kick_state == 5){
drive->prepareDrive(0,0,0);
bt->can_send = true;
bt->tosend = 'C';
if(millis() - kicktimer > 500){
kick_state++;
tone(BUZZER, 220, 250);
kicktimer = millis();
}
} else if(kick_state == 6){
bt->can_send = true;
bt->tosend = 'B';
if(millis() - kicktimer > 750){
kick_state++;
kicktimer = millis();
bt->can_send = false;
}
}else if (kick_state == 7) {
bt->can_send = false;
drive->prepareDrive(0,0,0);
}
}

155
src/strategy_roles/corner_kick_2.cpp
View File

@ -1,155 +0,0 @@
#include "strategy_roles/corner_kick_2.h"
#include "systems/position/positionsys_camera.h"
#include "vars.h"
#include "math.h"
CornerKick2::CornerKick2() : Game() {}
CornerKick2::CornerKick2(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) {}
void CornerKick2::init() {}
void CornerKick2::realPlay() {
if(bt->received == 'C') {
bt->can_send = true;
bt->tosend = 'B';
bt->can_send = false;
tone(BUZZER, 320, 250);
timer = millis();
state=1;
ball_catch_state = 0;
ball_catch_tilt = 0;
spinner_state = 0;
spinner_tilt = 0;
ball_catch_flag = false;
spinner_flag = false;
}
if(state == 1){
drive->prepareDrive(265, 50, 0);
if(CURRENT_DATA_READ.bSeen && millis() - timer > 1000) {
state ++;
timer = millis();
}
}else if(state == 2){
drive->prepareDrive(315, 50, 0);
if(millis() - timer > 400) state++;
}else if(state == 3){
catchBall();
}else if(state == 4){
spinner(-5);
}else if(state == 5){
drive->prepareDrive(0,0,0);
}
}
void CornerKick2::catchBall(){
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
else roller->speed(roller->MIN);
if(!ball->isInFront()){
ball_catch_state = 0;
ball_catch_flag = false;
}
if(ball_catch_state == 0){
drive->prepareDrive(0, MAX_VEL_HALF, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
if(ballPresence->isInMouth() && !ball_catch_flag){
ball_catch_flag = true;
ball_catch_timer = millis();
}
if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 350) {
ball_catch_state++;
ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(ball_catch_state == 1){
if(ball_catch_tilt > 180) ball_catch_tilt += 0.0075;
else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.0075;
drive->prepareDrive(0,0,ball_catch_tilt);
if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) state++;
// ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0);
// if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++;
}
}
void CornerKick2::spinner(int targetx){
if(spinner_state == 0){
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
if(ballPresence->isInMouth() && !spinner_flag){
spinner_flag = true;
spinner_timer = millis();
}
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
spinner_state=1;
spinner_flag = false;
}
}else if(spinner_state == 1){
roller->speed(roller->MAX);
int spotx = targetx;
// if(targetx >= 0) spotx = targetx-CK2_SPINNER_OVERHEAD;
// else spotx = targetx+CK2_SPINNER_OVERHEAD;
if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, -1)) {
if( !spinner_flag){
spinner_flag = true;
spinner_timer = millis();
}
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
spinner_state=2;
spinner_flag = false;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
// if(targetx >= 0) {
tilt1 = -0.0075;
tilt2 = 0.55;
limitx = 360-CK2_KICK_LIMIT_TILT1;
// }else{
// tilt1 = 0.01;
// tilt2 = -0.55;
// limitx = CK2_KICK_LIMIT_TILT1;
// }
}else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, -1);
}else if(spinner_state == 2){
roller->speed(roller->MAX);
spinner_tilt += tilt1;
drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) {
spinner_timer = millis();
spinner_state=3;
}
}else if(spinner_state == 3){
roller->speed(roller->MAX);
drive->prepareDrive(0,0,spinner_tilt);
if(millis() - spinner_timer > 150) {
spinner_state=4;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(spinner_state == 4){
drive->prepareDrive(0,0,0);
// spinner_tilt += tilt2;
// spinner_tilt = constrain(spinner_tilt, CK2_KICK_LIMIT_MIN, CK2_KICK_LIMIT_MAX);
// drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle >= CK2_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= CK2_KICK_LIMIT_MIN) {
roller->speed(roller->MIN);
state++;
}
}
}

18
src/strategy_roles/games.cpp
View File

@ -1,8 +1,7 @@
#define GAMES_CPP
#define GAMES_CPP
/* #include "sensors/linesys_2019.h" */
#include "systems/lines/linesys_camera.h"
#include "systems/lines/linesys_camera_roundrobin.h"
#include "systems/systems.h"
#include "systems/position/positionsys_zone.h"
#include "systems/position/positionsys_camera.h"
@ -12,19 +11,6 @@ void initGames(){
vector<DataSource*> lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) };
vector<DataSource*> lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) };
// striker_test = new StrikerTest(new LineSysCamera(lIn, lOut), new PositionSysCamera());
striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera());
pass_and_shoot = new PassAndShoot(new LineSysCamera(lIn, lOut), new PositionSysCamera());
precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera());
striker_roller = new StrikerRoller(new LineSysCamera(lIn, lOut), new PositionSysCamera());
tc1 = new StrikerRoller(new LineSystemEmpty(), new PositionSysCamera());
tc2 = new RoundRobin(new LineSysCameraRR(lIn, lOut), new PositionSysCamera());
tc3_1 = new CornerKick(new LineSystemEmpty(), new PositionSysCamera());
tc3_2 = new CornerKick2(new LineSystemEmpty(), new PositionSysCamera());
st_tc1 = new SpotFinder(new LineSystemEmpty(), new PositionSysCamera());
st_tc3 = new Spinner(new LineSystemEmpty(), new PositionSysCamera());
// keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
}

103
src/strategy_roles/pass_and_shoot.cpp
View File

@ -1,103 +0,0 @@
#include "behaviour_control/status_vector.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "sensors/data_source_ball.h"
#include "strategy_roles/pass_and_shoot.h"
#include "vars.h"
#include "math.h"
PassAndShoot::PassAndShoot() : Game()
{
init();
}
PassAndShoot::PassAndShoot(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
{
init();
}
void PassAndShoot::init()
{
atk_speed = 0;
atk_direction = 0;
atk_tilt = 0;
cstorc = 0;
gotta_tilt = false;
pass_counter = millis();
}
unsigned long pass_timer = 0;
boolean flag = false;
void PassAndShoot::realPlay()
{
if (CURRENT_DATA_READ.ballSeen){
if(robot_indentifier == HIGH){
if(ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90){
if(!flag){
pass_timer = millis();
flag = true;
//Show on 32u4
Serial2.write(0b00000100);
}
}
if(!flag || (millis() - pass_timer <= 650)){
striker();
}else{
((PositionSysCamera*)ps)->setMoveSetpoints(CAMERA_GOAL_MIN_X, CAMERA_GOAL_Y);
bt->tosend = 'C';
roller->speed(0);
}
}else{
if(bt->received == 'C'){
striker();
}else drive->prepareDrive(0,0,0);
}
}else drive->prepareDrive(0,0,0);
}
void PassAndShoot::striker(){
#ifdef MAX_VEL
#undef MAX_VEL
#define MAX_VEL 100
#endif
//seguo palla
int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = PAS_PLUSANG;
if(ball_deg >= 346 || ball_deg <= 16) plusang = PAS_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
if(ball_deg > 180) ball_degrees2 = ball_deg - 360; //ragiono in +180 -180
else ball_degrees2 = ball_deg;
if(ball_degrees2 > 0) dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo
else dir = ball_deg - plusang; //se sto nel negativo sottraggo
dir = (dir + 360) % 360;
drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
if(ball->isInFront()) roller->speed(ROLLER_DEFAULT_SPEED);
else roller->speed(roller->MIN);
}
int PassAndShoot::tilt() {
if (ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90 /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
else gotta_tilt = false;
if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {
atk_tilt *= 0.8;
if(atk_tilt <= 10) atk_tilt = 0;
}else{
atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
}
return atk_tilt;
}

186
src/strategy_roles/precision_shooter.cpp
View File

@ -1,186 +0,0 @@
#include "behaviour_control/status_vector.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "sensors/data_source_ball.h"
#include "strategy_roles/precision_shooter.h"
#include "vars.h"
#include "math.h"
PrecisionShooter::PrecisionShooter() : Game()
{
init();
}
PrecisionShooter::PrecisionShooter(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
{
init();
}
void PrecisionShooter::init()
{
atk_speed = 0;
atk_direction = 0;
atk_tilt = 0;
cstorc = 0;
gotta_tilt = false;
ballAngleFilter = new ComplementaryFilter(0.85);
}
void PrecisionShooter::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
this->spinner(CURRENT_DATA_READ.xAtkFix);
// this->catchBall();
else{
ps->goCenter();
ball_catch_flag = false;
spinner_flag = false;
ball_catch_state = 0;
spinner_state = 0;
}
}
/*
Spinning kick state machine
0: wait for the ball to be in mouth, calculate movement direction
1: tilt toward 180 deg
2: tilt back to 0 in the needed direction, stopping the roller whenn needed
*/
void PrecisionShooter::spinner(int targetx){
// if(!ballPresence->isInMouth()) {
// spinner_state=0;
// spinner_flag = false;
// }
if(spinner_state == 0){
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
if(ballPresence->isInMouth() && !spinner_flag){
spinner_flag = true;
spinner_timer = millis();
}
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
spinner_state=1;
spinner_flag = false;
}
}else if(spinner_state == 1){
roller->speed(roller->MAX);
int spotx = targetx;
if(targetx >= 0) spotx = targetx-PS_SPINNER_OVERHEAD;
else spotx = targetx+PS_SPINNER_OVERHEAD;
if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)) {
if( !spinner_flag){
spinner_flag = true;
spinner_timer = millis();
}
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
spinner_state=2;
spinner_flag = false;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
if(targetx >= 0) {
tilt1 = -0.15;
tilt2 = 0.55;
limitx = 360-PS_KICK_LIMIT_TILT1;
}else{
tilt1 = 0.15;
tilt2 = -0.55;
limitx = PS_KICK_LIMIT_TILT1;
}
}else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
}else if(spinner_state == 2){
roller->speed(roller->MAX);
spinner_tilt += tilt1;
drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) {
spinner_timer = millis();
spinner_state=3;
}
}else if(spinner_state == 3){
roller->speed(roller->MAX);
drive->prepareDrive(0,0,spinner_tilt);
if(millis() - spinner_timer > 150) {
spinner_state=4;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(spinner_state == 4){
drive->prepareDrive(0,0,0);
// spinner_tilt += tilt2;
// spinner_tilt = constrain(spinner_tilt, PS_KICK_LIMIT_MIN, PS_KICK_LIMIT_MAX);
// drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle >= PS_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= PS_KICK_LIMIT_MIN) roller->speed(roller->MIN);
}
}
/*
Ball catch state machine
0: go towards the ball, until it's been in robot's mouth for 250ms
1: slowly return facing to north (slowly otherwise we might lose the ball)
2: reach the spot
*/
void PrecisionShooter::catchBall(){
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
else roller->speed(roller->MIN);
if(!ball->isInFront()){
ball_catch_state = 0;
ball_catch_flag = false;
}
if(ball_catch_state == 0){
drive->prepareDrive(0, MAX_VEL_HALF, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
if(ballPresence->isInMouth() && !ball_catch_flag){
ball_catch_flag = true;
ball_catch_timer = millis();
}
if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 350) {
ball_catch_state++;
ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(ball_catch_state == 1){
int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix);
drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val);
// if(ball_catch_tilt > 180) ball_catch_tilt += 0.15;
// else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15;
// drive->prepareDrive(0,0,ball_catch_tilt);
// if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 0;
// ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0);
// if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++;
}
}
int PrecisionShooter::tilt() {
if (ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90 /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
else gotta_tilt = false;
if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {
atk_tilt *= 0.8;
if(atk_tilt <= 10) atk_tilt = 0;
}else{
atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
}
return atk_tilt;
}

324
src/strategy_roles/round_robin.cpp
View File

@ -1,324 +0,0 @@
#include "behaviour_control/status_vector.h"
#include "systems/position/positionsys_camera.h"
#include "systems/lines/linesys_camera_roundrobin.h"
#include "sensors/sensors.h"
#include "sensors/data_source_ball.h"
#include "strategy_roles/round_robin.h"
#include "vars.h"
#include "math.h"
RoundRobin::RoundRobin() : Game()
{
init();
}
RoundRobin::RoundRobin(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
{
init();
}
void RoundRobin::init()
{
atk_speed = 0;
atk_direction = 0;
atk_tilt = 0;
cstorc = 0;
gotta_tilt = false;
ballAngleFilter = new ComplementaryFilter(0.1);
}
void RoundRobin::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
this->catchBall();
// this->spinner(0);
// this->push();
else{
ps->goCenter();
ball_catch_flag = false;
spinner_flag = false;
ball_catch_state = 0;
spinner_state = 0;
}
}
/*
Spinning kick state machine
0: wait for the ball to be in mouth, calculate movement direction
1: tilt toward 180 deg
2: tilt back to 0 in the needed direction, stopping the roller whenn needed
*/
void RoundRobin::spinner(int targetx){
if(spinner_state == 0){
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
if(ballPresence->isInMouth() && !spinner_flag){
spinner_flag = true;
spinner_timer = millis();
}
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
spinner_state=1;
spinner_flag = false;
}
}else if(spinner_state == 1){
roller->speed(roller->MAX);
int spotx = targetx;
if(targetx >= 0) spotx = targetx-RR_SPINNER_OVERHEAD;
else spotx = targetx+RR_SPINNER_OVERHEAD;
// if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)) {
// if( !spinner_flag){
// spinner_flag = true;
// spinner_timer = millis();
// }
// if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
spinner_state=2;
// spinner_flag = false;
// spinner_tilt = CURRENT_DATA_READ.IMUAngle;
// }
// if(targetx >= 0) {
tilt1 = -0.15;
tilt2 = -35;
limitx = 360-RR_KICK_LIMIT_TILT1;
// }else{
// tilt1 = 0.15;
// tilt2 = -0.55;
// limitx = RR_KICK_LIMIT_TILT1;
// }
// }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
}else if(spinner_state == 2){
roller->speed(roller->MAX);
spinner_tilt += tilt1;
drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) {
spinner_timer = millis();
spinner_state=3;
}
}else if(spinner_state == 3){
roller->speed(roller->MAX);
drive->prepareDrive(0,0,spinner_tilt);
if(millis() - spinner_timer > 150) {
spinner_state=4;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(spinner_state == 4){
// drive->prepareDrive(0,0,0);
spinner_tilt += tilt2;
spinner_tilt = constrain(spinner_tilt, RR_KICK_LIMIT_MIN, RR_KICK_LIMIT_MAX);
drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle >= RR_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= RR_KICK_LIMIT_MIN) {
ball_catch_state++;
roller->speed(roller->MIN);
}
}
}
void RoundRobin::spinnerStep(){
spinner_state++;
spinner_timer = millis();
}
/*void RoundRobin::push(){
if(spinner_state == 0){
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
if(ballPresence->isInMouth() && !spinner_flag){
spinner_flag = true;
spinner_timer = millis();
}
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
spinner_state=1;
spinner_flag = false;
}
}else if(spinner_state == 1){
roller->speed(roller->MAX);
if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -6)) spinner_state=4;
else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -6);
// } else if(spinner_state == 2){
// roller->speed(roller->MAX);
// if(((PositionSysCamera*)ps)->isInTheVicinityOf(16, -4)) spinnerStep();
// else ((PositionSysCamera*)ps)->setMoveSetpoints(16, -4);
// }else if(spinner_state == 3){
// drive->prepareDrive(0,0,0);
// if(millis() - spinner_timer > 750) spinnerStep();
}else if(spinner_state == 4){
drive->prepareDrive(45, 50, 0);
if( ((LineSysCameraRR*)ls)->tookLine ) spinnerStep();
}else if(spinner_state == 5){
roller->speed(roller->MIN);
if(millis()-spinner_timer > 1500) spinnerStep();
}else if(spinner_state == 6){
drive->prepareDrive(180, 30, 0);
if(millis() - spinner_timer > 750) spinnerStep();
}else if(spinner_state == 7){
roller->speed(roller->MIN);
drive->prepareDrive(0, 0, 55);
if(millis() - spinner_timer > 1000) spinnerStep();
}else if(spinner_state == 8){
drive->prepareDrive(drive->directionAccountingForTilt(0, 55), 100, 55);
if(millis() - spinner_timer > 500) spinnerStep();
}else if(spinner_state == 9){
drive->prepareDrive(drive->directionAccountingForTilt(180, 55), 50, 55);
if(millis() - spinner_timer > 750) spinnerStep();
}else if(spinner_state == 10){
if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -8)) spinnerStep();
else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -8);
}else ball_catch_state++;
}*/
void RoundRobin::push(){
if(spinner_state == 0){
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
if(ballPresence->isInMouth() && !spinner_flag){
spinner_flag = true;
spinner_timer = millis();
}
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
spinner_state=1;
spinner_flag = false;
}
}else if(spinner_state == 1){
roller->speed(roller->MAX);
// if(flip)
// if(CURRENT_DATA_READ.yDist <= 24) drive->prepareDrive(0, 50, 0);
// if(CURRENT_DATA_READ.yDist >= 36) drive->prepareDrive(180, 50, 0);
// else{
// if(CURRENT_DATA_READ.bDist <= 24) drive->prepareDrive(0, 50, 0);
// if(CURRENT_DATA_READ.bDist >= 36) drive->prepareDrive(180, 50, 0);
// }
if(flip==0){
if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -3)) spinnerStep();
else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -3);
}else{
if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -12)) spinnerStep();
else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -12);
}
} else if(spinner_state == 2){
// roller->speed(roller->MAX);
// if(((PositionSysCamera*)ps)->isInTheVicinityOf(16, -4)) spinnerStep();
// else ((PositionSysCamera*)ps)->setMoveSetpoints(16, -4);
// }else if(spinner_state == 3){
// drive->prepareDrive(0,0,0);
// if(millis() - spinner_timer > 750) spinnerStep();
// }else if(spinner_state == 4){
// drive->prepareDrive(45, 50, 0);
drive->prepareDrive(70, 40, 0);
if( ((LineSysCameraRR*)ls)->tookLine ) {
spinner_state = 5;
spinner_timer = millis();
}
}else if(spinner_state == 5){
roller->speed(roller->MIN);
if(millis()-spinner_timer > 1500) spinnerStep();
}else if(spinner_state == 6){
drive->prepareDrive(180, 30, 0);
if(millis() - spinner_timer > 650) spinnerStep();
}else if(spinner_state == 7){
roller->speed(roller->MIN);
drive->prepareDrive(0, 0, 55);
if(millis() - spinner_timer > 1000) spinnerStep();
}else if(spinner_state == 8){
drive->prepareDrive(drive->directionAccountingForTilt(0, 90), 100, 90);
if(millis() - spinner_timer > 400) spinnerStep();
}else if(spinner_state == 9){
drive->prepareDrive(drive->directionAccountingForTilt(180, 90), 50, 90);
if(millis() - spinner_timer > 750) spinnerStep();
}else if(spinner_state == 10){
if(flip==0){
if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -4)) spinnerStep();
else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -4);
}else{
if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -13)) spinnerStep();
else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -13);
}
}else ball_catch_state++;
}
/*
Ball catch state machine
0: go towards the ball, until it's been in robot's mouth for 250ms
1: slowly return facing to north (slowly otherwise we might lose the ball)
2: reach the spot
*/
void RoundRobin::catchBall(){
// if(!ball->isInFront() && ball_catch_state != 3){
// ball_catch_state = 0;
// ball_catch_flag = false;
// ball_catch_tilt = 0;
// }
if(ball_catch_state == 0){
((LineSysCameraRR*)ls)->flag = true;
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
else roller->speed(roller->MIN);
drive->prepareDrive(0, ROUND_ROBIN_VEL, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
if(ballPresence->isInMouth() && !ball_catch_flag){
ball_catch_flag = true;
ball_catch_timer = millis();
}
if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 50) {
ball_catch_state++;
ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(ball_catch_state == 1){
if(ball_catch_tilt > 180 && ball_catch_tilt < 360) ball_catch_tilt += 0.15;
else if(ball_catch_tilt <= 180 && ball_catch_tilt > 0) ball_catch_tilt -= 0.15;
drive->prepareDrive(0,0,ball_catch_tilt);
if(CURRENT_DATA_READ.IMUAngle >= 350 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 2;
}else if(ball_catch_state == 2){
spinner_tilt = 0;
spinner_flag = false;
spinner_timer = 0;
spinner_state = 1;
ball_catch_state = 3;
((LineSysCameraRR*)ls)->flag = false;
}else if(ball_catch_state == 3){
// this->spinner(25);
this->push();
}else if(ball_catch_state == 4){
drive->prepareDrive(270, 50, 0);
if(((LineSysCameraRR*)ls)->tookLine && CURRENT_DATA_READ.posx <= -10) ball_catch_state = 5;
}else if(ball_catch_state == 5){
if( ((LineSysCameraRR*)ls)->linetriggerI[1] || ((LineSysCameraRR*)ls)->linetriggerO[1] > 0) drive->prepareDrive(90, 50, 0); //with 2 and 4 you go right or left accordingly
else if( ((LineSysCameraRR*)ls)->linetriggerI[3] || ((LineSysCameraRR*)ls)->linetriggerO[3] > 0 ) drive->prepareDrive(270, 50, 0);
else drive->prepareDrive(0, 30, 0);
if(CURRENT_DATA_READ.posy > 7) {
ball_catch_state=0;
flip = 1-flip;
CURRENT_DATA_WRITE.IMUOffset = 180 * flip;
}
}else if(ball_catch_state==6) drive->prepareDrive(0,0,0);
}

33
src/strategy_roles/spot_finder.cpp
View File

@ -1,33 +0,0 @@
#include "strategy_roles/spot_finder.h"
#include "systems/position/positionsys_camera.h"
#include "vars.h"
#include "math.h"
SpotFinder::SpotFinder() : Game() {}
SpotFinder::SpotFinder(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) {}
void SpotFinder::init() {}
void SpotFinder::realPlay() {
if(millis() - t > 150){
int zone = 6;
if(CURRENT_DATA_READ.posx >= -X_COORD && CURRENT_DATA_READ.posx <= X_COORD && CURRENT_DATA_READ.posy >= -Y_COORD && CURRENT_DATA_READ.posy <= Y_COORD) BALL_32U4.write(0b000000110); //center
else if(CURRENT_DATA_READ.posx <= -X_COORD && CURRENT_DATA_READ.posy <= -Y_COORD) BALL_32U4.write(0b00000100); //bottom left
else if(CURRENT_DATA_READ.posx <= -X_COORD && CURRENT_DATA_READ.posy >= Y_COORD) BALL_32U4.write(2); //top left
else if(CURRENT_DATA_READ.posx >= X_COORD && CURRENT_DATA_READ.posy <= -Y_COORD) BALL_32U4.write(0b00000001); //bottom right
else if(CURRENT_DATA_READ.posx >= X_COORD && CURRENT_DATA_READ.posy >= Y_COORD) BALL_32U4.write(5); //top right
else BALL_32U4.write(0);
// if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(-X_COORD,-Y_COORD) ) BALL_32U4.write(1); //bottom left
// else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(-X_COORD,Y_COORD) ) BALL_32U4.write(2); //top left
// else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(0,0) ) BALL_32U4.write(3); //center
// else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(X_COORD,-Y_COORD) ) BALL_32U4.write(4); //bottom right
// else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(X_COORD,Y_COORD) ) BALL_32U4.write(5); //top right
t = millis();
}
drive->stopAll();
}

238
src/strategy_roles/striker_roller.cpp
View File

@ -1,238 +0,0 @@
#include "behaviour_control/status_vector.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "sensors/data_source_ball.h"
#include "strategy_roles/striker_roller.h"
#include "vars.h"
#include "math.h"
StrikerRoller::StrikerRoller() : Game()
{
init();
}
StrikerRoller::StrikerRoller(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
{
init();
}
void StrikerRoller::init()
{
atk_speed = 0;
atk_direction = 0;
atk_tilt = 0;
cstorc = 0;
gotta_tilt = false;
ballAngleFilter = new ComplementaryFilter(0);
}
void StrikerRoller::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
this->catchBall();
else{
ps->goCenter();
ball_catch_flag = false;
spinner_flag = false;
ball_catch_state = 0;
spinner_state = 0;
}
}
/*
Spinning kick state machine
0: wait for the ball to be in mouth, calculate movement direction
1: tilt toward 180 deg
2: tilt back to 0 in the needed direction, stopping the roller whenn needed
*/
void StrikerRoller::spinner(int targetx){
// if(!ballPresence->isInMouth()) {
// spinner_state=0;
// spinner_flag = false;
// }
if(spinner_state == 0){
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
if(ballPresence->isInMouth() && !spinner_flag){
spinner_flag = true;
spinner_timer = millis();
}
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 50) {
spinner_state=1;
spinner_flag = false;
}
}else if(spinner_state == 1){
roller->speed(roller->MAX);
spinner_state=2;
spinner_flag = false;
// decide with direction to rotate
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
if(CURRENT_DATA_READ.angleAtk >= 0) {
tilt1 = -0.25;
tilt2 = 5;
limitx = 360-STRL_KICK_LIMIT_TILT1;
}else{
tilt1 = 0.25;
tilt2 = -5;
limitx = STRL_KICK_LIMIT_TILT1;
}
// if(CURRENT_DATA_READ.posx >= -5 && CURRENT_DATA_READ.posx <= 5){
// if(CURRENT_DATA_READ.IMUAngle >= 180) {
// tilt1 = -0.25;
// tilt2 = 15;
// limitx = 360-STRL_KICK_LIMIT_TILT1;
// }else{
// tilt1 = 0.25;
// tilt2 = -15;
// limitx = STRL_KICK_LIMIT_TILT1;
// }
// }else if(CURRENT_DATA_READ.posx < -5){
// tilt1 = 0.25;
// tilt2 = -5;
// limitx = STRL_KICK_LIMIT_TILT1;
// }else if(CURRENT_DATA_READ.posx > 5){
// tilt1 = -0.25;
// tilt2 = 5;
// limitx = 360-STRL_KICK_LIMIT_TILT1;
// }
// if(CURRENT_DATA_READ.IMUAngle < limitx) tilt1 *= -1; //we need to invert the rotatiion if we are on the opposite side
}else if(spinner_state == 2){
roller->speed(roller->MAX);
spinner_tilt = spinner_tilt + tilt1;
// This ensures no strange rotation happens
if(spinner_tilt <= -360) spinner_tilt+=360;
if(spinner_tilt >= 360) spinner_tilt -= 360;
drive->prepareDrive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) {
spinner_state=3;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(spinner_state == 3){
roller->speed(roller->MAX);
// stay there a little bit
drive->prepareDrive(0,0,spinner_tilt);
if(millis() - spinner_timer > 150) {
spinner_state=4;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(spinner_state == 4){
// fast return which gives momentum to the ball
drive->prepareDrive(0,0,spinner_tilt);
spinner_tilt += tilt2;
spinner_tilt = constrain(spinner_tilt, 5, 355);
// turn roller off and kick
if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) {
roller->speed(roller->MIN);
if(!spinner_end_flag){
spinner_end_time = millis();
spinner_end_flag = true;
}
}
// if(ballPresence->isInMouth()) {
// spinner_state = 1;
// spinner_flag = false;
// }
}
}
/*
Ball catch state machine
0: go towards the ball, until it's been in robot's mouth for 250ms
1: slowly return facing to north (slowly otherwise we might lose the ball)
2: reach the spot
*/
void StrikerRoller::catchBall(){
if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
else roller->speed(roller->MIN);
// if(!ball->isInFront() && millis() - spinner_end_time > 500 && spinner_end_flag){
// ball_catch_state = 0;
// ball_catch_flag = false;
// spinner_end_flag = false;
// }
if(!ball->isInFront()){
ball_catch_state = 0;
ball_catch_flag = false;
}
if(ball_catch_state == 0){
// tilt torwards the ball until it's in the mouth
drive->prepareDrive(0, MAX_VEL_HALF, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
if(ballPresence->isInMouth() && !ball_catch_flag){
ball_catch_flag = true;
ball_catch_timer = millis();
}
if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 50) {
ball_catch_state++;
}
}else if(ball_catch_state == 1){
// go towards the goal without resetting tilt
// this makes it so that the robot sometimes walks backwards, hiding the ball from any opponent
// a spinning kick is the only way to free the ball here
int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix);
drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val);
if(CURRENT_DATA_READ.distAtk <= 26){
// ball_catch_state++;
// spinner_state = 0;
// spinner_flag = false;
// spinner_tilt = CURRENT_DATA_READ.IMUAngle;
drive->prepareDrive(0,0,0);
if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) roller->speed(roller->MIN);
}
}else if(ball_catch_state == 2){
// Spinny kick
// spinner(0);
if(CURRENT_DATA_READ.angleAtkFix >= 0) {
tilt2 = 3;
}else{
tilt2 = -3;
}
drive->prepareDrive(0,0,spinner_tilt);
spinner_tilt += tilt2;
spinner_tilt = constrain(spinner_tilt, 5, 355);
// turn roller off and kick
if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) {
roller->speed(roller->MIN);
if(!spinner_end_flag){
spinner_end_time = millis();
spinner_end_flag = true;
}
}
}
}
int StrikerRoller::tilt() {
if (ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90 /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
else gotta_tilt = false;
if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {
atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
}
return atk_tilt;
}

48
src/strategy_roles/the_spinner.cpp
View File

@ -1,48 +0,0 @@
#include "strategy_roles/the_spinner.h"
#include "systems/position/positionsys_camera.h"
#include "vars.h"
#include "math.h"
Spinner::Spinner() : Game() {}
Spinner::Spinner(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) {}
void Spinner::init() {}
void Spinner::realPlay() {
if(current_spot_i >= spots.size()) return;
current_spot = spots[current_spot_i];
if(doingCircle){
circle();
return;
}
if (((PositionSysCamera*)ps)->isInTheVicinityOf(current_spot.x, current_spot.y)) {
if(current_spot.x == 0 && current_spot.y == 0) {
current_spot_i++;
return;
}
doingCircle = true;
firstCircle = true;
step = 0;
}
else (((PositionSysCamera*)ps)->setMoveSetpoints(current_spot.x, current_spot.y));
}
void Spinner::circle(){
if(millis() - t < 1000){
if(step == 0) drive->prepareDrive(45, 50, 0);
else if(step == 1) drive->prepareDrive(315, 50, 0);
else if(step == 2) drive->prepareDrive(225, 50, 0);
else if(step == 3) {
if(firstCircle){
drive->prepareDrive(135, 80, 0);
firstCircle = false;
}else doingCircle = false;
}
}else {
step++;
t = millis();
}
}

117
src/system/lines/linesys_camera_recovery.cpp
View File

@ -1,117 +0,0 @@
#include "systems/lines/linesys_camera_recovery.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "strategy_roles/games.h"
#include "behaviour_control/status_vector.h"
LineSysCameraRecovery::LineSysCameraRecovery(){}
LineSysCameraRecovery::LineSysCameraRecovery(vector<DataSource*> in_, vector<DataSource*> out_){
this->in = in_;
this->out = out_;
fboundsX = false;
fboundsY = false;
slow = false;
linesensOldX = 0;
linesensOldY = 0;
tookLine = false;
for(int i = 0; i < 4; i++){
linetriggerI[i] = 0;
linetriggerO[i] = 0;
}
exitTimer = 0;
linesens = 0;
}
void LineSysCameraRecovery ::update() {
inV = 0;
outV = 0;
tookLine = false;
for(DataSource* d : in) d->readSensor();
for(DataSource* d : out) d->readSensor();
for(auto it = in.begin(); it != in.end(); it++){
i = it - in.begin();
ds = *it;
linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM;
}
for(auto it = out.begin(); it != out.end(); it++){
i = it - out.begin();
ds = *it;
linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM;
}
for(int i = 0; i < 4; i++){
inV = inV | (linetriggerI[i] << i);
outV = outV | (linetriggerO[i] << i);
}
if (inV > 0 || outV > 0) {
if(millis() - exitTimer > EXIT_TIME) {
fboundsX = true;
fboundsY = true;
}
exitTimer = millis();
}
linesens |= inV | outV;
outOfBounds();
}
void LineSysCameraRecovery::outOfBounds(){
// digitalWriteFast(BUZZER, LOW);
if(fboundsX == true) {
if(linesens & 0x02) linesensOldX = 2;
else if(linesens & 0x08) linesensOldX = 8;
if(linesensOldX != 0) fboundsX = false;
}
if(fboundsY == true) {
if(linesens & 0x01) linesensOldY = 1;
else if(linesens & 0x04) linesensOldY = 4;
if(linesensOldY != 0) fboundsY = false;
}
if (millis() - exitTimer < EXIT_TIME){
CURRENT_DATA_READ.game->ps->goCenter();
tookLine = true;
tone(BUZZER, 220.00, 250);
}else{
// drive->canUnlock = true;
linesens = 0;
linesensOldY = 0;
linesensOldX = 0;
}
}
void LineSysCameraRecovery::test(){
update();
DEBUG.print("In: ");
for(DataSource* d : in){
d->update();
DEBUG.print(d->getValue());
DEBUG.print(" | ");
}
DEBUG.print(" |---| ");
DEBUG.print("Out: ");
for(DataSource* d : out){
d->update();
DEBUG.print(d->getValue());
DEBUG.print(" | ");
}
DEBUG.println();
for(int i = 0; i < 4; i++){
DEBUG.print(linetriggerI[i]);
DEBUG.print(" | ");
DEBUG.println(linetriggerO[i]);
}
DEBUG.println(inV);
DEBUG.println(outV);
DEBUG.println();
}

77
src/system/lines/linesys_camera_roundrobin.cpp
View File

@ -1,77 +0,0 @@
#include "systems/lines/linesys_camera_roundrobin.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "strategy_roles/games.h"
#include "behaviour_control/status_vector.h"
LineSysCameraRR::LineSysCameraRR(){}
LineSysCameraRR::LineSysCameraRR(vector<DataSource*> in_, vector<DataSource*> out_){
this->in = in_;
this->out = out_;
linesensOldX = 0;
linesensOldY = 0;
tookLine = false;
for(int i = 0; i < 4; i++){
linetriggerI[i] = 0;
linetriggerO[i] = 0;
}
}
void LineSysCameraRR ::update() {
inV = 0;
outV = 0;
tookLine = false;
for(DataSource* d : in) d->readSensor();
for(DataSource* d : out) d->readSensor();
for(auto it = in.begin(); it != in.end(); it++){
i = it - in.begin();
ds = *it;
linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM;
}
for(auto it = out.begin(); it != out.end(); it++){
i = it - out.begin();
ds = *it;
linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM;
}
for(int i = 0; i < 4; i++){
inV = inV | (linetriggerI[i] << i);
outV = outV | (linetriggerO[i] << i);
}
tookLine = inV > 0 || outV > 0;
// if(flag && tookLine) ((PositionSysCamera*)CURRENT_DATA_READ.posSystem)->setMoveSetpoints(0, -8);
}
void LineSysCameraRR::test(){
update();
DEBUG.print("In: ");
for(DataSource* d : in){
d->update();
DEBUG.print(d->getValue());
DEBUG.print(" | ");
}
DEBUG.print(" |---| ");
DEBUG.print("Out: ");
for(DataSource* d : out){
d->update();
DEBUG.print(d->getValue());
DEBUG.print(" | ");
}
DEBUG.println();
for(int i = 0; i < 4; i++){
DEBUG.print(linetriggerI[i]);
DEBUG.print(" | ");
DEBUG.println(linetriggerO[i]);
}
DEBUG.println(inV);
DEBUG.println(outV);
DEBUG.println();
}

1
src/test_menu.cpp
View File

@ -92,7 +92,6 @@ void TestMenu::testMenu()
DEBUG.print("Left switch (goalOrientation): ");
DEBUG.println(camera->goalOrientation);
DEBUG.print("Robot Identifier: ");
DEBUG.println(robot_indentifier);
delay(50);
break;
case 'p':