EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixed a major issues with linesystems

code_midgen
EmaMaker 2020-01-15 13:55:51 +01:00
parent 8fffdfd83b
commit 023d42c868
11 changed files with 65 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/linesys_2019.h
View File

@ -27,8 +27,6 @@ class LineSys2019 : public LineSystem{
void update() override; void update() override;
void test() override; void test() override;
void outOfBounds(); void outOfBounds();
void handleIntern();
void handleExtern();
private: private:
vector<DataSource*> in, out; vector<DataSource*> in, out;
@ -36,6 +34,7 @@ class LineSys2019 : public LineSystem{
bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow; bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow;
int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], i; int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], i;
elapsedMillis exitTimer; elapsedMillis exitTimer;
byte outDir, outVel, linesens; int outDir, outVel;
byte linesens;
unsigned long unlockTime; unsigned long unlockTime;
}; };

2
include/sensors.h
View File

@ -23,8 +23,6 @@
void initSensors(); void initSensors();
void updateSensors(); void updateSensors();
s_extr vector<DataSource*> lIn;
s_extr vector<DataSource*> lOut;
s_extr vector<DataSource*> dUs; s_extr vector<DataSource*> dUs;
s_extr DataSourceCtrl* usCtrl; s_extr DataSourceCtrl* usCtrl;

18
src/data_source_camera.cpp
View File

