128 lines
2.9 KiB
C++
128 lines
2.9 KiB
C++
#include "systems/lines/linesys_camera_keeper.h"
|
|
#include "systems/position/positionsys_camera.h"
|
|
#include "sensors/sensors.h"
|
|
#include "strategy_roles/games.h"
|
|
#include "behaviour_control/status_vector.h"
|
|
|
|
LineSysCameraKeeper::LineSysCameraKeeper(){}
|
|
LineSysCameraKeeper::LineSysCameraKeeper(vector<DataSource*> in_, vector<DataSource*> out_){
|
|
this->in = in_;
|
|
this->out = out_;
|
|
|
|
fboundsX = false;
|
|
fboundsY = false;
|
|
slow = false;
|
|
|
|
linesensOldX = 0;
|
|
linesensOldY = 0;
|
|
|
|
tookLine = false;
|
|
|
|
for(int i = 0; i < 4; i++){
|
|
linetriggerI[i] = 0;
|
|
linetriggerO[i] = 0;
|
|
}
|
|
|
|
exitTimer = 0;
|
|
linesens = 0;
|
|
}
|
|
|
|
|
|
void LineSysCameraKeeper ::update() {
|
|
CURRENT_INPUT_WRITE.lineByte = 0;
|
|
inV = 0;
|
|
outV = 0;
|
|
tookLine = false;
|
|
|
|
for (DataSource *d : in)
|
|
d->readSensor();
|
|
for (DataSource *d : out)
|
|
d->readSensor();
|
|
|
|
for (auto it = in.begin(); it != in.end(); it++)
|
|
{
|
|
i = it - in.begin();
|
|
ds = *it;
|
|
linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM;
|
|
CURRENT_INPUT_WRITE.lineByte |= linetriggerI[i] << i;
|
|
}
|
|
for (auto it = out.begin(); it != out.end(); it++)
|
|
{
|
|
i = it - out.begin();
|
|
ds = *it;
|
|
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;
|
|
CURRENT_DATA_WRITE.lineActive = linesens;
|
|
outOfBounds();
|
|
}
|
|
|
|
void LineSysCameraKeeper::outOfBounds(){
|
|
// digitalWriteFast(BUZZER, LOW);
|
|
if(fboundsX == true) {
|
|
if(linesens & 0x02) linesensOldX = 2;
|
|
else if(linesens & 0x08) linesensOldX = 8;
|
|
if(linesensOldX != 0) fboundsX = 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){
|
|
// CURRENT_DATA_READ.game->ps->centerGoal();
|
|
tookLine = true;
|
|
tone(BUZZER, 220.00, 250);
|
|
}else{
|
|
// drive->canUnlock = true;
|
|
linesens = 0;
|
|
linesensOldY = 0;
|
|
linesensOldX = 0;
|
|
}
|
|
}
|
|
|
|
void LineSysCameraKeeper::test(){
|
|
update();
|
|
DEBUG.print("In: ");
|
|
for(DataSource* d : in){
|
|
d->update();
|
|
DEBUG.print(d->getValue());
|
|
DEBUG.print(" | ");
|
|
}
|
|
DEBUG.print(" |---| ");
|
|
DEBUG.print("Out: ");
|
|
for(DataSource* d : out){
|
|
d->update();
|
|
DEBUG.print(d->getValue());
|
|
DEBUG.print(" | ");
|
|
}
|
|
DEBUG.println();
|
|
for(int i = 0; i < 4; i++){
|
|
DEBUG.print(linetriggerI[i]);
|
|
DEBUG.print(" | ");
|
|
DEBUG.println(linetriggerO[i]);
|
|
}
|
|
|
|
DEBUG.println(inV);
|
|
DEBUG.println(outV);
|
|
DEBUG.println();
|
|
} |