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

not working

code_midgen
u-siri-ous 2020-02-12 18:53:25 +01:00
parent fc840b84be
commit 5a9ce6692f
8 changed files with 24 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/goalie.h
View File

@ -16,7 +16,7 @@
#define GOALIE_ATKDIR_PLUSANG2_COR 70
#define GOALIE_ATKDIR_PLUSANG3_COR 70
class Goalie : public Game{
class Goalie : public Game, public PositionSysZone{
public:
Goalie();

3
include/positionsys_zone.h
View File

@ -60,6 +60,7 @@ class PositionSysZone : public PositionSystem{
PositionSysZone();
void update() override;
void test() override;
void goCenter();
//returns the zone calculated
int getValue();
@ -110,7 +111,7 @@ class PositionSysZone : public PositionSystem{
void testLogicZone();
//movement
void goCenter();
//void goCenter();
void goGoalPost();
void centerGoalPost();
void centerGoalPostCamera(bool);

2
src/drivecontroller.cpp
View File

@ -31,7 +31,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
setpoint = 0;
pid->SetMode(AUTOMATIC);
pid->SetSampleTime(2);
pid->SetSampleTime(5);
canUnlock = true;
unlockTime = 0;

11
src/goalie.cpp
View File

@ -1,6 +1,7 @@
#include "goalie.h"
#include "sensors.h"
#include "vars.h"
#include "positionsys_zone.h"
Goalie::Goalie() : Game() {
init();
@ -17,7 +18,7 @@ void Goalie::init(){
}
void Goalie::realPlay(){
if(ball->ballSeen) this->goalie(50);
if(ball->ballSeen) this->goCenter();
else drive->prepareDrive(0,0,0);
}
@ -36,11 +37,11 @@ void Goalie::goalie(int plusang) {
ball->b = ball->dir;
storcimentoPorta();
if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 350, cstorc);
else {
//if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 350, cstorc);
//else {
drive->prepareDrive(ball->dir, 350, 0);
cstorc = 0;
}
// cstorc = 0;
//}
}
}

12
src/positionsys_zone.cpp
View File

@ -373,7 +373,15 @@ void PositionSysZone::testLogicZone(){
void PositionSysZone::goCenter() {
if (zoneIndex == 8)
if((camera->true_yb + camera->true_yy) >= 3 && (camera->true_yb + camera->true_yy) <= -3) {
//dx o sx
if((camera->true_yb + camera->true_yy) < 0) drive->prepareDrive(0, 200, 0);
else if ((camera->true_yb + camera->true_yy) > 0) drive->prepareDrive(270, 200, 0);
}
if(camera->true_xb >= -10 && camera->true_xb <= 10);
else if(camera->true_xb >= -5 && camera->true_xb <= 35) drive->prepareDrive(90, 200, 0);
else if(camera->true_xb >= 5 && camera->true_xb <= -35) drive->prepareDrive(180, 200, 0);
/* if (zoneIndex == 8)
drive->prepareDrive(330, GOCENTER_VEL);
if (zoneIndex == 7)
drive->prepareDrive(0, GOCENTER_VEL);
@ -390,7 +398,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); */
}

4
utility/OpenMV/conic_eff.py
View File

@ -40,7 +40,7 @@ blue_led.on()
thresholds = [ (30, 70, -12, 19, 10, 57), # thresholds yellow goal
(0, 31, -2, 39, -68, -25)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
(0, 44, -5, 42, -65, -13)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152)
@ -62,7 +62,7 @@ sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_contrast(+3)
sensor.set_saturation(0)
sensor.set_brightness(0)
sensor.set_brightness(-2)
sensor.set_quality(0)
sensor.set_auto_exposure(False, 10000)
sensor.set_auto_gain(True)

13
utility/OpenMV/main_test_conic.py
View File

@ -29,13 +29,8 @@ blue_led.on()
#thresholds = [ (54, 93, -10, 25, 55, 70), # thresholds yellow goal
# (30, 45, 1, 40, -60, -19)] # thresholds blue goal
#
<<<<<<< HEAD
thresholds = [ (49, 84, -8, 31, 20, 80), # thresholds yellow goal
(0, 51, -4, 44, -59, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
=======
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)
>>>>>>> 3c09f031f1833bf48d5dc14c166307217dee7fcf
roi = (0, 6, 318, 152)
@ -57,11 +52,7 @@ sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_contrast(+2)
sensor.set_saturation(+1)
<<<<<<< HEAD
sensor.set_brightness(-3)
=======
sensor.set_brightness(-2)
>>>>>>> 3c09f031f1833bf48d5dc14c166307217dee7fcf
sensor.set_quality(0)
sensor.set_auto_exposure(False, 6000)
sensor.set_auto_gain(True)
@ -84,11 +75,11 @@ while(True):
blue_led.off()
tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
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=150, area_threshold=150, merge = True):
for blob in img.find_blobs(thresholds, pixels_threshold=100, area_threshold=150, merge = True):
img.draw_rectangle(blob.rect())
img.draw_cross(blob.cx(), blob.cy())

117
utility/OpenMV/main_test_conic.py.autosave
View File

@ -1,117 +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)
# 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 = [ (30, 70, -12, 19, 41, 68) , # thresholds yellow goal
(0, 70, -2, 34, -59, -21)] # 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(+1)
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=100, area_threshold=150, 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] # coordinata x del piu' grande y se montata al contrario
cx = img.width() / 2 - cx
cy = img.height() / 2 - cy
angle = math.pi/2 - math.atan2(cy, cx)
dist = math.sqrt(cx*cx + cy*cy)
string_yellow = "Y"+str(cx)+" | "+str(cy)+" | "+str(angle)+" | "+str(dist)+str(area)+"y"
print (string_yellow) # test on serial terminal
'''Blue'''
area,cx,cy,code = tt_blue[nb-1] # coordinata x del piu' grande y se montata al contrario
cx = img.width() / 2 - cx
cy = img.height() / 2 - cy
angle = math.pi/2 - math.atan2(cy, cx)
dist = math.sqrt(cx*cx + cy*cy)
string_blue = "B"+str(cx)+" | "+str(cy)+" | |"+str(angle)+" | "+str(dist)+str(area)+"b"
print (string_blue) # test on serial terminal
#print ("..................................")
print(clock.fps())