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

strategy: introduce a test striker with empty systems and set as only role

pull/1/head
EmaMaker 2020-12-23 21:25:52 +01:00
parent f4863eab06
commit 62821341b0
12 changed files with 217 additions and 153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/strategy_roles/games.h
View File

@ -9,9 +9,11 @@
#include <Arduino.h>
#include "strategy_roles/game.h"
#include "strategy_roles/striker.h"
#include "strategy_roles/striker_test.h"
#include "strategy_roles/keeper.h"
void initGames();
g_extr Game* striker;
g_extr Game* striker_test;
g_extr Game* keeper;

36
include/strategy_roles/striker_test.h Normal file
View File

@ -0,0 +1,36 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define TILT_MULT 1.8
#define TILT_DIST 170
#define CATCH_DIST 150
#define GOALIE_ATKSPD_LAT 120 //255
#define GOALIE_ATKSPD_BAK 120
#define GOALIE_ATKSPD_FRT 120
#define GOALIE_ATKSPD_STRK 120
#define GOALIE_ATKDIR_PLUSANG1 20
#define GOALIE_ATKDIR_PLUSANG2 35
#define GOALIE_ATKDIR_PLUSANG3 40
#define GOALIE_ATKDIR_PLUSANGBAK 40
#define GOALIE_ATKDIR_PLUSANG1_COR 60
#define GOALIE_ATKDIR_PLUSANG2_COR 70
#define GOALIE_ATKDIR_PLUSANG3_COR 70
class StrikerTest : public Game{
public:
StrikerTest();
StrikerTest(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void striker();
void ballBack();
int atk_speed, atk_direction;
};

2
include/systems/systems.h
View File

@ -17,12 +17,14 @@ class PositionSystem{
class LineSystemEmpty : public LineSystem{
public:
LineSystemEmpty();
void update() override;
void test() override;
};
class PositionSystemEmpty : public PositionSystem{
public:
PositionSystemEmpty();
void update() override;
void test() override;
void goCenter() override;

9
src/main.cpp
View File

@ -25,6 +25,8 @@ void setup() {
initGames();
delay(500);
drive->prepareDrive(0,0,0);
}
@ -32,8 +34,11 @@ void loop() {
updateSensors();
if(DEBUG.available()) testmenu->testMenu();
striker->play(role==1);
keeper->play(role==0);
striker_test->play(1);
// striker->play(role==1);
// keeper->play(role==0);
// drive->prepareDrive(0,0,0);
// Last thing to do: movement and update status vector
drive->drivePrepared();

2
src/sensors/sensors.cpp
View File

@ -9,7 +9,7 @@ void initSensors(){
pinMode(LED_Y, OUTPUT);
pinMode(LED_G, OUTPUT);
drive = new DriveController(new Motor(11, 12, 4, 35), new Motor(24, 25, 5, 135), new Motor(26, 27, 2, 225), new Motor(28, 29, 3, 325));
drive = new DriveController(new Motor(12, 11, 4, 55), new Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305));
//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(&Serial2, 57600);

1
src/strategy_roles/game.cpp
View File

@ -17,6 +17,5 @@ void Game::play(bool condition){
CURRENT_DATA_WRITE.posSystem = (this->ps);
CURRENT_DATA_WRITE.lineSystem = (this->ls);
CURRENT_DATA_WRITE.game = this;
((PositionSysCamera*)ps)->CameraPID();
}
}

2
src/strategy_roles/games.cpp
View File

@ -2,6 +2,7 @@
/* #include "sensors/linesys_2019.h" */
#include "systems/lines/linesys_camera.h"
#include "systems/systems.h"
#include "systems/position/positionsys_zone.h"
#include "systems/position/positionsys_camera.h"
#include "strategy_roles/games.h"
@ -10,6 +11,7 @@ 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) };
striker_test = new StrikerTest(new LineSystemEmpty(), new PositionSystemEmpty());
striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera());
keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
}

82
src/strategy_roles/striker_test.cpp Normal file
View File

