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

2
include/sensors.h
View File

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

18
src/data_source_camera.cpp
View File

@ -60,7 +60,7 @@ void DataSourceCamera :: readSensor(){
negateB = false;
datavalid ++;
}
}
}
}
void DataSourceCamera :: postProcess(){
@ -92,7 +92,7 @@ void DataSourceCamera :: postProcess(){
}
}
int DataSourceCamera :: getValueAtk(bool fixed){
int DataSourceCamera::getValueAtk(bool fixed){
//attacco gialla
if(digitalRead(SWITCH_DX) == HIGH){
if(fixed) return fixCamIMU(valY);
@ -100,12 +100,12 @@ int DataSourceCamera :: getValueAtk(bool fixed){
}
//attacco blu
if(digitalRead(SWITCH_DX) == LOW){
if(fixed) return fixCamIMU(valB);
return valB;
if(fixed) return fixCamIMU(valB);
return valB;
}
}
int DataSourceCamera :: getValueDef(bool fixed){
int DataSourceCamera::getValueDef(bool fixed){
//difendo gialla
if(digitalRead(SWITCH_DX) == HIGH){
if(fixed) return fixCamIMU(valY);
@ -113,12 +113,12 @@ int DataSourceCamera :: getValueDef(bool fixed){
}
//difendo blu
if(digitalRead(SWITCH_DX) == LOW){
if(fixed) return fixCamIMU(valB);
return valB;
if(fixed) return fixCamIMU(valB);
return valB;
}
}
void DataSourceCamera :: test(){
void DataSourceCamera::test(){
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu
@ -133,7 +133,7 @@ void DataSourceCamera :: test(){
delay(100);
}
int DataSourceCamera :: fixCamIMU(int d){
int DataSourceCamera::fixCamIMU(int d){
if(compass->getValue() > 0 && compass->getValue() < 180) imuOff = compass->getValue();
else if (compass->getValue() <= 360 && compass->getValue() >= 180) imuOff = compass->getValue() - 360;
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;
}
void DriveController::prepareDrive(int dir, int speed, int tilt){
void DriveController::prepareDrive(int dir=0, int speed=0, int tilt=0){
pDir = dir;
pSpeed = speed;
pTilt = tilt;

6
src/game.cpp
View File

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

7
src/games.cpp
View File

@ -5,6 +5,9 @@
#include "positionsys_zone.h"
void initGames(){
goalie = new Goalie(new LineSys2019(), new PositionSysZone());
keeper = new Keeper(new LineSys2019(), new PositionSysZone());
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) };
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(){
atk_speed = 0;
atk_direction = 0;
cstorc = 0;
}
void Goalie::realPlay(){
goalie();
if(ball->ballSeen) this->goalie();
else drive->prepareDrive(0,0,0);
}
void Goalie::goalie() {
compass->readSensor();
if(ball->angle >= 350 || ball->angle <= 10) {
if(ball->distance > 190) atk_direction = 0;
else atk_direction = ball->angle;
@ -29,61 +29,35 @@ void Goalie::goalie() {
}
if(ball->angle >= 90 && ball->angle <= 270) {
ballBack();
this->ballBack();
atk_speed = GOALIE_ATKSPD_BAK;
}
if(digitalRead(SWITCH_DX) == 1) {
if(ball->angle > 10 && ball->angle < 30) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle >= 30 && ball->angle < 45) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle >= 45 && ball->angle < 90) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 270 && ball->angle <= 315) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 315 && ball->angle <= 330) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 330 && ball->angle < 350) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
} else {
if(ball->angle > 10 && ball->angle < 30) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle >= 30 && ball->angle < 45) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle >= 45 && ball->angle < 90) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 270 && ball->angle <= 315) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 315 && ball->angle <= 330) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 330 && ball->angle < 350) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 10 && ball->angle < 30) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle >= 30 && ball->angle < 45) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle >= 45 && ball->angle < 90) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 270 && ball->angle <= 315) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 315 && ball->angle <= 330) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 330 && ball->angle < 350) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
this->storcimentoPorta();
if((ball->angle >= 330 || ball->angle <= 30) && ball->distance > 190) { //storcimento
atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito
drive->prepareDrive(atk_direction, atk_speed, cstorc);

5
src/keeper.cpp
View File

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

5
src/main.cpp
View File

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

8
src/sensors.cpp
View File

@ -5,18 +5,18 @@ void initSensors(){
pinMode(SWITCH_DX, 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)),
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));
compass = new DataSourceBNO055();
ball = new DataSourceBall(&Serial4, 57600);
camera = new DataSourceCamera(&Serial2, 19200);
usCtrl = new DataSourceCtrl(dUs);
linesCtrl = new LineSys2019(lIn, lOut);
bt = new DataSourceBT(&Serial3, 115200);
}