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

197
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,100 +19,128 @@ 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; {
linetriggerO[i] = 0; linetriggerI[i] = 0;
linetriggerO[i] = 0;
} }
exitTimer = 0; exitTimer = 0;
linesens = 0; linesens = 0;
} }
void LineSysCamera ::update()
{
CURRENT_INPUT_WRITE.lineByte = 0;
inV = 0;
outV = 0;
tookLine = false;
void LineSysCamera ::update() { for (DataSource *d : in)
inV = 0; d->readSensor();
outV = 0; for (DataSource *d : out)
tookLine = false; d->readSensor();
for(DataSource* d : in) d->readSensor(); for (auto it = in.begin(); it != in.end(); it++)
for(DataSource* d : out) d->readSensor(); {
i = it - in.begin();
for(auto it = in.begin(); it != in.end(); it++){ ds = *it;
i = it - in.begin(); linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM;
ds = *it; CURRENT_INPUT_WRITE.lineByte |= linetriggerI[i] << i;
linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM; }
} 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++){
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; for (int i = 0; i < 4; i++)
outOfBounds(); {
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;
CURRENT_DATA_WRITE.lineActive = linesens;
outOfBounds();
} }
void LineSysCamera::outOfBounds(){ void LineSysCamera::outOfBounds()
// digitalWriteFast(BUZZER, LOW); {
if(fboundsX == true) { // digitalWriteFast(BUZZER, LOW);
if(linesens & 0x02) linesensOldX = 2; if (fboundsX == true)
else if(linesens & 0x08) linesensOldX = 8; {
if(linesensOldX != 0) fboundsX = false; if (linesens & 0x02)
} linesensOldX = 2;
if(fboundsY == true) { else if (linesens & 0x08)
if(linesens & 0x01) linesensOldY = 1; linesensOldX = 8;
else if(linesens & 0x04) linesensOldY = 4; if (linesensOldX != 0) fboundsX = false;
if(linesensOldY != 0) fboundsY = 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){ if (millis() - exitTimer < EXIT_TIME)
CURRENT_DATA_READ.game->ps->goCenter(); {
tookLine = true; CURRENT_DATA_WRITE.game->ps->goCenter();
tone(BUZZER, 220.00, 250); tookLine = true;
}else{ tone(BUZZER, 220.00, 250);
// drive->canUnlock = true; }
linesens = 0; else
linesensOldY = 0; {
linesensOldX = 0; // drive->canUnlock = true;
} linesens = 0;
linesensOldY = 0;
linesensOldX = 0;
}
} }
void LineSysCamera::test(){ void LineSysCamera::test()
update(); {
DEBUG.print("In: "); update();
for(DataSource* d : in){ DEBUG.print("Byte Read: ");
d->update(); DEBUG.println(CURRENT_INPUT_READ.lineByte, BIN);
DEBUG.print(d->getValue()); DEBUG.print("In: ");
DEBUG.print(" | "); for (DataSource *d : in)
} {
DEBUG.print(" |---| "); d->update();
DEBUG.print("Out: "); DEBUG.print(d->getValue());
for(DataSource* d : out){ DEBUG.print(" | ");
d->update(); }
DEBUG.print(d->getValue()); DEBUG.print(" |---| ");
DEBUG.print(" | "); DEBUG.print("Out: ");
} for (DataSource *d : out)
DEBUG.println(); {
for(int i = 0; i < 4; i++){ d->update();
DEBUG.print(linetriggerI[i]); DEBUG.print(d->getValue());
DEBUG.print(" | "); DEBUG.print(" | ");
DEBUG.println(linetriggerO[i]); }
} DEBUG.println();
for (int i = 0; i < 4; i++)
{
DEBUG.print(linetriggerI[i]);
DEBUG.print(" | ");
DEBUG.println(linetriggerO[i]);
}
DEBUG.println(inV); DEBUG.println(inV);
DEBUG.println(outV); DEBUG.println(outV);
DEBUG.println(); DEBUG.println();
} }