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

line-sys-camera: better handling of edge cases

pull/2/head
emamaker 2022-07-07 21:52:52 +02:00
parent eb9e8ad475
commit ae149dc71c
2 changed files with 59 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
include/systems/lines/linesys_camera.h
View File

@ -5,7 +5,7 @@
#include "behaviour_control/ds_ctrl.h"
#include "systems/systems.h"
#include "vars.h"
#include "vars.h"
#define S1I A14
#define S1O A15
@ -19,23 +19,25 @@
#define LINE_THRESH_CAM 100
#define EXIT_TIME 100
class LineSysCamera : public LineSystem{
class LineSysCamera : public LineSystem
{
public:
LineSysCamera();
LineSysCamera(vector<DataSource*> in_, vector<DataSource*> out);
public:
LineSysCamera();
LineSysCamera(vector<DataSource *> in_, vector<DataSource *> out);
void update() override;
void test() override;
void outOfBounds();
void checkLineSensors();
private:
vector<DataSource*> in, out;
DataSource* ds;
bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow;
int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], linepins[4], i;
unsigned long exitTimer;
int outDir, outVel;
byte linesens;
void update() override;
void test() override;
void outOfBounds();
void checkLineSensors();
bool angleCritic(int angle);
private:
vector<DataSource *> in, out;
DataSource *ds;
bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow;
int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], linepins[4], i;
unsigned long exitTimer;
int outDir, outVel;
byte linesens;
};

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

@ -99,19 +99,45 @@ void LineSysCamera::outOfBounds()
if (millis() - exitTimer < EXIT_TIME)
{
int dir = 0;
int speed = CURRENT_DATA_READ.ySeen || CURRENT_DATA_READ.bSeen ? MAX_VEL : 0;
int yangle = CURRENT_DATA_READ.yangle_fix * CURRENT_DATA_READ.ySeen;
int bangle = CURRENT_DATA_READ.bangle_fix * CURRENT_DATA_READ.bSeen;
int ydist = CURRENT_DATA_READ.ydist;
int bdist = CURRENT_DATA_READ.bdist;
int diffB = abs(min(yangle, bangle) - max(yangle, bangle));
int diffB1 = 360 - diffB;
int diff = min(diffB, diffB1);
if (!CURRENT_DATA_READ.bSeen && !CURRENT_DATA_READ.ySeen)
{
dir = 0;
speed = 0;
}
else
{
if (angleCritic(yangle) || angleCritic(bangle))
{
if(CURRENT_DATA_READ.bSeen && CURRENT_DATA_READ.ySeen){
dir = ydist > bdist ? yangle : bangle;
}else{
if (CURRENT_DATA_READ.bSeen) dir = bangle;
else
dir = yangle;
}
}
else
{
int diffB = abs(min(yangle, bangle) - max(yangle, bangle));
int diffB1 = 360 - diffB;
int diff = min(diffB, diffB1);
dir = min(yangle, bangle) + diff * 0.5f;
}
drive->prepareDrive(dir, speed, CURRENT_DATA_WRITE.tilt);
DEBUG.println("AngleY " + String(yangle) + " AngleB" + String(bangle) + " Dir " + String(min(yangle, bangle) + diff));
// DEBUG.println("AngleY:" + String(yangle) + " AngleB:" + String(bangle) + " DistY: " + String(ydist) + " DistB: " +String(bdist) + " Dir " + String(dir));
drive->prepareDrive(min(yangle, bangle) + diff, CURRENT_DATA_READ.ySeen || CURRENT_DATA_READ.bSeen ? MAX_VEL : 0, CURRENT_DATA_WRITE.tilt);
// CURRENT_DATA_WRITE.game->ps->goCenter();
// tookLine = true;
tone(BUZZER, 220.00, 250);
// CURRENT_DATA_WRITE.game->ps->goCenter();
// tookLine = true;
tone(BUZZER, 220.00, 250);
}
}
else
{
@ -122,6 +148,11 @@ void LineSysCamera::outOfBounds()
}
}
bool LineSysCamera::angleCritic(int angle)
{
return angle >= 355 || angle <= 5 || (angle >= 175 && angle <= 185);
}
void LineSysCamera::test()
{
update();