reimproted the zone position system
EmaMaker
parent
4a1ba46f5b
commit
6a0d660aab
|
|
@ -1,23 +0,0 @@
|
|||
#pragma once
|
||||
#include "data_source.h"
|
||||
#include "vars.h"
|
||||
#include <Arduino.h>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
class DataSourceController {
|
||||
|
||||
public:
|
||||
DataSourceController();
|
||||
DataSourceController(vector<DataSource*>);
|
||||
|
||||
public:
|
||||
void update();
|
||||
void test();
|
||||
void postProcess();
|
||||
void readSensor();
|
||||
void getValue();
|
||||
|
||||
vector<DataSource*> ds;
|
||||
|
||||
};
|
||||
|
|
@ -17,6 +17,7 @@ class DataSourceCtrl {
|
|||
virtual void test();
|
||||
virtual void postProcess();
|
||||
virtual void read();
|
||||
int getValue(int i);
|
||||
|
||||
vector<DataSource*> ds;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,10 +0,0 @@
|
|||
#include "vars.h"
|
||||
#include "ds_ctrl.h"
|
||||
#include "ds_ctrl_lines.h"
|
||||
|
||||
class DSCtrlPosition : public DataSourceController {
|
||||
|
||||
public:
|
||||
DSCtrlPosition();
|
||||
|
||||
};
|
||||
|
|
@ -2,11 +2,16 @@
|
|||
|
||||
#include "vars.h"
|
||||
#include "sensors.h"
|
||||
#include "systems.h"
|
||||
|
||||
class Game {
|
||||
public:
|
||||
Game();
|
||||
Game(LineSystem* ls, PositionSystem* ps);
|
||||
void play(bool condition=true);
|
||||
private:
|
||||
virtual void realPlay() = 0;
|
||||
virtual void init() = 0;
|
||||
LineSystem* ls;
|
||||
PositionSystem* ps;
|
||||
};
|
||||
|
|
@ -20,12 +20,17 @@ class Goalie : public Game{
|
|||
|
||||
public:
|
||||
Goalie();
|
||||
Goalie(LineSystem* ls, PositionSystem* ps);
|
||||
|
||||
private:
|
||||
void realPlay() override;
|
||||
void init() override;
|
||||
void goalie();
|
||||
void ballBack();
|
||||
void storcimentoPorta();
|
||||
|
||||
int atk_speed, atk_direction;
|
||||
float cstorc;
|
||||
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -2,22 +2,35 @@
|
|||
|
||||
#include "game.h"
|
||||
|
||||
//KEEPER
|
||||
#define KEEPER_ATTACK_DISTANCE 200
|
||||
#define KEEPER_ATTACK_DISTANCE 190
|
||||
#define KEEPER_ALONE_ATTACK_TIME 5000 //in millis
|
||||
#define KEEPER_COMRADE_ATTACK_TIME 3000//in millis
|
||||
#define KEEPER_BASE_VEL 320
|
||||
#define KEEPER_VEL_MULT 1.4
|
||||
#define KEEPER_BALL_BACK_ANGLE 30
|
||||
#define KEEPER_ANGLE_SX 265
|
||||
#define KEEPER_ANGLE_DX 85
|
||||
|
||||
//POSITION
|
||||
#define CENTERGOALPOST_VEL1 220
|
||||
#define CENTERGOALPOST_VEL2 220
|
||||
#define CENTERGOALPOST_VEL3 220
|
||||
#define CENTERGOALPOST_CAM_MIN -40
|
||||
#define CENTERGOALPOST_CAM_MAX 40
|
||||
#define CENTERGOALPOST_US_MAX 60
|
||||
#define CENTERGOALPOST_US_CRITIC 25
|
||||
|
||||
class Keeper : public Game{
|
||||
|
||||
public:
|
||||
Keeper();
|
||||
Keeper(LineSystem*, PositionSystem*);
|
||||
|
||||
private:
|
||||
void realPlay() override;
|
||||
void init() override;
|
||||
void keeper();
|
||||
void centerGoalPostCamera(bool);
|
||||
|
||||
int defSpeed, defDir;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,11 +0,0 @@
|
|||
#pragma once
|
||||
#include "ds_ctrl_lines.h"
|
||||
#include "games.h"
|
||||
#include "game.h"
|
||||
|
||||
class LineSystem : public DSCtrlLines{
|
||||
public:
|
||||
LineSystem(DSCtrlLines* method, Game* strategy);
|
||||
virtual void update() = 0;
|
||||
virtual void test() = 0;
|
||||
};
|
||||
|
|
@ -1,6 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "ds_ctrl.h"
|
||||
#include "data_source.h"
|
||||
#include "systems.h"
|
||||
#include "vars.h"
|
||||
#include <Arduino.h>
|
||||
|
||||
|
|
@ -16,19 +17,17 @@
|
|||
#define LINE_THRESH 90
|
||||
#define EXTIME 100
|
||||
|
||||
class DSCtrlLines : public DataSourceCtrl{
|
||||
class LineSys2019 : public LineSystem{
|
||||
|
||||
public:
|
||||
DSCtrlLines();
|
||||
DSCtrlLines(vector<DataSource*> in_, vector<DataSource*> out);
|
||||
LineSys2019();
|
||||
LineSys2019(vector<DataSource*> in_, vector<DataSource*> out);
|
||||
|
||||
void read() override;
|
||||
void postProcess() override;
|
||||
void update() override;
|
||||
void test() override;
|
||||
void outOfBounds();
|
||||
void handleIntern();
|
||||
void handleExtern();
|
||||
int getValue();
|
||||
void test() override;
|
||||
|
||||
private:
|
||||
vector<DataSource*> in, out;
|
||||
|
|
@ -0,0 +1,120 @@
|
|||
#pragma once
|
||||
|
||||
#include "systems.h"
|
||||
|
||||
//POSITION
|
||||
#define CENTERGOALPOST_VEL1 220
|
||||
#define CENTERGOALPOST_VEL2 220
|
||||
#define CENTERGOALPOST_VEL3 220
|
||||
#define CENTERGOALPOST_CAM_MIN -40
|
||||
#define CENTERGOALPOST_CAM_MAX 40
|
||||
#define CENTERGOALPOST_US_MAX 60
|
||||
#define CENTERGOALPOST_US_CRITIC 25
|
||||
#define GOCENTER_VEL 280
|
||||
|
||||
#define ZONE_MAX_VALUE 150
|
||||
#define ZONE_LOOP_DECREASE_VALUE 4
|
||||
#define ZONE_US_UNKNOWN_INCREASE_VALUE 4
|
||||
#define ZONE_US_INDEX_INCREASE_VALUE 9
|
||||
#define ZONE_CAM_INCREASE_VALUE 3
|
||||
#define ZONE_CAM_CENTER_RANGE 25
|
||||
#define ZONE_LINES_INCREASE_VALUE 100
|
||||
#define ZONE_LINES_ERROR_VALUE 30
|
||||
|
||||
// You can modify this if you need
|
||||
// LIMITI DEL CAMPO
|
||||
#define Lx_min 115 // valore minimo accettabile di larghezza
|
||||
#define Lx_max 195 // valore massimo accettabile di larghezza (larghezza campo)
|
||||
#define LyF_min 190 // valore minimo accettabile di lunghezza sulle fasce
|
||||
#define LyF_max 270 // valore massimo accettabile di lunghezza sulle fasce (lunghezza campo)
|
||||
#define LyP_min 139 // valore minimo accettabile di lunghezza tra le porte
|
||||
#define LyP_max 250 // valore massimo accettabile di lunghezza tra le porte// con misura x OK con us_dx o us_sx < DxF sto nelle fasce 30 + 30 - 1/2 robot
|
||||
|
||||
#define DyF 91 // con misura y OK e robot al CENTER (tra le porte) con us_fx o us_px < DyP sto a NORTH o a SOUTH era - 22
|
||||
#define DyP 69
|
||||
#define DxF_Atk 48 // per attaccante, fascia centrale allargata
|
||||
#define DxF_Def 48 // per portiere, fascia centrale ristretta quEASTa roba viene fatta dentro WhereAmI
|
||||
#define robot 21 // diametro del robot
|
||||
|
||||
|
||||
// ZONE DEL CAMPO
|
||||
// codici utilizzabili per una matice 3x3
|
||||
#define EAST 2
|
||||
#define WEST 0
|
||||
#define CENTER 1
|
||||
#define NORTH 0
|
||||
#define SOUTH 2
|
||||
|
||||
#define NORTH_WEST 1
|
||||
#define NORTH_CENTER 2
|
||||
#define NORTH_EAST 3
|
||||
#define CENTER_WEST 4
|
||||
#define CENTER_CENTER 5 // codici zona nuovi
|
||||
#define CENTER_EAST 6
|
||||
#define SOUTH_WEST 7
|
||||
#define SOUTH_CENTER 8
|
||||
#define SOUTH_EAST 9
|
||||
|
||||
class PositionSysZone : public PositionSystem{
|
||||
public:
|
||||
PositionSysZone();
|
||||
void update() override;
|
||||
void test() override;
|
||||
|
||||
//returns the zone calculated
|
||||
int getValue();
|
||||
|
||||
private:
|
||||
int zone[3][3];
|
||||
|
||||
int zoneIndex;
|
||||
int p;
|
||||
int camA;
|
||||
int camD;
|
||||
unsigned long ao;
|
||||
|
||||
int old_status_x; // posizione precedente nel campo vale EST, OVEST o CENTRO o 255 >USI FUTURI<
|
||||
int old_status_y; // posizione precedente nel campo vale SUD, NORD o CENTRO o 255 >USI FUTURI<
|
||||
bool good_field_x; // vedo tutta la larghezza del campo si/no
|
||||
bool good_field_y; // vedo tutta la lunghezza del campo si/no
|
||||
int status_x; // posizione nel campo vale EST, OVEST o CENTRO o 255
|
||||
int status_y; // posizione nel campo vale SUD, NORD o CENTRO o 255
|
||||
int guessed_x, guessed_y;
|
||||
bool calcPhyZoneCam;
|
||||
int DxF; // con misura y OK e robot a EST o A OVEST con us_fx o us_px < DyF sto a NORD o a SUD era - 10
|
||||
bool goal_zone;
|
||||
|
||||
//calculations
|
||||
void increaseIndex(int, int, int);
|
||||
void increaseCol(int, int);
|
||||
void increaseRow(int, int);
|
||||
void increaseAll(int);
|
||||
void decreaseIndex(int, int, int);
|
||||
void decreaseCol(int, int);
|
||||
void decreaseRow(int, int);
|
||||
void decreaseAll(int);
|
||||
void increaseRowWithLimit(int, int);
|
||||
void increaseColWithLimit(int, int);
|
||||
void decreaseRowWithLimit(int, int);
|
||||
void decreaseColWithLimit(int, int);
|
||||
|
||||
//reading
|
||||
void readPhyZone();
|
||||
void phyZoneDirection();
|
||||
void phyZoneCam();
|
||||
void phyZoneUS();
|
||||
void phyZoneLines();
|
||||
|
||||
//testing
|
||||
void testPhyZone();
|
||||
void testLogicZone();
|
||||
|
||||
//movement
|
||||
void goCenter();
|
||||
void goGoalPost();
|
||||
void centerGoalPost();
|
||||
void centerGoalPostCamera(bool);
|
||||
void AAANGOLO();
|
||||
int diff(int, int);
|
||||
|
||||
};
|
||||
|
|
@ -12,11 +12,13 @@
|
|||
#include "data_source_ball.h"
|
||||
#include "data_source_camera.h"
|
||||
#include "data_source_us.h"
|
||||
#include "ds_ctrl_lines.h"
|
||||
#include "motor.h"
|
||||
#include "ds_ctrl.h"
|
||||
#include "drivecontroller.h"
|
||||
#include "data_source_bt.h"
|
||||
#include "systems.h"
|
||||
#include "linesys_2019.h"
|
||||
#include "positionsys_zone.h"
|
||||
|
||||
void initSensors();
|
||||
void updateSensors();
|
||||
|
|
@ -26,7 +28,7 @@ s_extr vector<DataSource*> lOut;
|
|||
s_extr vector<DataSource*> dUs;
|
||||
|
||||
s_extr DataSourceCtrl* usCtrl;
|
||||
s_extr DSCtrlLines* linesCtrl;
|
||||
s_extr LineSys2019* linesCtrl;
|
||||
|
||||
s_extr DataSourceBNO055* compass;
|
||||
s_extr DataSourceBall* ball;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,13 @@
|
|||
#pragma once
|
||||
|
||||
class LineSystem{
|
||||
public:
|
||||
virtual void update() = 0;
|
||||
virtual void test() = 0;
|
||||
};
|
||||
|
||||
class PositionSystem{
|
||||
public:
|
||||
virtual void update() = 0;
|
||||
virtual void test() = 0;
|
||||
};
|
||||
|
|
@ -23,6 +23,10 @@ void DataSourceCtrl::postProcess(){
|
|||
}
|
||||
}
|
||||
|
||||
int DataSourceCtrl::getValue(int i){
|
||||
return this->ds[i]->getValue();
|
||||
}
|
||||
|
||||
void DataSourceCtrl::test(){
|
||||
DEBUG.println("========================================");
|
||||
for(DataSource* d : ds){
|
||||
|
|
|
|||
|
|
@ -1,7 +1,14 @@
|
|||
#include "game.h"
|
||||
|
||||
Game::Game() {}
|
||||
Game::Game(LineSystem* ls_, PositionSystem* ps_) {
|
||||
this->ls = ls_;
|
||||
this->ps = ps_;
|
||||
}
|
||||
|
||||
void Game::play(bool condition){
|
||||
ps->update();
|
||||
if(condition) realPlay();
|
||||
ls->update();
|
||||
|
||||
}
|
||||
|
|
@ -1,8 +1,10 @@
|
|||
#define GAMES_CPP
|
||||
|
||||
#include "games.h"
|
||||
#include "linesys_2019.h"
|
||||
#include "positionsys_zone.h"
|
||||
|
||||
void initGames(){
|
||||
goalie = new Goalie();
|
||||
keeper = new Keeper();
|
||||
goalie = new Goalie(new LineSys2019(), new PositionSysZone());
|
||||
keeper = new Keeper(new LineSys2019(), new PositionSysZone());
|
||||
}
|
||||
|
|
@ -3,8 +3,17 @@
|
|||
#include "vars.h"
|
||||
|
||||
Goalie::Goalie() : Game() {
|
||||
init();
|
||||
}
|
||||
|
||||
Goalie::Goalie(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) {
|
||||
init();
|
||||
}
|
||||
|
||||
void Goalie::init(){
|
||||
atk_speed = 0;
|
||||
atk_direction = 0;
|
||||
|
||||
}
|
||||
|
||||
void Goalie::realPlay(){
|
||||
|
|
|
|||
|
|
@ -4,6 +4,12 @@
|
|||
#include <Arduino.h>
|
||||
|
||||
Keeper::Keeper() : Game() {
|
||||
init();
|
||||
}
|
||||
|
||||
Keeper::Keeper(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_){}
|
||||
|
||||
void Keeper::init(){
|
||||
defSpeed = 0;
|
||||
defDir = 0;
|
||||
angle = 0;
|
||||
|
|
@ -29,18 +35,37 @@ void Keeper::realPlay() {
|
|||
|
||||
}else{
|
||||
|
||||
angle = (90 + ball->angle) * M_PI / 180;
|
||||
angle = (KEEPER_ANGLE_DX + ball->angle) * M_PI / 180;
|
||||
angleX = abs(cos(angle));
|
||||
|
||||
if(ball->angle >= 0 && ball->angle <= 90 && camera->getValueDef(true) < 30) drive->prepareDrive(90, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT);
|
||||
else if(ball->angle >= 270 && ball->angle <= 360 && camera->getValueDef(true) > -30) drive->prepareDrive(270, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT);
|
||||
else if(ball->angle < 270 && ball->angle > 90){
|
||||
if(ball->angle >= 0 && ball->angle <= KEEPER_ANGLE_DX && camera->getValueDef(true) < 30) drive->prepareDrive(KEEPER_ANGLE_DX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT);
|
||||
else if(ball->angle >= KEEPER_ANGLE_SX && ball->angle <= 360 && camera->getValueDef(true) > -30) drive->prepareDrive(KEEPER_ANGLE_SX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT);
|
||||
else if(ball->angle < KEEPER_ANGLE_SX && ball->angle > KEEPER_ANGLE_DX){
|
||||
int ball_degrees2 = ball->angle > 180? ball->angle-360:ball->angle;
|
||||
int dir = ball_degrees2 > 0 ? ball->angle + KEEPER_BALL_BACK_ANGLE : ball->angle - KEEPER_BALL_BACK_ANGLE;
|
||||
dir = dir < 0? dir + 360: dir;
|
||||
|
||||
drive->prepareDrive(dir, KEEPER_BASE_VEL);
|
||||
}
|
||||
//centerGoalPostCamera(false);
|
||||
}
|
||||
}
|
||||
|
||||
void Keeper::centerGoalPostCamera(bool checkBack){
|
||||
if (camera->getValueDef(true) > CENTERGOALPOST_CAM_MAX) {
|
||||
drive->prepareDrive(KEEPER_ANGLE_SX, CENTERGOALPOST_VEL1);
|
||||
} else if (camera->getValueDef(true) < CENTERGOALPOST_CAM_MIN) {
|
||||
drive->prepareDrive(KEEPER_ANGLE_DX, CENTERGOALPOST_VEL1);
|
||||
}else if(camera->getValueDef(true) > CENTERGOALPOST_CAM_MIN && camera->getValueDef(true) < CENTERGOALPOST_CAM_MAX){
|
||||
if(!ball->ballSeen) drive->prepareDrive(0, 0, 0);
|
||||
if(checkBack){
|
||||
if(usCtrl->getValue(2) > CENTERGOALPOST_US_MAX){
|
||||
drive->prepareDrive(180, CENTERGOALPOST_VEL2);
|
||||
} else{
|
||||
if(usCtrl->getValue(2) < CENTERGOALPOST_US_CRITIC) drive->prepareDrive(0, CENTERGOALPOST_VEL3);
|
||||
|
||||
keeper_backToGoalPost = false;
|
||||
keeper_tookTimer = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,9 +1,9 @@
|
|||
#include "ds_ctrl_lines.h"
|
||||
#include "linesys_2019.h"
|
||||
#include "sensors.h"
|
||||
|
||||
using namespace std;
|
||||
DSCtrlLines::DSCtrlLines() {}
|
||||
DSCtrlLines::DSCtrlLines(vector<DataSource*> in_, vector<DataSource*> out_){
|
||||
LineSys2019::LineSys2019() {}
|
||||
LineSys2019::LineSys2019(vector<DataSource*> in_, vector<DataSource*> out_){
|
||||
this->in = in_;
|
||||
this->out = out_;
|
||||
|
||||
|
|
@ -24,7 +24,7 @@ DSCtrlLines::DSCtrlLines(vector<DataSource*> in_, vector<DataSource*> out_){
|
|||
exitTimer = 0;
|
||||
}
|
||||
|
||||
void DSCtrlLines::read(){
|
||||
void LineSys2019::update(){
|
||||
inV = 0;
|
||||
outV = 0;
|
||||
for(DataSource* d : in) d->readSensor();
|
||||
|
|
@ -47,9 +47,7 @@ void DSCtrlLines::read(){
|
|||
}
|
||||
|
||||
inV = inV | outV;
|
||||
}
|
||||
|
||||
void DSCtrlLines::postProcess(){
|
||||
if ((inV > 0) || (outV > 0)) {
|
||||
fboundsOX = true;
|
||||
fboundsOY = true;
|
||||
|
|
@ -63,12 +61,12 @@ void DSCtrlLines::postProcess(){
|
|||
outOfBounds();
|
||||
}
|
||||
|
||||
void DSCtrlLines::outOfBounds(){
|
||||
void LineSys2019::outOfBounds(){
|
||||
handleExtern();
|
||||
handleIntern();
|
||||
}
|
||||
|
||||
void DSCtrlLines::handleIntern(){
|
||||
void LineSys2019::handleIntern(){
|
||||
|
||||
if(fboundsX == true) {
|
||||
if(inV & 0x02) inVOldX = 2;
|
||||
|
|
@ -162,14 +160,14 @@ void DSCtrlLines::handleIntern(){
|
|||
else slow = false;
|
||||
}
|
||||
|
||||
void DSCtrlLines::handleExtern(){
|
||||
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 DSCtrlLines::test(){
|
||||
void LineSys2019::test(){
|
||||
update();
|
||||
DEBUG.print("In: ");
|
||||
for(DataSource* d : in){
|
||||
|
|
@ -0,0 +1,428 @@
|
|||
#include "positionsys_zone.h"
|
||||
#include "vars.h"
|
||||
#include "sensors.h"
|
||||
|
||||
PositionSysZone::PositionSysZone(){
|
||||
for(int i = 0; i < 3; i++){
|
||||
for(int j = 0; j < 3; j++){
|
||||
zone[i][j] = 0;
|
||||
}
|
||||
}
|
||||
zoneIndex = 0;
|
||||
camA = 0;
|
||||
camD = 0;
|
||||
ao = 0;
|
||||
p = 4;
|
||||
}
|
||||
|
||||
void PositionSysZone::PositionSysZone::update() {
|
||||
decreaseAll(ZONE_LOOP_DECREASE_VALUE);
|
||||
|
||||
readPhyZone();
|
||||
//calculates guessed_x and guessed_y and zoneIndex
|
||||
//zoneIndex is just 2D to 1D of the guessed x and y
|
||||
//(y_position * width + x_position)
|
||||
|
||||
int top = 0;
|
||||
for(int i = 0; i < 3; i++){
|
||||
for(int j = 0; j < 3; j++){
|
||||
if(zone[i][j] > top){
|
||||
guessed_x = i;
|
||||
guessed_y = j;
|
||||
top = zone[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
zoneIndex = guessed_y * 3 + guessed_x;
|
||||
}
|
||||
|
||||
int PositionSysZone::getValue(){
|
||||
return zoneIndex;
|
||||
}
|
||||
|
||||
void PositionSysZone::PositionSysZone::test() {
|
||||
//outpus the matrix
|
||||
if (millis() - ao >= 500) {
|
||||
DEBUG.println("------");
|
||||
for (int i = 0; i < 4; i++) {
|
||||
DEBUG.print("US: ");
|
||||
usCtrl->test();
|
||||
DEBUG.print(" | ");
|
||||
}
|
||||
DEBUG.println();
|
||||
testPhyZone();
|
||||
testLogicZone();
|
||||
compass->test();
|
||||
|
||||
// if (comrade){
|
||||
// DEBUG.print("FriendZone: ");
|
||||
// DEBUG.println(friendZone);
|
||||
// }
|
||||
DEBUG.println("------");
|
||||
ao = millis();
|
||||
}
|
||||
}
|
||||
|
||||
int zone[3][3];
|
||||
|
||||
void PositionSysZone::increaseIndex(int i, int j, int ammount){
|
||||
if(i < 3 && j < 3){
|
||||
zone[i][j] += ammount;
|
||||
zone[i][j] = constrain(zone[i][j], 0, ZONE_MAX_VALUE);
|
||||
}
|
||||
}
|
||||
|
||||
void PositionSysZone::decreaseIndex(int i, int j, int ammount){
|
||||
increaseIndex(i, j, -ammount);
|
||||
}
|
||||
|
||||
void PositionSysZone::increaseRow(int i, int ammount){
|
||||
if(i < 3){
|
||||
for(int a = 0; a < 3; a++){
|
||||
increaseIndex(a, i, ammount);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PositionSysZone::decreaseRow(int i, int ammount){
|
||||
increaseRow(i, -ammount);
|
||||
}
|
||||
|
||||
void PositionSysZone::increaseCol(int i, int ammount){
|
||||
if(i < 3){
|
||||
for(int a = 0; a < 3; a++){
|
||||
increaseIndex(i, a, ammount);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PositionSysZone::decreaseCol(int i, int ammount){
|
||||
increaseCol(i, -ammount);
|
||||
}
|
||||
|
||||
void PositionSysZone::increaseColWithLimit(int i, int ammount){
|
||||
if(zone[i][0] + ammount < ZONE_MAX_VALUE && zone[i][1] + ammount < ZONE_MAX_VALUE && zone[i][2] + ammount < ZONE_MAX_VALUE){
|
||||
increaseCol(i, ammount);
|
||||
}
|
||||
}
|
||||
|
||||
void PositionSysZone::increaseRowWithLimit(int i, int ammount){
|
||||
if(zone[0][1] + ammount < ZONE_MAX_VALUE && zone[1][i] + ammount < ZONE_MAX_VALUE && zone[2][i] + ammount < ZONE_MAX_VALUE){
|
||||
increaseRow(i, ammount);
|
||||
}
|
||||
}
|
||||
|
||||
void PositionSysZone::decreaseColWithLimit(int i, int ammount){
|
||||
if(zone[i][0] - ammount >= 0 && zone[i][1] - ammount >= 0 && zone[i][2] - ammount >= 0){
|
||||
decreaseCol(i, ammount);
|
||||
}
|
||||
}
|
||||
|
||||
void PositionSysZone::decreaseRowWithLimit(int i, int ammount){
|
||||
if(zone[0][i] - ammount >= 0 && zone[1][i] - ammount >= 0 && zone[2][i] - ammount >= 0){
|
||||
decreaseRow(i, ammount);
|
||||
}
|
||||
}
|
||||
|
||||
void PositionSysZone::increaseAll(int val){
|
||||
//decrease all
|
||||
for(int i = 0; i < 3; i++){
|
||||
for(int j = 0; j < 3; j++){
|
||||
increaseIndex(i, j, val);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PositionSysZone::decreaseAll(int val){
|
||||
increaseAll(-val);
|
||||
}
|
||||
|
||||
|
||||
void PositionSysZone::readPhyZone(){
|
||||
phyZoneUS();
|
||||
phyZoneCam();
|
||||
phyZoneLines();
|
||||
// phyZoneDirection();
|
||||
}
|
||||
|
||||
//old WhereAmI. Renamed to be coerent. Now also adds to the logic zone
|
||||
void PositionSysZone::phyZoneUS(){
|
||||
// decide la posizione in orizzontale e verticale
|
||||
// Aggiorna i flag : good_field_x good_field_y non utilizzata da altre
|
||||
// routines
|
||||
// goal_zone non utilizzata da altre
|
||||
// routines
|
||||
// Aggiorna le variabili:
|
||||
// status_x (con valori CENTER EAST WEST 255 = non lo so)
|
||||
// status_y (con valori CENTER NORTH SOUTH 255 = non lo so)
|
||||
|
||||
int Lx_mis; // larghezza totale stimata dalle misure
|
||||
int Ly_mis; // lunghezza totale stimata dalle misure
|
||||
int Ly_min; // Limite inferiore con cui confrontare la misura y
|
||||
int Ly_max; // Limite inferiore con cui confrontare la misura y
|
||||
int Dy; // Limite per decidere NORTH SOUTH in funzione della posizione
|
||||
// orizzontale
|
||||
|
||||
old_status_x = status_x;
|
||||
old_status_y = status_y;
|
||||
good_field_x = false; // non é buona x
|
||||
good_field_y = false; // non é buona y
|
||||
goal_zone = false; // non sono davanti alla porta avversaria
|
||||
|
||||
if (role == HIGH)
|
||||
DxF = DxF_Atk;
|
||||
else
|
||||
DxF = DxF_Def;
|
||||
|
||||
Lx_mis = usCtrl->getValue(1) + usCtrl->getValue(3) + robot; // larghezza totale stimata
|
||||
Ly_mis = usCtrl->getValue(0) + usCtrl->getValue(2) + robot; // lunghezza totale stimata
|
||||
|
||||
// controllo orizzontale
|
||||
if ((Lx_mis < Lx_max) && (Lx_mis > Lx_min) && (usCtrl->getValue(1) > 25) && (usCtrl->getValue(3) > 25)) {
|
||||
// se la misura orizzontale é accettabile
|
||||
good_field_x = true;
|
||||
status_x = CENTER;
|
||||
if (usCtrl->getValue(1) < DxF) // robot é vicino al bordo dEASTro
|
||||
status_x = EAST;
|
||||
if (usCtrl->getValue(3) < DxF) // robot é vicino al bordo sinistro
|
||||
status_x = WEST;
|
||||
|
||||
if (status_x == CENTER) {
|
||||
// imposto limiti di controllo lunghezza verticale tra le porte
|
||||
Ly_min = LyP_min;
|
||||
Ly_max = LyP_max;
|
||||
Dy = DyP;
|
||||
} else {
|
||||
// imposto limiti di controllo lunghezza verticale in fascia
|
||||
Ly_min = LyF_min;
|
||||
Ly_max = LyF_max;
|
||||
Dy = DyF;
|
||||
}
|
||||
} else {
|
||||
// la misura non é pulita per la presenza di un ostacolo
|
||||
if ((usCtrl->getValue(1) >= (DxF + 10)) || (usCtrl->getValue(3) >= (DxF + 10))) {
|
||||
// se ho abbastanza spazio a dEASTra o a sinistra
|
||||
// devo stare per forza al cento
|
||||
status_x = CENTER;
|
||||
// imposto limiti di controllo lunghezza verticale tra le porte
|
||||
Ly_min = LyP_min;
|
||||
Ly_max = LyP_max;
|
||||
Dy = DyP;
|
||||
} else {
|
||||
status_x = 255;
|
||||
// non so la coordinata x
|
||||
// imposto i limiti di controllo verticale in base alla posizione
|
||||
// orizzontale precedente
|
||||
if (old_status_x == CENTER) {
|
||||
// controlla la posizione precedente per decidere limiti di controllo y
|
||||
// imposto limiti di controllo lunghezza verticale tra le porte
|
||||
Ly_min = LyP_min;
|
||||
Ly_max = LyP_max;
|
||||
Dy = DyP;
|
||||
} else {
|
||||
// imposto limiti di controllo lunghezza verticale in fascia anche per x
|
||||
// incognita
|
||||
Ly_min = LyF_min;
|
||||
Ly_max = LyF_max;
|
||||
Dy = DyF;
|
||||
}
|
||||
}
|
||||
}
|
||||
// controllo verticale
|
||||
if ((Ly_mis < Ly_max) && (Ly_mis > Ly_min)) {
|
||||
// se la misura verticale é accettabile
|
||||
good_field_y = true;
|
||||
status_y = CENTER;
|
||||
if (usCtrl->getValue(0) < Dy) {
|
||||
status_y = NORTH; // robot é vicino alla porta avversaria
|
||||
if (Dy == DyP)
|
||||
goal_zone = true; // davanti alla porta in zona goal
|
||||
}
|
||||
if (usCtrl->getValue(2) < Dy)
|
||||
status_y = SOUTH; // robot é vicino alla propria porta
|
||||
} else {
|
||||
// la misura non é pulita per la presenza di un ostacolo
|
||||
status_y = 255; // non so la coordinata y
|
||||
if (usCtrl->getValue(0) >= (Dy + 0))
|
||||
status_y = CENTER; // ma se ho abbastanza spazio dietro o avanti
|
||||
if (usCtrl->getValue(2) >= (Dy + 0))
|
||||
status_y = CENTER; // e'probabile che stia al CENTER
|
||||
}
|
||||
|
||||
//now operates on the matrix
|
||||
if (status_x == 255 && status_y != 255) {
|
||||
increaseRow(status_y, ZONE_US_UNKNOWN_INCREASE_VALUE);
|
||||
} else if (status_x != 255 && status_y == 255) {
|
||||
increaseCol(status_x, ZONE_US_UNKNOWN_INCREASE_VALUE);
|
||||
} else {
|
||||
increaseIndex(status_x, status_y, ZONE_US_INDEX_INCREASE_VALUE);
|
||||
}
|
||||
}
|
||||
|
||||
void PositionSysZone::phyZoneCam(){
|
||||
|
||||
//IMU-fixed attack angle
|
||||
camA = camera->getValueAtk(true);
|
||||
//IMU-fixed defence angle
|
||||
camD = camera->getValueDef(true);
|
||||
|
||||
//Negative angle means that the robot is positioned to the right of the goalpost
|
||||
//Positive angle means that the robot is positioned to the left of the goalpost
|
||||
|
||||
p = 4;
|
||||
|
||||
if(abs(diff(camA, camD)) <= ZONE_CAM_CENTER_RANGE){
|
||||
//at center row, you can consider both camA and camD
|
||||
p = 1;
|
||||
}else if(camA > camD){
|
||||
p = 0;
|
||||
}else if(camD > camA){
|
||||
p = 2;
|
||||
}
|
||||
|
||||
increaseColWithLimit(p, ZONE_CAM_INCREASE_VALUE);
|
||||
|
||||
calcPhyZoneCam = false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void PositionSysZone::phyZoneLines(){
|
||||
// //ZONE_LINES_ERROR_VALUE is a random error code not used in line exit direction calculations
|
||||
// if(lineSensByteBak != ZONE_LINES_ERROR_VALUE){
|
||||
// switch(lineSensByteBak) {
|
||||
// case 1: //NORTH
|
||||
// increaseRow(0, ZONE_LINES_INCREASE_VALUE);
|
||||
// decreaseRow(1, ZONE_LINES_INCREASE_VALUE);
|
||||
// decreaseRow(2, ZONE_LINES_INCREASE_VALUE);
|
||||
// break;
|
||||
|
||||
// case 2: //EAST
|
||||
// decreaseCol(0, ZONE_LINES_INCREASE_VALUE);
|
||||
// decreaseCol(1, ZONE_LINES_INCREASE_VALUE);
|
||||
// increaseCol(2, ZONE_LINES_INCREASE_VALUE);
|
||||
// break;
|
||||
|
||||
// case 4: //SOUTH
|
||||
// decreaseRow(0, ZONE_LINES_INCREASE_VALUE);
|
||||
// decreaseRow(1, ZONE_LINES_INCREASE_VALUE);
|
||||
// decreaseRow(2, ZONE_LINES_INCREASE_VALUE);
|
||||
// break;
|
||||
|
||||
// case 8: //WEST
|
||||
// increaseCol(0, ZONE_LINES_INCREASE_VALUE);
|
||||
// decreaseCol(1, ZONE_LINES_INCREASE_VALUE);
|
||||
// decreaseCol(2, ZONE_LINES_INCREASE_VALUE);
|
||||
// break;
|
||||
|
||||
// case 3:
|
||||
// decreaseAll(ZONE_LINES_INCREASE_VALUE);
|
||||
// increaseIndex(0, 2, 2*ZONE_LINES_INCREASE_VALUE);
|
||||
// break;
|
||||
|
||||
// case 6:
|
||||
// decreaseAll(ZONE_LINES_INCREASE_VALUE);
|
||||
// increaseIndex(2, 2, 2*ZONE_LINES_INCREASE_VALUE);
|
||||
// break;
|
||||
|
||||
// case 9:
|
||||
// decreaseAll(ZONE_LINES_INCREASE_VALUE);
|
||||
// increaseIndex(0, 0, 2*ZONE_LINES_INCREASE_VALUE);
|
||||
// break;
|
||||
|
||||
// default:
|
||||
// break;
|
||||
// }
|
||||
// //Last thing to do, sets the var to an error code, so next time it will be called will be because of the outOfBounds function being called
|
||||
// lineSensByteBak = ZONE_LINES_ERROR_VALUE;
|
||||
// }
|
||||
}
|
||||
|
||||
|
||||
void PositionSysZone::testPhyZone(){
|
||||
updateSensors();
|
||||
|
||||
readPhyZone();
|
||||
DEBUG.print("Measured US location:\t");
|
||||
DEBUG.print(status_x);
|
||||
DEBUG.print(" | ");
|
||||
DEBUG.println(status_y);
|
||||
DEBUG.print("Measured Cam Column (4 is error):\t");
|
||||
DEBUG.println(p);
|
||||
}
|
||||
|
||||
void PositionSysZone::testLogicZone(){
|
||||
update();
|
||||
DEBUG.println("-----------------");
|
||||
|
||||
for (int j = 0; j < 3; j++) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
DEBUG.print(zone[i][j]);
|
||||
DEBUG.print(" | ");
|
||||
}
|
||||
DEBUG.println();
|
||||
}
|
||||
DEBUG.println("-----------------");
|
||||
DEBUG.print("Guessed location:\t");
|
||||
DEBUG.print(guessed_x);
|
||||
DEBUG.print(" | ");
|
||||
DEBUG.println(guessed_y);
|
||||
DEBUG.print("Zone Index: ");
|
||||
DEBUG.println(zoneIndex);
|
||||
}
|
||||
|
||||
|
||||
void PositionSysZone::goCenter() {
|
||||
if (zoneIndex == 8)
|
||||
drive->prepareDrive(330, GOCENTER_VEL);
|
||||
if (zoneIndex == 7)
|
||||
drive->prepareDrive(0, GOCENTER_VEL);
|
||||
if (zoneIndex == 6)
|
||||
drive->prepareDrive(45, GOCENTER_VEL);
|
||||
if (zoneIndex == 5)
|
||||
drive->prepareDrive(270, GOCENTER_VEL);
|
||||
if (zoneIndex == 4)
|
||||
drive->prepareDrive(0, 0);
|
||||
if (zoneIndex == 3)
|
||||
drive->prepareDrive(90, GOCENTER_VEL);
|
||||
if (zoneIndex == 2)
|
||||
drive->prepareDrive(255, GOCENTER_VEL);
|
||||
if (zoneIndex == 1)
|
||||
drive->prepareDrive(180, GOCENTER_VEL);
|
||||
if (zoneIndex == 0)
|
||||
drive->prepareDrive(135, GOCENTER_VEL);
|
||||
}
|
||||
|
||||
|
||||
// int vel = 160;
|
||||
// int usDist = 70;
|
||||
// void PositionSysZone::centerGoalPost() {
|
||||
// x = 1;
|
||||
// y = 1;
|
||||
// int vel = 255;
|
||||
// if ((zoneIndex >= 0 && zoneIndex <= 2) || zoneIndex == 4) {
|
||||
// drive->prepareDrive(180, vel);
|
||||
// } else if (zoneIndex == 3 || zoneIndex == 6) {
|
||||
// drive->prepareDrive(90, vel);
|
||||
// } else if (zoneIndex == 5 || zoneIndex == 8) {
|
||||
// drive->prepareDrive(270, vel);
|
||||
// } else {
|
||||
// stop_menamoli = false;
|
||||
// if (usCtrl->getValue(2) >= 25)
|
||||
// drive->prepareDrive(180, vel);
|
||||
// else
|
||||
// drive->prepareDrive(0, 0);
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
void PositionSysZone::AAANGOLO() {
|
||||
if((usCtrl->getValue(2) <= 45) && ((usCtrl->getValue(1) <= 50) || (usCtrl->getValue(3) <= 50))) drive->prepareDrive(0, 350, 0);
|
||||
}
|
||||
|
||||
int PositionSysZone::diff(int a, int b){
|
||||
int diffB = abs(min(a, b) - max(a, b));
|
||||
int diffB1 = 360-diffB;
|
||||
int diff = min(diffB, diffB1);
|
||||
return diff;
|
||||
}
|
||||
|
|
@ -16,13 +16,9 @@ void initSensors(){
|
|||
ball = new DataSourceBall(&Serial4, 57600);
|
||||
camera = new DataSourceCamera(&Serial2, 19200);
|
||||
usCtrl = new DataSourceCtrl(dUs);
|
||||
linesCtrl = new DSCtrlLines(lIn, lOut);
|
||||
linesCtrl = new LineSys2019(lIn, lOut);
|
||||
bt = new DataSourceBT(&Serial3, 115200);
|
||||
|
||||
/*game = new Game();
|
||||
goalie = new Goalie();
|
||||
keeper = new Keeper();*/
|
||||
}
|
||||
}
|
||||
|
||||
void updateSensors(){
|
||||
role = digitalRead(SWITCH_DX);
|
||||
|
|
|
|||
Loading…
Reference in New Issue