EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

striker: follow the ball, pick it and score 

Our roller is about to fall apart and i have no specific sensor to detect the ball, which means in perfect conditions it works but it really doesn't in real world conditions
Also we need to ameliorate the communication protocol between teensy and 32u4 in order to not lose precision
robocup
EmaMaker 2021-05-28 20:53:35 +02:00
parent 393ee29b7e
commit 4397d602cd
14 changed files with 276 additions and 386 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/motors_movement/drivecontroller.h
View File

@ -10,7 +10,7 @@
//BEST NUMBERS YET
//USE MOVING AVERAGE AND ANGLE WRAP
#define KP 0.8
#define KP 1.35
#define KI 0.0
#define KD 0.025

2
include/motors_movement/roller.h
View File

@ -2,7 +2,7 @@
#include "ESC.h"
#define ROLLER_DEFAULT_SPEED 1200
#define ROLLER_DEFAULT_SPEED 1300
class Roller{
public:

2
include/sensors/data_source_ball.h
View File

@ -4,7 +4,7 @@
#define MOUTH_MIN_ANGLE 340
#define MOUTH_MAX_ANGLE 20
#define MOUTH_DISTANCE 115
#define MOUTH_DISTANCE 100
#define MOUTH_MAX_DISTANCE 140
class DataSourceBall : public DataSource{

5
include/strategy_roles/striker.h
View File

@ -20,9 +20,10 @@ class Striker : public Game{
void init() override;
void striker();
int tilt();
float ballTilt();
int atk_speed, atk_direction, atk_tilt;
float cstorc;
int atk_speed, atk_direction;
float atk_tilt, ball_tilt;
bool gotta_tilt;
};

49
include/systems/lines/linesys_camera_recovery.h Normal file
View File

@ -0,0 +1,49 @@
#pragma once
#include <Arduino.h>
#include "behaviour_control/ds_ctrl.h"
#include "systems/systems.h"
#include "vars.h"
#define S1O A7
#define S1I A6
#define S2O A2
#define S2I A3
#define S3I A9
#define S3O A8
#define S4I A0
#define S4O A1
#define LINE_THRESH_CAM_REC 350
#define EXIT_TIME_REC 300
#define X_LIMIT 4
#define Y_LIMIT 5
class LineSysCameraRecovery : public LineSystem{
public:
LineSysCameraRecovery();
LineSysCameraRecovery(vector<DataSource*> in_, vector<DataSource*> out);
void update() override;
void test() override;
void outOfBounds();
void checkLineSensors();
void striker();
bool isInLimitX();
bool isInLimitY();
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;
int outX, outY;
};

145
lib/ArduinoPIDLibraryFlavio/PID_v2.cpp
View File

@ -17,12 +17,12 @@
* The parameters specified here are those for for which we can't set up
* reliable defaults, so we need to have the user set them.
***************************************************************************/
PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int POn, int ControllerDirection)
PID::PID(double* Input_, double* Output_, double* Setpoint_,
double Kp, double Ki, double Kd, int ControllerDirection)
{
myOutput = Output;
myInput = Input;
mySetpoint = Setpoint;
Output = Output_;
Input = Input_;
Setpoint = Setpoint_;
inAuto = false;
PID::SetOutputLimits(0, 255); //default output limit corresponds to
@ -31,24 +31,11 @@ PID::PID(double* Input, double* Output, double* Setpoint,
SampleTime = 100; //default Controller Sample Time is 0.1 seconds
PID::SetControllerDirection(ControllerDirection);
PID::SetTunings(Kp, Ki, Kd, POn);
PID::SetTunings(Kp, Ki, Kd);
lastTime = millis()-SampleTime;
}
/*Constructor (...)*********************************************************
* To allow backwards compatability for v1.1, or for people that just want
* to use Proportional on Error without explicitly saying so
***************************************************************************/
PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int ControllerDirection)
:PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection)
{
}
/* Compute() **********************************************************************
* This, as they say, is where the magic happens. this function should be called
* every time "void loop()" executes. the function will decide for itself whether a new
@ -57,55 +44,38 @@ PID::PID(double* Input, double* Output, double* Setpoint,
**********************************************************************************/
bool PID::Compute()
{
if(!inAuto) return false;
if(!inAuto) return;
unsigned long now = millis();
unsigned long timeChange = (now - lastTime);
int timeChange = (now - lastTime);
if(timeChange>=SampleTime)
{
/*Compute all the working error variables*/
double input = *myInput;
double error = *mySetpoint - input;
if(angleWrap){
if(error < -179) error += 360;
if(error > 180) error -= 360;
}
double error = *Setpoint - *Input;
double dInput = (input - lastInput);
outputSum+= (ki * error);
/*Add Proportional on Measurement, if P_ON_M is specified*/
if(!pOnE) outputSum-= kp * dInput;
if(outputSum > outMax) outputSum= outMax;
else if(outputSum < outMin) outputSum= outMin;
/*Add Proportional on Error, if P_ON_E is specified*/
double output;
if(pOnE) output = kp * error;
else output = 0;
if(kd_lagpam <=1){
/*Compute Rest of PID Output*/
filteredDerivative =(1.0-kd_lagpam)*filteredDerivative
+ (kd_lagpam)*dInput;
if(angleWrap){
if(error > 180) error = error - 360;
else if(error < -180) error = error + 360;
}
else{
filteredDerivative = maf.process(dInput);
}
output += outputSum - kd * filteredDerivative;
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
*myOutput = output;
ITerm+= (ki * error);
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
double dInput = (*Input - lastInput);
if(angleWrap){
if(dInput > 180) dInput = dInput - 360;
else if(dInput < -180) dInput = dInput + 360;
}
/*Compute PID Output*/
*Output = kp * error + ITerm- kd * dInput;
if(*Output > outMax) *Output = outMax;
else if(*Output < outMin) *Output = outMin;
/*Remember some variables for next time*/
lastInput = input;
lastInput = *Input;
lastTime = now;
return true;
}
else return false;
}
/* SetTunings(...)*************************************************************
@ -113,20 +83,15 @@ bool PID::Compute()
* it's called automatically from the constructor, but tunings can also
* be adjusted on the fly during normal operation
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd, int POn)
void PID::SetTunings(double Kp, double Ki, double Kd)
{
if (Kp<0 || Ki<0 || Kd<0) return;
pOn = POn;
pOnE = POn == P_ON_E;
dispKp = Kp; dispKi = Ki; dispKd = Kd;
double SampleTimeInSec = ((double)SampleTime)/1000;
if (Kp<0 || Ki<0|| Kd<0) return;
double SampleTimeInSec = ((double)SampleTime)/1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;
if(controllerDirection ==REVERSE)
{
kp = (0 - kp);
@ -135,13 +100,6 @@ void PID::SetTunings(double Kp, double Ki, double Kd, int POn)
}
}
/* SetTunings(...)*************************************************************
* Set Tunings using the last-rembered POn setting
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd){
SetTunings(Kp, Ki, Kd, pOn);
}
/* SetSampleTime(...) *********************************************************
* sets the period, in Milliseconds, at which the calculation is performed
******************************************************************************/
@ -167,18 +125,15 @@ void PID::SetSampleTime(int NewSampleTime)
**************************************************************************/
void PID::SetOutputLimits(double Min, double Max)
{
if(Min >= Max) return;
if(Min > Max) return;
outMin = Min;
outMax = Max;
if(inAuto)
{
if(*myOutput > outMax) *myOutput = outMax;
else if(*myOutput < outMin) *myOutput = outMin;
if(outputSum > outMax) outputSum= outMax;
else if(outputSum < outMin) outputSum= outMin;
}
if(*Output > outMax) *Output = outMax;
else if(*Output < outMin) *Output = outMin;
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
}
/* SetMode(...)****************************************************************
@ -189,9 +144,9 @@ void PID::SetOutputLimits(double Min, double Max)
void PID::SetMode(int Mode)
{
bool newAuto = (Mode == AUTOMATIC);
if(newAuto && !inAuto)
if(newAuto == !inAuto)
{ /*we just went from manual to auto*/
PID::Initialize();
Initialize();
}
inAuto = newAuto;
}
@ -202,10 +157,10 @@ void PID::SetMode(int Mode)
******************************************************************************/
void PID::Initialize()
{
outputSum = *myOutput;
lastInput = *myInput;
if(outputSum > outMax) outputSum = outMax;
else if(outputSum < outMin) outputSum = outMin;
lastInput = *Input;
ITerm = *Output;
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
}
/* SetControllerDirection(...)*************************************************
@ -216,15 +171,13 @@ void PID::Initialize()
******************************************************************************/
void PID::SetControllerDirection(int Direction)
{
if(inAuto && Direction !=controllerDirection)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
controllerDirection = Direction;
}
void PID::setAngleWrap(bool val){
angleWrap = val;
}
/* Status Funcions*************************************************************
* Just because you set the Kp=-1 doesn't mean it actually happened. these
* functions query the internal state of the PID. they're here for display

36
lib/ArduinoPIDLibraryFlavio/PID_v2.h
View File

@ -9,7 +9,6 @@
class PID
{
public:
//Constants used in some of the functions below
@ -17,8 +16,6 @@ class PID
#define MANUAL 0
#define DIRECT 0
#define REVERSE 1
#define P_ON_M 0
#define P_ON_E 1
//commonly used functions **************************************************************************
PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and
@ -55,18 +52,7 @@ class PID
void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which
// the PID calculation is performed. default is 100
void SetDerivativeLag(double val){
kd_lagpam = val;
}
void setAngleWrap(bool a){
angleWrap = a;
}
double getDerivative(){
return filteredDerivative;
}
void setAngleWrap(bool a);
//Display functions ****************************************************************
double GetKp(); // These functions query the pid for interal values.
@ -84,24 +70,22 @@ class PID
double dispKd; //
double kp; // * (P)roportional Tuning Parameter
double ki; // * (I)ntegral Tuning Parameter
double kd; // * (D)erivative Tuning Parameter
double filteredDerivative;
double kd_lagpam = 1; //* 0.15 to 0.35
double ki; // * (I)ntegral Tuning Parameter
double kd; // * (D)erivative Tuning Parameter
int controllerDirection;
int pOn;
double *myInput; // * Pointers to the Input, Output, and Setpoint variables
double *myOutput; // This creates a hard link between the variables and the
double *mySetpoint; // PID, freeing the user from having to constantly tell us
// what these values are. with pointers we'll just know.
double *Input; // * Pointers to the Input, Output, and Setpoint variables
double *Output; // This creates a hard link between the variables and the
double *Setpoint; // PID, freeing the user from having to constantly tell us
// what these values are. with pointers we'll just know.
unsigned long lastTime;
double outputSum, lastInput;
double lastInput, ITerm;
unsigned long SampleTime;
double outMin, outMax;
bool inAuto, pOnE, angleWrap;
bool inAuto;
bool angleWrap;
};
#endif

18
src/main.cpp
View File

@ -13,12 +13,12 @@ void updateRoller();
TestMenu* testmenu;
#define BALL_IN A13
bool striker_condition = false;
bool keeper_condition = false;
void setup() {
pinMode(BUZZER, OUTPUT);
tone(BUZZER, 220, 250);
delay(1500);
DEBUG.begin(115200);
@ -49,19 +49,17 @@ void setup() {
void loop() {
updateSensors();
drive->resetDrive();
striker_condition = role == HIGH;
// striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
keeper_condition = role == LOW;
striker->play(1);
// if(role) precision_shooter->play(1);
// else pass_and_shoot->play(1);
testmenu->testMenu();
// keeper_condition = role == LOW;
// keeper->play(keeper_condition);
// testmenu->testMenu();
// // Last thing to do: movement and update status vector
// Last thing to do: movement and update status vector
drive->drivePrepared();
updateStatusVector();
}

4
src/motors_movement/drivecontroller.cpp
View File

@ -26,11 +26,11 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
output = 0;
setpoint = 0;
pid = new PID(&input, &output, &setpoint, KP, KI, KD, 1,DIRECT);
pid = new PID(&input, &output, &setpoint, KP, KI, KD,DIRECT);
pid->SetSampleTime(2.5);
pid->setAngleWrap(true);
pid->SetDerivativeLag(2);
// pid->SetDerivativeLag(2);
pid->SetOutputLimits(-255,255);
pid->SetMode(AUTOMATIC);

9
src/sensors/data_source_ball.cpp
View File

@ -14,9 +14,8 @@ void DataSourceBall :: postProcess(){
ballSeen = distance == 255 ? 0 : 1;
}else{
angle = value * 2;
int imuAngle = CURRENT_DATA_READ.IMUAngle > 180 ? 360 -CURRENT_DATA_READ.IMUAngle : CURRENT_DATA_READ.IMUAngle;
int ballAngle = angle > 180 ? 360 -angle : angle;
angleFix = (ballAngle-imuAngle+360)%360;
int imuAngle = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle;
angleFix = (angle+imuAngle+360)%360;
}
CURRENT_INPUT_WRITE.ballByte = value;
CURRENT_DATA_WRITE.ballAngle = angle;
@ -45,9 +44,9 @@ bool DataSourceBall::isInFront(){
}
bool DataSourceBall::isInMouth(){
return CURRENT_DATA_READ.ballSeen && (isInFront() && CURRENT_DATA_READ.ballDistance<=MOUTH_DISTANCE);
return isInFront() && CURRENT_DATA_READ.ballDistance<=MOUTH_DISTANCE;
}
bool DataSourceBall::isInMouthMaxDistance(){
return CURRENT_DATA_READ.ballSeen && (isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE);
return isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE;
}

52
src/strategy_roles/striker.cpp
View File

@ -22,7 +22,6 @@ void Striker::init()
atk_speed = 0;
atk_direction = 0;
atk_tilt = 0;
cstorc = 0;
gotta_tilt = false;
}
@ -31,38 +30,49 @@ void Striker::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
this->striker();
else
else{
ps->goCenter();
roller->speed(roller->MIN);
}
}
float ctilt = 0;
unsigned long ttilt = 0;
void Striker::striker(){
//seguo palla
int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG;
if(ball_deg >= 344 || ball_deg <= 16) plusang = STRIKER_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
if(ball_deg > 180) ball_degrees2 = ball_deg - 360; //ragiono in +180 -180
else ball_degrees2 = ball_deg;
if(ball_degrees2 > 0) dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo
else dir = ball_deg - plusang; //se sto nel negativo sottraggo
dir = (dir + 360) % 360;
drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
else roller->speed(roller->MIN);
int ball_angle = CURRENT_DATA_READ.ballAngleFix;
if(ball_angle > 180) ball_angle -= 360;
if(!ball->isInMouth())ttilt=millis();
// int tmp_ball_tilt = (0.25f*ball_angle+old_ball_Angle*0.75f);
// ball_tilt = ball->isInMouth() ? ball_angle : ballTilt();
// // drive->prepareDrive(0,30,ball->isInMouth() && roller->roller_armed ? tilt() : ballTilt());
drive->prepareDrive(0,30, millis() - ttilt > 250 ? tilt(): ball_angle);
// old_ball_Angle = ball_angle;
// old_ball_tilt = (int) ball_tilt;
}
int Striker::tilt() {
if (ball->isInMouth() /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
else gotta_tilt = false;
float Striker::ballTilt(){
if(CURRENT_DATA_READ.ballAngle > 180 && CURRENT_DATA_READ.ballAngle < 340) ball_tilt -= 0.15f;
else if(CURRENT_DATA_READ.ballAngle <= 180 && CURRENT_DATA_READ.ballAngle > 20) ball_tilt += 0.15f;
return ball_tilt;
}
if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {
int Striker::tilt() {
if(CURRENT_DATA_READ.atkSeen && (CURRENT_DATA_READ.ballAngleFix > 340 || CURRENT_DATA_READ.ballAngleFix < 20)){
atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
}else{
atk_tilt *= 0.8;
if(atk_tilt <= 10) atk_tilt = 0;
}else{
atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
}
return atk_tilt;

117
src/system/lines/linesys_camera_recovery.cpp Normal file
View File

@ -0,0 +1,117 @@
#include "systems/lines/linesys_camera_recovery.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "strategy_roles/games.h"
#include "behaviour_control/status_vector.h"
LineSysCameraRecovery::LineSysCameraRecovery(){}
LineSysCameraRecovery::LineSysCameraRecovery(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 LineSysCameraRecovery ::update() {
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;
}
for(auto it = out.begin(); it != out.end(); it++){
i = it - out.begin();
ds = *it;
linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM;
}
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;
outOfBounds();
}
void LineSysCameraRecovery::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->goCenter();
tookLine = true;
tone(BUZZER, 220.00, 250);
}else{
// drive->canUnlock = true;
linesens = 0;
linesensOldY = 0;
linesensOldX = 0;
}
}
void LineSysCameraRecovery::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();
}

2
src/system/positions/positionsys_camera.cpp
View File

@ -22,12 +22,10 @@ PositionSysCamera::PositionSysCamera() {
X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE);
X->SetOutputLimits(-MAX_X, MAX_X);
X->SetMode(AUTOMATIC);
X->SetDerivativeLag(1);
X->SetSampleTime(2);
Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE);
Y->SetOutputLimits(-MAX_Y,MAX_Y);
Y->SetMode(AUTOMATIC);
Y->SetDerivativeLag(1);
Y->SetSampleTime(2);
filterDir = new ComplementaryFilter(0.35);

219
utility/OpenMV/conic_eff_h7.py.autosave
View File

@ -1,219 +0,0 @@
# color tracking with conic mirror - By: EmaMaker - wed 15 jan 2020
# Based on:
# color tracking - By: paolix - ven mag 18 2018
# Automatic RGB565 Color Tracking Example
#
import sensor, image, time, pyb, math
from pyb import UART
uart = UART(3,19200, timeout_char = 1000)
START_BYTE = chr(105) #'i'
END_BYTE = chr(115) #'s'
BYTE_UNKNOWN = chr(116) #'t'
y_found = False
b_found = False
#From Arduino Documentation at: https://www.arduino.cc/reference/en/language/functions/math/map/
def val_map(x, in_min, in_max, out_min, out_max):
x = int(x)
in_min = int(in_min)
in_max = int(in_max)
out_min = int(out_min)
out_max = int(out_max)
return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
# Check side
def isInLeftSide(img, x):
return x < img.width() / 2
def isInRightSide(img, x):
return x > img.width() / 2
# LED Setup ##################################################################
red_led = pyb.LED(1)
green_led = pyb.LED(2)
blue_led = pyb.LED(3)
red_led.off()
green_led.off()
blue_led.on()
##############################################################################
thresholds = [ (70, 100, -36, 8, 38, 112), # thresholds yellow goalz
(50, 77, -23, 8, -60, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (50, 0, 250, 200)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
sensor.set_auto_exposure(False, 10000) vbc
#sensor.set_backlight(1)
#sensor.set_brightness(+2 )
#sensor.set_windowing(roi)
clock = time.clock()'''
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
#sensor.set_windowing(roi)
sensor.set_contrast(0)
sensor.set_saturation(2)
sensor.set_brightness(3)
sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -1.804))
sensor.set_auto_exposure(False, 6576)
#sensor.set_auto_gain(False, gain_db=8.78)
sensor.skip_frames(time = 300)
clock = time.clock()
##############################################################################
while(True):
clock.tick()
print("Exposure: " + str(sensor.get_exposure_us()) + " Gain: " + str(sensor.get_gain_db()) + " White Bal: " + str(sensor.get_rgb_gain_db()))
blue_led.off()
y_found = False
b_found = False
tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
tt_blue = [(0,999,0,2)] ## creo una lista di tuple per il blue, valore x = 999 : non trovata
img = sensor.snapshot()
for blob in img.find_blobs(thresholds, pixels_threshold=80, area_threshold=100, merge = True):
img.draw_rectangle(blob.rect())
#img.draw_cross(blob.cx(), blob.cy())
if (blob.code() == 1):
tt_yellow = tt_yellow + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
y_found = True
if (blob.code() == 2):
tt_blue = tt_blue + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
b_found = True
tt_yellow.sort(key=lambda tup: tup[0]) ## ordino le liste
tt_blue.sort(key=lambda tup: tup[0]) ## ordino le liste
ny = len(tt_yellow)
nb = len(tt_blue)
#Formulas to compute position of points, considering that the H7 is rotated by a certain angle
#x = y-offset
#y = offset - x
#Compute everything related to Yellow First
y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1]
y_cx = int(y1_cy - img.height() / 2)
y_cy = int(img.width() / 2 - y1_cx)
#Normalize data between 0 and 100
if y_found == True:
img.draw_cross(y1_cx, y1_cy)
y_cx = val_map(y_cx, -img.height() / 2, img.height() / 2, 100, 0)
y_cy = val_map(y_cy, -img.width() / 2, img.width() / 2, 0, 100)
#Prepare for send as a list of characters
s_ycx = chr(y_cx)
s_ycy = chr(y_cy)
else:
y_cx = BYTE_UNKNOWN
y_cy = BYTE_UNKNOWN
#Prepare for send as a list of characters
s_ycx = y_cx
s_ycy = y_cy
#Compute everything relative to Blue
'''Given the light situation in our lab and given that blue is usually harder to spot than yellow, we need to check it we got
a blue blob that is in the same side of the ground as the yellow one, if so, discard it and check a new one
'''
b_cx = BYTE_UNKNOWN
b_cy = BYTE_UNKNOWN
#Prepare for send as a list of characters
s_bcx = b_cx
s_bcy = b_cy
if b_found == True:
for i in range(nb-1, 0,-1):
b_area, b1_cx, b1_cy, b_code = tt_blue[i]
if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))):
img.draw_cross(b1_cx, b1_cy)
b_cx = int(b1_cy - img.height() / 2)
b_cy = int(img.width() / 2 - b1_cx)
#print("before :" + str(b_cx) + " " + str(b_cy))
b_cx = val_map(b_cx, -img.height() / 2, img.height() / 2, 100, 0)
b_cy = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 100)
#print("after :" + str(b_cx) + " " + str(b_cy))
#Prepare for send as a list of characters
s_bcx = chr(b_cx)
s_bcy = chr(b_cy)
'''index = 1
if b_found == True:
while nb-index >= 0:
b_area, b1_cx, b1_cy, b_code = tt_blue[nb-index]
index += 1
# If the two blobs are on opposide side of the field, everything is good
if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))):
img.draw_cross(b1_cx, b1_cy)
b_cx = int(b1_cy - img.height() / 2)
b_cy = int(img.width() / 2 - b1_cx)
print("before :" + str(b_cx) + " " + str(b_cy))
b_cx = val_map(b_cx, -img.height() / 2, img.height() / 2, 100, 0)
b_cy = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 100)
print("after :" + str(b_cx) + " " + str(b_cy))
#Prepare for send as a list of characters
s_bcx = chr(b_cx)
s_bcy = chr(b_cy)
break
else:
b_cx = BYTE_UNKNOWN
b_cy = BYTE_UNKNOWN
#Prepare for send as a list of characters
s_bcx = b_cx
s_bcy = b_cy'''
#print(str(y_cx) + " | " + str(y_cy) + " --- " + str(b_cx) + " | " + str(b_cy))
uart.write(START_BYTE)
uart.write(s_bcx)
uart.write(s_bcy)
uart.write(s_ycx)
uart.write(s_ycy)
uart.write(END_BYTE)