@ -60,7 +60,7 @@ void DataSourceCamera :: readSensor(){
negateB = false; negateB = false;
datavalid ++; datavalid ++;
} }
} }
} }
void DataSourceCamera :: postProcess(){ void DataSourceCamera :: postProcess(){
@ -92,7 +92,7 @@ void DataSourceCamera :: postProcess(){
} }
} }
int DataSourceCamera :: getValueAtk(bool fixed){ int DataSourceCamera::getValueAtk(bool fixed){
//attacco gialla //attacco gialla
if(digitalRead(SWITCH_DX) == HIGH){ if(digitalRead(SWITCH_DX) == HIGH){
if(fixed) return fixCamIMU(valY); if(fixed) return fixCamIMU(valY);
@ -100,12 +100,12 @@ int DataSourceCamera :: getValueAtk(bool fixed){
} }
//attacco blu //attacco blu
if(digitalRead(SWITCH_DX) == LOW){ if(digitalRead(SWITCH_DX) == LOW){
if(fixed) return fixCamIMU(valB); if(fixed) return fixCamIMU(valB);
return valB; return valB;
} }
} }
int DataSourceCamera :: getValueDef(bool fixed){ int DataSourceCamera::getValueDef(bool fixed){
//difendo gialla //difendo gialla
if(digitalRead(SWITCH_DX) == HIGH){ if(digitalRead(SWITCH_DX) == HIGH){
if(fixed) return fixCamIMU(valY); if(fixed) return fixCamIMU(valY);
@ -113,12 +113,12 @@ int DataSourceCamera :: getValueDef(bool fixed){
} }
//difendo blu //difendo blu
if(digitalRead(SWITCH_DX) == LOW){ if(digitalRead(SWITCH_DX) == LOW){
if(fixed) return fixCamIMU(valB); if(fixed) return fixCamIMU(valB);
return valB; return valB;
} }
} }
void DataSourceCamera :: test(){ void DataSourceCamera::test(){
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu
@ -133,7 +133,7 @@ void DataSourceCamera :: test(){
delay(100); delay(100);
} }
int DataSourceCamera :: fixCamIMU(int d){ int DataSourceCamera::fixCamIMU(int d){
if(compass->getValue() > 0 && compass->getValue() < 180) imuOff = compass->getValue(); if(compass->getValue() > 0 && compass->getValue() < 180) imuOff = compass->getValue();
else if (compass->getValue() <= 360 && compass->getValue() >= 180) imuOff = compass->getValue() - 360; else if (compass->getValue() <= 360 && compass->getValue() >= 180) imuOff = compass->getValue() - 360;
imuOff = constrain(imuOff*0.8, -30, 30); imuOff = constrain(imuOff*0.8, -30, 30);

2
src/drivecontroller.cpp
View File

@ -34,7 +34,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
integral = 0; integral = 0;
} }
void DriveController::prepareDrive(int dir, int speed, int tilt){ void DriveController::prepareDrive(int dir=0, int speed=0, int tilt=0){
pDir = dir; pDir = dir;
pSpeed = speed; pSpeed = speed;
pTilt = tilt; pTilt = tilt;

6
src/game.cpp
View File

@ -8,7 +8,9 @@ Game::Game(LineSystem* ls_, PositionSystem* ps_) {
void Game::play(bool condition){ void Game::play(bool condition){
ps->update(); ps->update();
if(condition) realPlay(); if(condition) {
ls->update(); realPlay();
ls->update();
}
} }

7
src/games.cpp
View File

@ -5,6 +5,9 @@
#include "positionsys_zone.h" #include "positionsys_zone.h"
void initGames(){ void initGames(){
goalie = new Goalie(new LineSys2019(), new PositionSysZone()); vector<DataSource*> lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) };
keeper = new Keeper(new LineSys2019(), new PositionSysZone()); vector<DataSource*> lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) };
goalie = new Goalie(new LineSys2019(lIn, lOut), new PositionSysZone());
keeper = new Keeper(new LineSys2019(lOut, lOut), new PositionSysZone());
} }

82
src/goalie.cpp
View File

@ -13,15 +13,15 @@ Goalie::Goalie(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) {
void Goalie::init(){ void Goalie::init(){
atk_speed = 0; atk_speed = 0;
atk_direction = 0; atk_direction = 0;
cstorc = 0;
} }
void Goalie::realPlay(){ void Goalie::realPlay(){
goalie(); if(ball->ballSeen) this->goalie();
else drive->prepareDrive(0,0,0);
} }
void Goalie::goalie() { void Goalie::goalie() {
compass->readSensor();
if(ball->angle >= 350 || ball->angle <= 10) { if(ball->angle >= 350 || ball->angle <= 10) {
if(ball->distance > 190) atk_direction = 0; if(ball->distance > 190) atk_direction = 0;
else atk_direction = ball->angle; else atk_direction = ball->angle;
@ -29,61 +29,35 @@ void Goalie::goalie() {
} }
if(ball->angle >= 90 && ball->angle <= 270) { if(ball->angle >= 90 && ball->angle <= 270) {
ballBack(); this->ballBack();
atk_speed = GOALIE_ATKSPD_BAK; atk_speed = GOALIE_ATKSPD_BAK;
} }
if(digitalRead(SWITCH_DX) == 1) { if(ball->angle > 10 && ball->angle < 30) {
if(ball->angle > 10 && ball->angle < 30) { atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1;
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1; atk_speed = GOALIE_ATKSPD_LAT;
atk_speed = GOALIE_ATKSPD_LAT; }
} if(ball->angle >= 30 && ball->angle < 45) {
if(ball->angle >= 30 && ball->angle < 45) { atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2;
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2; atk_speed = GOALIE_ATKSPD_LAT;
atk_speed = GOALIE_ATKSPD_LAT; }
} if(ball->angle >= 45 && ball->angle < 90) {
if(ball->angle >= 45 && ball->angle < 90) { atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3;
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3; atk_speed = GOALIE_ATKSPD_LAT;
atk_speed = GOALIE_ATKSPD_LAT; }
} if(ball->angle > 270 && ball->angle <= 315) {
if(ball->angle > 270 && ball->angle <= 315) { atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR;
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR; atk_speed = GOALIE_ATKSPD_LAT;
atk_speed = GOALIE_ATKSPD_LAT; }
} if(ball->angle > 315 && ball->angle <= 330) {
if(ball->angle > 315 && ball->angle <= 330) { atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR;
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR; atk_speed = GOALIE_ATKSPD_LAT;
atk_speed = GOALIE_ATKSPD_LAT; }
} if(ball->angle > 330 && ball->angle < 350) {
if(ball->angle > 330 && ball->angle < 350) { atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR;
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR; atk_speed = GOALIE_ATKSPD_LAT;
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;
}
} }
this->storcimentoPorta();
if((ball->angle >= 330 || ball->angle <= 30) && ball->distance > 190) { //storcimento if((ball->angle >= 330 || ball->angle <= 30) && ball->distance > 190) { //storcimento
atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito
drive->prepareDrive(atk_direction, atk_speed, cstorc); drive->prepareDrive(atk_direction, atk_speed, cstorc);

5
src/keeper.cpp
View File

@ -24,6 +24,11 @@ void Keeper::init(){
} }
void Keeper::realPlay() { void Keeper::realPlay() {
if(ball->ballSeen) keeper();
else drive->prepareDrive(0,0,0);
}
void Keeper::keeper() {
if(ball->distance > KEEPER_ATTACK_DISTANCE){ if(ball->distance > KEEPER_ATTACK_DISTANCE){
// Ball is quite near // Ball is quite near

17
src/linesys_2019.cpp
View File

@ -7,8 +7,6 @@ LineSys2019::LineSys2019(vector<DataSource*> in_, vector<DataSource*> out_){
this->in = in_; this->in = in_;
this->out = out_; this->out = out_;
fboundsOX = false;
fboundsOY = false;
fboundsX = false; fboundsX = false;
fboundsY = false; fboundsY = false;
slow = false; slow = false;
@ -24,11 +22,13 @@ LineSys2019::LineSys2019(vector<DataSource*> in_, vector<DataSource*> out_){
} }
exitTimer = 0; exitTimer = 0;
linesens = 0;
} }
void LineSys2019::update(){ void LineSys2019::update(){
inV = 0; inV = 0;
outV = 0; outV = 0;
tookLine = false;
for(DataSource* d : in) d->readSensor(); for(DataSource* d : in) d->readSensor();
for(DataSource* d : out) d->readSensor(); for(DataSource* d : out) d->readSensor();
@ -49,11 +49,8 @@ void LineSys2019::update(){
outV = outV | (linetriggerO[i] << i); outV = outV | (linetriggerO[i] << i);
} }
linesens |= inV | outV;
if ((linesensOldX > 0) || (linesensOldX > 0)) { if ((inV > 0) || (outV > 0)) {
fboundsOX = true;
fboundsOY = true;
if(exitTimer > EXTIME) { if(exitTimer > EXTIME) {
fboundsX = true; fboundsX = true;
fboundsY = true; fboundsY = true;
@ -61,6 +58,7 @@ void LineSys2019::update(){
exitTimer = 0; exitTimer = 0;
} }
linesens |= inV | outV;
outOfBounds(); outOfBounds();
} }
@ -79,6 +77,7 @@ void LineSys2019::outOfBounds(){
if (exitTimer <= EXTIME){ if (exitTimer <= EXTIME){
//fase di rientro //fase di rientro
digitalWrite(LED_R, HIGH);
if(linesens == 15) linesens = linesensOldY | linesensOldX; //ZOZZATA MAXIMA if(linesens == 15) linesens = linesensOldY | linesensOldX; //ZOZZATA MAXIMA
unlockTime = millis(); unlockTime = millis();
@ -103,12 +102,8 @@ void LineSys2019::outOfBounds(){
else if(linesensOldY == 1) outDir = 180; else if(linesensOldY == 1) outDir = 180;
} }
outVel = LINES_EXIT_SPD; drive->prepareDrive(outDir, LINES_EXIT_SPD, 0);
drive->prepareDrive(outDir, outVel, 0);
tookLine = true; tookLine = true;
// keeper_backToGoalPost = true;
// keeper_tookTimer = true;
}else{ }else{
//fine rientro //fine rientro
if(linesens == 1) drive->vyp = 1; if(linesens == 1) drive->vyp = 1;

5
src/main.cpp
View File

@ -20,11 +20,6 @@ void loop() {
goalie->play(role==1); goalie->play(role==1);
keeper->play(role==0); keeper->play(role==0);
//Check lines before final movement
linesCtrl->update();
// Last thing to do: movement // Last thing to do: movement
drive->drivePrepared(); drive->drivePrepared();
/* compass->test(); */
} }

8
src/sensors.cpp
View File

@ -5,18 +5,18 @@ void initSensors(){
pinMode(SWITCH_DX, INPUT); pinMode(SWITCH_DX, INPUT);
pinMode(SWITCH_SX, INPUT); pinMode(SWITCH_SX, INPUT);
pinMode(LED_R, OUTPUT);
pinMode(LED_Y, OUTPUT);
pinMode(LED_G, OUTPUT);
dUs = { new DataSourceUS(&Wire1, int(112)), new DataSourceUS(&Wire1, int(113)), dUs = { new DataSourceUS(&Wire1, int(112)), new DataSourceUS(&Wire1, int(113)),
new DataSourceUS(&Wire1, int(114)), new DataSourceUS(&Wire1, int(115)) }; new DataSourceUS(&Wire1, int(114)), new DataSourceUS(&Wire1, int(115)) };
lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) };
lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) };
drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315)); drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
compass = new DataSourceBNO055(); compass = new DataSourceBNO055();
ball = new DataSourceBall(&Serial4, 57600); ball = new DataSourceBall(&Serial4, 57600);
camera = new DataSourceCamera(&Serial2, 19200); camera = new DataSourceCamera(&Serial2, 19200);
usCtrl = new DataSourceCtrl(dUs); usCtrl = new DataSourceCtrl(dUs);
linesCtrl = new LineSys2019(lIn, lOut);
bt = new DataSourceBT(&Serial3, 115200); bt = new DataSourceBT(&Serial3, 115200);
} }