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

linesyscamera: better use of status vector 

* update with inputs
* use CURRENT_DATA_WRITE to call Game's PositionSystem, leads to seagfault when on a line on the first loop cycle otherwise
pull/1/head
emamaker 2022-06-29 15:04:16 +02:00 committed by EmaMaker
parent eb9fa12532
commit e24ace0a7a
1 changed files with 113 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
src/system/lines/linesys_camera.cpp
View File

@ -1,11 +1,12 @@
#include "systems/lines/linesys_camera.h" #include "systems/lines/linesys_camera.h"
#include "systems/position/positionsys_camera.h" #include "behaviour_control/status_vector.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "strategy_roles/games.h" #include "strategy_roles/games.h"
#include "behaviour_control/status_vector.h" #include "systems/position/positionsys_camera.h"
LineSysCamera::LineSysCamera() {} LineSysCamera::LineSysCamera() {}
LineSysCamera::LineSysCamera(vector<DataSource*> in_, vector<DataSource*> out_){ LineSysCamera::LineSysCamera(vector<DataSource *> in_, vector<DataSource *> out_)
{
this->in = in_; this->in = in_;
this->out = out_; this->out = out_;
@ -18,7 +19,8 @@ LineSysCamera::LineSysCamera(vector<DataSource*> in_, vector<DataSource*> out_){
tookLine = false; tookLine = false;
for(int i = 0; i < 4; i++){ for (int i = 0; i < 4; i++)
{
linetriggerI[i] = 0; linetriggerI[i] = 0;
linetriggerO[i] = 0; linetriggerO[i] = 0;
} }
@ -27,33 +29,43 @@ LineSysCamera::LineSysCamera(vector<DataSource*> in_, vector<DataSource*> out_){
linesens = 0; linesens = 0;
} }
void LineSysCamera ::update()
void LineSysCamera ::update() { {
CURRENT_INPUT_WRITE.lineByte = 0;
inV = 0; inV = 0;
outV = 0; outV = 0;
tookLine = false; tookLine = false;
for(DataSource* d : in) d->readSensor(); for (DataSource *d : in)
for(DataSource* d : out) d->readSensor(); d->readSensor();
for (DataSource *d : out)
d->readSensor();
for(auto it = in.begin(); it != in.end(); it++){ for (auto it = in.begin(); it != in.end(); it++)
{
i = it - in.begin(); i = it - in.begin();
ds = *it; ds = *it;
linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM; linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM;
CURRENT_INPUT_WRITE.lineByte |= linetriggerI[i] << i;
} }
for(auto it = out.begin(); it != out.end(); it++){ for (auto it = out.begin(); it != out.end(); it++)
{
i = it - out.begin(); i = it - out.begin();
ds = *it; ds = *it;
linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM; linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM;
CURRENT_INPUT_WRITE.lineByte |= linetriggerO[i] << (4+i);
} }
for(int i = 0; i < 4; i++){ for (int i = 0; i < 4; i++)
{
inV = inV | (linetriggerI[i] << i); inV = inV | (linetriggerI[i] << i);
outV = outV | (linetriggerO[i] << i); outV = outV | (linetriggerO[i] << i);
} }
if (inV > 0 || outV > 0) { if (inV > 0 || outV > 0)
if(millis() - exitTimer > EXIT_TIME) { {
if (millis() - exitTimer > EXIT_TIME)
{
fboundsX = true; fboundsX = true;
fboundsY = true; fboundsY = true;
} }
@ -61,27 +73,38 @@ void LineSysCamera ::update() {
} }
linesens |= inV | outV; linesens |= inV | outV;
CURRENT_DATA_WRITE.lineActive = linesens;
outOfBounds(); outOfBounds();
} }
void LineSysCamera::outOfBounds(){ void LineSysCamera::outOfBounds()
{
// digitalWriteFast(BUZZER, LOW); // digitalWriteFast(BUZZER, LOW);
if(fboundsX == true) { if (fboundsX == true)
if(linesens & 0x02) linesensOldX = 2; {
else if(linesens & 0x08) linesensOldX = 8; if (linesens & 0x02)
linesensOldX = 2;
else if (linesens & 0x08)
linesensOldX = 8;
if (linesensOldX != 0) fboundsX = false; if (linesensOldX != 0) fboundsX = false;
} }
if(fboundsY == true) { if (fboundsY == true)
if(linesens & 0x01) linesensOldY = 1; {
else if(linesens & 0x04) linesensOldY = 4; if (linesens & 0x01)
linesensOldY = 1;
else if (linesens & 0x04)
linesensOldY = 4;
if (linesensOldY != 0) fboundsY = false; if (linesensOldY != 0) fboundsY = false;
} }
if (millis() - exitTimer < EXIT_TIME){ if (millis() - exitTimer < EXIT_TIME)
CURRENT_DATA_READ.game->ps->goCenter(); {
CURRENT_DATA_WRITE.game->ps->goCenter();
tookLine = true; tookLine = true;
tone(BUZZER, 220.00, 250); tone(BUZZER, 220.00, 250);
}else{ }
else
{
// drive->canUnlock = true; // drive->canUnlock = true;
linesens = 0; linesens = 0;
linesensOldY = 0; linesensOldY = 0;
@ -89,23 +112,29 @@ void LineSysCamera::outOfBounds(){
} }
} }
void LineSysCamera::test(){ void LineSysCamera::test()
{
update(); update();
DEBUG.print("Byte Read: ");
DEBUG.println(CURRENT_INPUT_READ.lineByte, BIN);
DEBUG.print("In: "); DEBUG.print("In: ");
for(DataSource* d : in){ for (DataSource *d : in)
{
d->update(); d->update();
DEBUG.print(d->getValue()); DEBUG.print(d->getValue());
DEBUG.print(" | "); DEBUG.print(" | ");
} }
DEBUG.print(" |---| "); DEBUG.print(" |---| ");
DEBUG.print("Out: "); DEBUG.print("Out: ");
for(DataSource* d : out){ for (DataSource *d : out)
{
d->update(); d->update();
DEBUG.print(d->getValue()); DEBUG.print(d->getValue());
DEBUG.print(" | "); DEBUG.print(" | ");
} }
DEBUG.println(); DEBUG.println();
for(int i = 0; i < 4; i++){ for (int i = 0; i < 4; i++)
{
DEBUG.print(linetriggerI[i]); DEBUG.print(linetriggerI[i]);
DEBUG.print(" | "); DEBUG.print(" | ");
DEBUG.println(linetriggerO[i]); DEBUG.println(linetriggerO[i]);