@ -0,0 +1,82 @@
#include "behaviour_control/status_vector.h"
#include "sensors/sensors.h"
#include "strategy_roles/striker_test.h"
#include "vars.h"
#include "math.h"
StrikerTest::StrikerTest() : Game() {
init();
}
StrikerTest::StrikerTest(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) {
init();
}
void StrikerTest::init(){
atk_speed = 0;
atk_direction = 0;
}
void StrikerTest::realPlay(){
if(CURRENT_DATA_READ.ballSeen) striker();
else drive->prepareDrive(0,0,0);
}
void StrikerTest::striker() {
if(CURRENT_DATA_READ.ballAngle>= 350 || CURRENT_DATA_READ.ballAngle<= 20) {
if(CURRENT_DATA_READ.ballDistance > 190) atk_direction = 0;
else atk_direction = CURRENT_DATA_READ.ballAngle;
atk_speed = GOALIE_ATKSPD_FRT;
}
if(CURRENT_DATA_READ.ballAngle>= 90 && CURRENT_DATA_READ.ballAngle<= 270) {
int ball_degrees2;
int dir;
int plusang;
if(CURRENT_DATA_READ.ballDistance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK;
else plusang = 0;
if(CURRENT_DATA_READ.ballAngle> 180) ball_degrees2 = CURRENT_DATA_READ.ballAngle- 360;
else ball_degrees2 = CURRENT_DATA_READ.ballAngle;
if(ball_degrees2 > 0) dir = CURRENT_DATA_READ.ballAngle+ plusang; //45 con 8 ruote
else dir = CURRENT_DATA_READ.ballAngle- plusang; //45 con 8 ruote
if(dir < 0) dir = dir + 360;
else dir = dir;
atk_direction = dir;
atk_speed = GOALIE_ATKSPD_BAK;
}
if(CURRENT_DATA_READ.ballAngle> 10 && CURRENT_DATA_READ.ballAngle< 30) {
atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG1;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(CURRENT_DATA_READ.ballAngle>= 30 && CURRENT_DATA_READ.ballAngle< 45) {
atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG2;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(CURRENT_DATA_READ.ballAngle>= 45 && CURRENT_DATA_READ.ballAngle< 90) {
atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG3;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(CURRENT_DATA_READ.ballAngle> 270 && CURRENT_DATA_READ.ballAngle<= 315) {
atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG3_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(CURRENT_DATA_READ.ballAngle> 315 && CURRENT_DATA_READ.ballAngle<= 330) {
atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG2_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(CURRENT_DATA_READ.ballAngle> 330 && CURRENT_DATA_READ.ballAngle< 350) {
atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG1_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
drive->prepareDrive(atk_direction, atk_speed);
}
void StrikerTest::ballBack() {
}

3
src/system/lines/linesys_empty.cpp
View File

@ -3,6 +3,9 @@
#include "sensors/sensors.h"
#include <Arduino.h>
LineSystemEmpty::LineSystemEmpty(){
}
void LineSystemEmpty::update(){
tookLine = false;
}

4
src/system/positions/positionsys_empty.cpp
View File

@ -3,6 +3,10 @@
#include "sensors/sensors.h"
#include <Arduino.h>
PositionSystemEmpty::PositionSystemEmpty(){
}
void PositionSystemEmpty::update(){
}

149
utility/OpenMV/conic_eff.py.autosave
View File

@ -1,149 +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)
# 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 = [ (71, 100, -24, 12, 57, 99), # thresholds yellow goal
(38, 55, -33, -1, 0, 26)] # 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) 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.QQVGA)
sensor.set_contrast(3)
sensor.set_saturation(3)
sensor.set_brightness(0)
sensor.set_quality(0)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False, 5500)
sensor.set_auto_gain(True)
sensor.skip_frames(time = 300)
clock = time.clock()
##############################################################################
while(True):
clock.tick()
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=50, area_threshold=50, 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)
y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1]
b_area, b1_cx, b1_cy, b_code = tt_blue[nb-1]
y_cx = int(img.width() / 2 - y1_cx)
y_cy = int(img.height() / 2 - y1_cy)
b_cx = int(img.width() / 2 - b1_cx)
b_cy = int(img.height() / 2 - b1_cy)
#Normalize data between 0 and 100
if y_found == True:
y_cx = val_map(y_cx, -img.width() / 2, img.width() / 2, 100, 0)
y_cy = val_map(y_cy, -img.height() / 2, img.height() / 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
if b_found == True:
b_cx = val_map(b_cx, -img.width() / 2, img.width() / 2, 100, 0)
b_cy = val_map(b_cy, -img.height() / 2, img.height() / 2, 0, 100)
#Prepare for send as a list of characters
s_bcx = chr(b_cx)
s_bcy = chr(b_cy)
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)

78
utility/OpenMV/test.py Normal file
View File

@ -0,0 +1,78 @@
# 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)
# 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 = [ (71, 100, -24, 12, 57, 99), # thresholds yellow goal
(38, 55, -33, -1, 0, 26)] # 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) 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.QQVGA)
sensor.set_contrast(3)
sensor.set_saturation(3)
sensor.set_brightness(0)
sensor.set_quality(0)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False, 5500)
sensor.set_auto_gain(True)
sensor.skip_frames(time = 300)
clock = time.clock()
##############################################################################
while(True):
clock.tick()
uart.write(42)