New conic shaped mirror, works better. It's higher and has more noticeable changes on the y axis
EmaMaker
parent
2ac684220f
commit
42d73d7e85
|
|
@ -4,10 +4,10 @@
|
|||
|
||||
#define startp 105
|
||||
#define endp 115
|
||||
#define unkn 116
|
||||
//Coords are mapped from 0 up to this value
|
||||
#define MAP_MAX 100
|
||||
#define HALF_MAP_MAX 50
|
||||
//#define unkn 0b01101001
|
||||
|
||||
class DataSourceCameraConic : public DataSource{
|
||||
|
||||
|
|
|
|||
|
|
@ -6,9 +6,17 @@
|
|||
|
||||
//PID Constants
|
||||
#define KP 1.5
|
||||
#define KI 0
|
||||
#define KI 0.2
|
||||
#define KD 0.1
|
||||
|
||||
#define KSPD 0.3
|
||||
|
||||
//BEST NUMBERS YET
|
||||
//USE MOVING AVERAGE AND ANGLE WRAP
|
||||
// #define KP 1.5
|
||||
// #define KI 0
|
||||
// #define KD 0.1
|
||||
|
||||
#define UNLOCK_THRESH 800
|
||||
|
||||
class DriveController{
|
||||
|
|
@ -33,8 +41,7 @@ class DriveController{
|
|||
Motor* m3;
|
||||
Motor* m4;
|
||||
PID* pid;
|
||||
int pDir, pSpeed, pTilt;
|
||||
int gDir, gSpeed, gTilt;
|
||||
int pDir, pSpeed, pTilt, oldSpeed;
|
||||
float x, y, vx, vy, speed1, speed2, speed3, speed4, pidfactor, delta;
|
||||
|
||||
double input, output, setpoint;
|
||||
|
|
|
|||
|
|
@ -4,6 +4,9 @@
|
|||
#include "sensors.h"
|
||||
#include "data_source_camera_vshapedmirror.h"
|
||||
|
||||
#define TILT_MULT 1.8
|
||||
#define TILT_DIST 180
|
||||
#define CATCH_DIST 150
|
||||
#define GOALIE_ATKSPD_LAT 255
|
||||
#define GOALIE_ATKSPD_BAK 350
|
||||
#define GOALIE_ATKSPD_FRT 345
|
||||
|
|
|
|||
|
|
@ -0,0 +1,14 @@
|
|||
#include "systems.h"
|
||||
|
||||
#define CAMERA_CENTER_X 0
|
||||
#define CAMERA_CENTER_Y 0
|
||||
|
||||
class PositionSysCamera : public PositionSystem{
|
||||
|
||||
public:
|
||||
PositionSysCamera();
|
||||
void goCenter();
|
||||
void update() override;
|
||||
void test() override;
|
||||
|
||||
};
|
||||
|
|
@ -55,9 +55,6 @@
|
|||
#define SOUTH_CENTER 8
|
||||
#define SOUTH_EAST 9
|
||||
|
||||
#define CAMERA_CENTER_X 3
|
||||
#define CAMERA_CENTER_Y 6
|
||||
|
||||
class PositionSysZone : public PositionSystem{
|
||||
public:
|
||||
PositionSysZone();
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ s_extr LineSys2019* linesCtrl;
|
|||
|
||||
s_extr DataSourceBNO055* compass;
|
||||
s_extr DataSourceBall* ball;
|
||||
s_extr DataSourceCameraVShaped* camera;
|
||||
s_extr DataSourceCameraConic* camera;
|
||||
s_extr DriveController* drive;
|
||||
s_extr DataSourceBT* bt;
|
||||
|
||||
|
|
|
|||
|
|
@ -35,13 +35,21 @@ typedef struct input{
|
|||
}input;
|
||||
|
||||
typedef struct data{
|
||||
int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, angleAtk, angleAtkFix, angleDef, angleDefFix,
|
||||
speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role, axisBlock[4];
|
||||
int IMUAngle, ballAngle, ballDistance,
|
||||
yAngle, bAngle, yAngleFix, bAngleFix,
|
||||
yDist, bDist,
|
||||
angleAtk, angleAtkFix, angleDef, angleDefFix,
|
||||
speed, tilt, dir, axisBlock[4],
|
||||
USfr, USsx, USdx, USrr,
|
||||
lineOutDir, matePos, role;
|
||||
Game* game;
|
||||
LineSystem* lineSystem;
|
||||
PositionSystem* posSystem;
|
||||
byte xb, yb, xy, yy, lineSeen, lineActive;
|
||||
bool mate, ATKgoal, DEFgoal, ballSeen;
|
||||
bool mate,
|
||||
ATKgoal, DEFgoal,
|
||||
atkSeen, defSeen, bSeen, ySeen,
|
||||
ballSeen;
|
||||
}data;
|
||||
|
||||
sv_extr input inputs[dim];
|
||||
|
|
|
|||
|
|
@ -1,6 +1,8 @@
|
|||
#pragma once
|
||||
#define DEBUG Serial3
|
||||
|
||||
#define GLOBAL_SPD_MULT 1.0
|
||||
|
||||
#define LED_R 20
|
||||
#define LED_Y 17
|
||||
#define LED_G 13
|
||||
|
|
|
|||
|
|
@ -78,29 +78,39 @@ void DataSourceCameraConic :: readSensor(){
|
|||
CURRENT_DATA_WRITE.bAngleFix = bAngleFix;
|
||||
CURRENT_DATA_WRITE.yDist = yDist;
|
||||
CURRENT_DATA_WRITE.bDist = bDist;
|
||||
|
||||
if(xb == unkn || yb == unkn) CURRENT_DATA_WRITE.bSeen = false;
|
||||
else CURRENT_DATA_WRITE.bSeen = true;
|
||||
if(xy == unkn || yy == unkn) CURRENT_DATA_WRITE.ySeen = false;
|
||||
else CURRENT_DATA_WRITE.ySeen = true;
|
||||
|
||||
if(goalOrientation == HIGH){
|
||||
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle;
|
||||
CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
|
||||
CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.ySeen;
|
||||
CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.bAngle;
|
||||
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix;
|
||||
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.bSeen;
|
||||
}else{
|
||||
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle;
|
||||
CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
|
||||
CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.bSeen;
|
||||
CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle;
|
||||
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
|
||||
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// int DataSourceCameraConic::getValueAtk(bool fixed){
|
||||
// int DataSource<CameraConic::getValueAtk(bool fixed){
|
||||
// if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix;
|
||||
// else return goalOrientation == HIGH ? yAngle : bAngle;
|
||||
// }
|
||||
// int DataSourceCameraConic::getValueDef(bool fixed){
|
||||
// if(fixed) return goalOrientation == LOW ? yAngleFix : bAngleFix;
|
||||
// else return goalOrientation == LOW ? yAngle : bAngle;
|
||||
// }
|
||||
// }>
|
||||
|
||||
void DataSourceCameraConic::test(){
|
||||
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu
|
||||
|
|
|
|||
|
|
@ -63,6 +63,9 @@ float DriveController::torad(float f){
|
|||
|
||||
void DriveController::drive(int dir, int speed, int tilt){
|
||||
|
||||
speed = (speed * KSPD + oldSpeed * (1-KSPD))*GLOBAL_SPD_MULT;
|
||||
tilt = tilt > 180 ? tilt - 360 : tilt;
|
||||
|
||||
vx = ((speed * cosins[dir]));
|
||||
vy = ((-speed * sins[dir]));
|
||||
|
||||
|
|
@ -82,11 +85,11 @@ void DriveController::drive(int dir, int speed, int tilt){
|
|||
speed4 = -(speed2);
|
||||
|
||||
// calcola l'errore di posizione rispetto allo 0
|
||||
delta = compass->getValue();
|
||||
delta = CURRENT_DATA_READ.IMUAngle;
|
||||
if(delta > 180) delta = delta - 360;
|
||||
|
||||
input = delta;
|
||||
setpoint = 0;
|
||||
setpoint = tilt;
|
||||
|
||||
pid->Compute();
|
||||
|
||||
|
|
@ -106,6 +109,8 @@ void DriveController::drive(int dir, int speed, int tilt){
|
|||
m3->drive((int) speed3);
|
||||
m4->drive((int) speed4);
|
||||
|
||||
oldSpeed = speed;
|
||||
|
||||
CURRENT_DATA_WRITE.dir = dir;
|
||||
CURRENT_DATA_WRITE.speed = speed;
|
||||
CURRENT_DATA_WRITE.tilt = tilt;
|
||||
|
|
|
|||
|
|
@ -3,11 +3,12 @@
|
|||
#include "games.h"
|
||||
#include "linesys_2019.h"
|
||||
#include "positionsys_zone.h"
|
||||
#include "positionsys_camera.h"
|
||||
|
||||
void initGames(){
|
||||
vector<DataSource*> lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) };
|
||||
vector<DataSource*> lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) };
|
||||
|
||||
goalie = new Goalie(new LineSys2019(lIn, lOut), new PositionSysZone());
|
||||
keeper = new Keeper(new LineSys2019(lOut, lOut), new PositionSysZone());
|
||||
goalie = new Goalie(new LineSys2019(lIn, lOut), new PositionSysCamera());
|
||||
keeper = new Keeper(new LineSys2019(lOut, lOut), new PositionSysCamera());
|
||||
}
|
||||
|
|
@ -3,7 +3,7 @@
|
|||
#include "vars.h"
|
||||
#include "status_vector.h"
|
||||
#include "math.h"
|
||||
#include "positionsys_zone.h"
|
||||
#include "positionsys_camera.h"
|
||||
|
||||
Goalie::Goalie() : Game() {
|
||||
init();
|
||||
|
|
@ -20,15 +20,15 @@ void Goalie::init(){
|
|||
}
|
||||
|
||||
void Goalie::realPlay(){
|
||||
if(ball->ballSeen) this->goalie(45);
|
||||
else ((PositionSysZone*)ps)->goCenter();
|
||||
if(ball->ballSeen) this->goalie(50);
|
||||
else ((PositionSysCamera*)ps)->goCenter();
|
||||
}
|
||||
|
||||
int dir, degrees2;
|
||||
void Goalie::goalie(int plusang) {
|
||||
if(ball->distance < 160) drive->prepareDrive(ball->angle, 350, 0);
|
||||
if(ball->distance < CATCH_DIST) drive->prepareDrive(ball->angle, 350, 0);
|
||||
else{
|
||||
if(ball->angle > 340 || ball->angle < 20) plusang *= 0.15;
|
||||
if(ball->angle > 345 || ball->angle < 15) plusang *= 0.15;
|
||||
if(ball->angle > 180) degrees2 = ball->angle - 360;
|
||||
else degrees2 = ball->angle;
|
||||
|
||||
|
|
@ -39,7 +39,7 @@ void Goalie::goalie(int plusang) {
|
|||
else dir = dir;
|
||||
|
||||
storcimentoPorta();
|
||||
if(ball->distance > 185 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc);
|
||||
if(ball->distance > TILT_DIST && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc);
|
||||
else {
|
||||
drive->prepareDrive(dir, 350, 0);
|
||||
cstorc = 0;
|
||||
|
|
@ -48,8 +48,8 @@ void Goalie::goalie(int plusang) {
|
|||
}
|
||||
|
||||
void Goalie::storcimentoPorta() {
|
||||
if (CURRENT_DATA_READ.angleAtkFix >= 10 && CURRENT_DATA_READ.angleAtkFix <= 90) cstorc+=9;
|
||||
else if (CURRENT_DATA_READ.angleAtkFix <= -10 && CURRENT_DATA_READ.angleAtkFix >= -90) cstorc-=9;
|
||||
// else cstorc *= 0.7;
|
||||
if (CURRENT_DATA_READ.angleAtkFix >= 5 && CURRENT_DATA_READ.angleAtkFix <= 60) cstorc+=9;
|
||||
else if (CURRENT_DATA_READ.angleAtkFix <= 355 && CURRENT_DATA_READ.angleAtkFix >= 210) cstorc-=9;
|
||||
else cstorc *= 0.9;
|
||||
cstorc = constrain(cstorc, -45, 45);
|
||||
}
|
||||
|
|
@ -24,7 +24,7 @@ void loop() {
|
|||
keeper->play(role==0);
|
||||
|
||||
// Last thing to do: movement and update status vector
|
||||
drive->prepareDrive(0,0,0);
|
||||
// drive->prepareDrive(0,0, CURRENT_DATA_READ.angleAtkFix);
|
||||
drive->drivePrepared();
|
||||
updateStatusVector();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,43 @@
|
|||
#include "positionsys_camera.h"
|
||||
#include "status_vector.h"
|
||||
#include "vars.h"
|
||||
#include "sensors.h"
|
||||
|
||||
PositionSysCamera::PositionSysCamera() {}
|
||||
|
||||
void PositionSysCamera::update(){
|
||||
}
|
||||
|
||||
void PositionSysCamera::test(){
|
||||
}
|
||||
|
||||
void PositionSysCamera::goCenter(){
|
||||
/*WORKS BUT CAN BE BETTER*/
|
||||
//Y
|
||||
if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
|
||||
else if ((camera->true_yb + camera->true_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
|
||||
//X
|
||||
else if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
|
||||
else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
|
||||
else drive->prepareDrive(0, 0, 0);
|
||||
|
||||
|
||||
/*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/
|
||||
// int x = 1;
|
||||
// int y = 1;
|
||||
|
||||
// //Trying using an angle
|
||||
// if((CURRENT_DATA_READ.yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.yb + CURRENT_DATA_READ.yy) < -CAMERA_CENTER_Y) y = CURRENT_DATA_READ.yb + CURRENT_DATA_READ.yy;
|
||||
// if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.xb;
|
||||
// if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.xy;
|
||||
|
||||
// DEBUG.print(x);
|
||||
// DEBUG.print(":");
|
||||
// DEBUG.println(y);
|
||||
|
||||
// int dir = 90-(atan2(y,x)*180/3.14);
|
||||
// dir = (dir+360) % 360;
|
||||
// DEBUG.println(dir);
|
||||
// drive->prepareDrive(dir, 100, 0);
|
||||
|
||||
}
|
||||
|
|
@ -374,14 +374,6 @@ void PositionSysZone::testLogicZone(){
|
|||
|
||||
|
||||
void PositionSysZone::goCenter() {
|
||||
// if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
|
||||
// else if ((camera->true_yb + camera->true_yy) < CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
|
||||
// else drive->prepareDrive(0, 0, 0);
|
||||
/* if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
|
||||
else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
|
||||
else drive->prepareDrive(0, 0, 0); */
|
||||
/*
|
||||
PREVIOUS
|
||||
if (zoneIndex == 8)
|
||||
drive->prepareDrive(330, GOCENTER_VEL);
|
||||
if (zoneIndex == 7)
|
||||
|
|
@ -399,7 +391,7 @@ void PositionSysZone::goCenter() {
|
|||
if (zoneIndex == 1)
|
||||
drive->prepareDrive(180, GOCENTER_VEL);
|
||||
if (zoneIndex == 0)
|
||||
drive->prepareDrive(135, GOCENTER_VEL); */
|
||||
drive->prepareDrive(135, GOCENTER_VEL);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ void initSensors(){
|
|||
drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
|
||||
compass = new DataSourceBNO055();
|
||||
ball = new DataSourceBall(&Serial4, 57600);
|
||||
camera = new DataSourceCameraVShaped(&Serial2, 19200);
|
||||
camera = new DataSourceCameraConic(&Serial2, 19200);
|
||||
usCtrl = new DataSourceCtrl(dUs);
|
||||
bt = new DataSourceBT(&Serial3, 115200);
|
||||
}
|
||||
|
|
@ -26,6 +26,6 @@ void updateSensors(){
|
|||
|
||||
compass->update();
|
||||
ball->update();
|
||||
// camera->update();
|
||||
camera->update();
|
||||
usCtrl->update();
|
||||
}
|
||||
|
|
@ -39,8 +39,8 @@ blue_led.on()
|
|||
|
||||
|
||||
|
||||
thresholds = [ (26, 74, -11, 6, 17, 50), # thresholds yellow goal
|
||||
(12, 44, -34, 42, -105, -25)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
|
||||
thresholds = [ (40, 100, -14, 21, 16, 69), # thresholds yellow goal
|
||||
(14, 46, -11, 12, -47, -19)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
|
||||
|
||||
roi = (0, 6, 318, 152)
|
||||
|
||||
|
|
@ -62,9 +62,9 @@ sensor.set_pixformat(sensor.RGB565)
|
|||
sensor.set_framesize(sensor.QQVGA)
|
||||
sensor.set_contrast(+3)
|
||||
sensor.set_saturation(0)
|
||||
sensor.set_brightness(-2)
|
||||
sensor.set_brightness(-1)
|
||||
sensor.set_quality(0)
|
||||
sensor.set_auto_exposure(False, 10000)
|
||||
sensor.set_auto_exposure(False, 3000)
|
||||
sensor.set_auto_gain(True)
|
||||
sensor.skip_frames(time = 300)
|
||||
|
||||
|
|
@ -84,7 +84,7 @@ while(True):
|
|||
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=75, area_threshold=130, merge = True):
|
||||
for blob in img.find_blobs(thresholds, pixels_threshold=30, area_threshold=70, merge = True):
|
||||
img.draw_rectangle(blob.rect())
|
||||
img.draw_cross(blob.cx(), blob.cy())
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,161 @@
|
|||
# goal dist tracking with conic mirror - By: EmaMaker - fri 21 feb 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)
|
||||
|
||||
def torad(f):
|
||||
return (f*math.pi/180) % math.pi
|
||||
|
||||
#These measures are in centimeters
|
||||
FIELD_W = 131
|
||||
FIELD_H = 193
|
||||
GOALS_DEPTH = 207
|
||||
|
||||
#Attack 1 means attacking yellow, attack 0 means attacking blue
|
||||
ATTACKING = 0
|
||||
|
||||
# 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 = [ (30, 100, 15, 127, 15, 127), # generic_red_thresholds
|
||||
# (30, 100, -64, -8, -32, 32), # generic_green_thresholds
|
||||
# (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds
|
||||
|
||||
#thresholds = [ (54, 93, -10, 25, 55, 70), # thresholds yellow goal
|
||||
# (30, 45, 1, 40, -60, -19)] # thresholds blue goal
|
||||
#
|
||||
thresholds = [ (40, 100, -3, 35, 16, 96) , # thresholds yellow goal
|
||||
(39, 59, -13, 12, -43, -19)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
|
||||
|
||||
roi = (0, 6, 318, 152)
|
||||
|
||||
# Camera Setup ###############################################################
|
||||
'''sensor.reset()
|
||||
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)
|
||||
#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.QQVGA)
|
||||
sensor.set_contrast(+2)
|
||||
sensor.set_saturation(2)
|
||||
sensor.set_brightness(-3)
|
||||
sensor.set_quality(0)
|
||||
sensor.set_auto_exposure(False, 6000)
|
||||
sensor.set_auto_gain(True)
|
||||
sensor.skip_frames(time = 300)
|
||||
|
||||
clock = time.clock()
|
||||
##############################################################################
|
||||
|
||||
|
||||
# [] list
|
||||
# () tupla
|
||||
|
||||
'''while(True):
|
||||
clock.tick()
|
||||
img = sensor.snapshot()'''
|
||||
|
||||
while(True):
|
||||
clock.tick()
|
||||
|
||||
blue_led.off()
|
||||
|
||||
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=45, area_threshold=80, 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() ) ]
|
||||
if (blob.code() == 2):
|
||||
tt_blue = tt_blue + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
|
||||
|
||||
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)
|
||||
|
||||
'''Yellow'''
|
||||
area,cx,cy,code = tt_yellow[ny-1]
|
||||
cx = img.width() / 2 - cx
|
||||
cy = img.height() / 2 - cy
|
||||
yAngle = math.pi/2 - math.atan2(cy, cx)
|
||||
yDist = math.sqrt(cx*cx + cy*cy)
|
||||
string_yellow = "Y"+str(cx)+" | "+str(cy)+" | "+str(yAngle)+" | "+str(yDist)+str(area)+"y"
|
||||
#print (string_yellow) # test on serial terminal
|
||||
|
||||
'''Blue'''
|
||||
area,cx,cy,code = tt_blue[nb-1]
|
||||
cx = img.width() / 2 - cx
|
||||
cy = img.height() / 2 - cy
|
||||
bAngle = math.pi/2 - math.atan2(cy, cx)
|
||||
bDist = math.sqrt(cx*cx + cy*cy)
|
||||
string_blue = "B"+str(cx)+" | "+str(cy)+" | |"+str(bAngle)+" | "+str(bDist)+str(area)+"b"
|
||||
#print (string_blue) # test on serial terminal
|
||||
|
||||
#Now calculate distance and position
|
||||
#Goal 1 is the one in front of the robot
|
||||
#Goal 2 is the one facing the back of the robot
|
||||
|
||||
#convert in [0, 360), to be sure
|
||||
bAngle = int(bAngle * 180 / math.pi)
|
||||
yAngle = int(yAngle * 180 / math.pi)
|
||||
|
||||
bAngle = (bAngle + 360) % 360;
|
||||
yAngle = (yAngle + 360) % 360;
|
||||
|
||||
#Now bring it back to [-179, 180]
|
||||
if bAngle > 180:
|
||||
bAngle = bAngle - 360
|
||||
|
||||
if yAngle > 180:
|
||||
yAngle = yAngle - 360
|
||||
|
||||
if ATTACKING == 1:
|
||||
angle1 = abs(yAngle)
|
||||
angle2 = abs(bAngle - 180)
|
||||
else:
|
||||
angle1 = abs(bAngle)
|
||||
angle2 = abs(yAngle - 180)
|
||||
|
||||
dist1 = (GOALS_DEPTH * math.sin(angle2) ) / (math.sin(angle1+angle2))
|
||||
dist2 = (GOALS_DEPTH * math.sin(angle1) ) / (math.sin(angle1+angle2))
|
||||
|
||||
print("------")
|
||||
print(angle1)
|
||||
print(angle2)
|
||||
print(dist1)
|
||||
print(dist2)
|
||||
print("------")
|
||||
|
||||
|
||||
#print ("..................................")
|
||||
|
||||
print(clock.fps())
|
||||
|
|
@ -29,8 +29,8 @@ blue_led.on()
|
|||
#thresholds = [ (54, 93, -10, 25, 55, 70), # thresholds yellow goal
|
||||
# (30, 45, 1, 40, -60, -19)] # thresholds blue goal
|
||||
#
|
||||
thresholds = [ (30, 70, -12, 19, 41, 68) , # thresholds yellow goal
|
||||
(0, 70, -2, 34, -59, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
|
||||
thresholds = [ (57, 93, -18, 14, 28, 77) , # thresholds yellow goal
|
||||
(31, 68, -20, 18, -47, -17)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
|
||||
|
||||
roi = (0, 6, 318, 152)
|
||||
|
||||
|
|
@ -79,7 +79,7 @@ while(True):
|
|||
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=100, area_threshold=150, merge = True):
|
||||
for blob in img.find_blobs(thresholds, pixels_threshold=70, area_threshold=100, merge = True):
|
||||
img.draw_rectangle(blob.rect())
|
||||
img.draw_cross(blob.cx(), blob.cy())
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue