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

romecup: precision shooter

pull/1/head
EmaMaker 2021-05-13 08:38:20 +02:00
parent 3631fe18ab
commit 16a781df34
16 changed files with 153 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/motors_movement/drivecontroller.h
View File

@ -20,7 +20,7 @@
//Max possible vel 310
#define MAX_VEL 150
#define MAX_VEL 180
#define MAX_VEL_EIGTH ((int)MAX_VEL*0.8)
#define MAX_VEL_HALF ((int)MAX_VEL*0.5)
#define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75)

4
include/sensors/data_source_camera_conicmirror.h
View File

@ -21,8 +21,8 @@
/*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide
To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time.
These values need to be subtracted from the coords used in setMoveSetpoints*/
#define CAMERA_TRANSLATION_X 1
#define CAMERA_TRANSLATION_Y -4
#define CAMERA_TRANSLATION_X 0
#define CAMERA_TRANSLATION_Y 0
class DataSourceCameraConic : public DataSource{

4
include/strategy_roles/games.h
View File

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

28
include/strategy_roles/precision_shooter.h Normal file
View File

@ -0,0 +1,28 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define PS_ATTACK_DISTANCE 110
#define PS_TILT_STOP_DISTANCE 140
#define PS_PLUSANG 55
#define PS_PLUSANG_VISIONCONE 7
class PrecisionShooter : public Game{
public:
PrecisionShooter();
PrecisionShooter(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void striker();
int tilt();
int atk_speed, atk_direction, atk_tilt;
float cstorc;
bool gotta_tilt;
};

2
include/strategy_roles/striker.h
View File

@ -6,7 +6,7 @@
#define STRIKER_ATTACK_DISTANCE 110
#define STRIKER_TILT_STOP_DISTANCE 140
#define STRIKER_PLUSANG 55
#define STRIKER_PLUSANG 57
#define STRIKER_PLUSANG_VISIONCONE 7
class Striker : public Game{

26
include/strategy_roles/striker_test.h
View File

@ -1,26 +0,0 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define TARGET_DIST 120
#define ATTACK_DIST 150
#define ANGLE_SHIFT_STEP 5
#define STRIKER_SPD 80
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;
};

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

@ -7,14 +7,14 @@
#include "vars.h"
#define S1I A7
#define S1O A6
#define S2I A2
#define S2O A3
#define S3I A0
#define S3O A1
#define S4I A9
#define S4O A8
#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 325
#define EXIT_TIME 300

6
src/main.cpp
View File

@ -53,8 +53,10 @@ void loop() {
striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
keeper_condition = role == LOW;
striker->play(striker_condition);
keeper->play(keeper_condition);
// striker->play(striker_condition);
// keeper->play(keeper_condition);
precision_shooter->play(1);
testmenu->testMenu();
// Last thing to do: movement and update status vector

1
src/strategy_roles/games.cpp
View File

@ -13,5 +13,6 @@ void initGames(){
// striker_test = new StrikerTest(new LineSysCamera(lIn, lOut), new PositionSysCamera());
striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera());
precision_shooter = new PrecisionShooter(new LineSystemEmpty(), new PositionSysCamera());
keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
}

88
src/strategy_roles/precision_shooter.cpp Normal file
View File

@ -0,0 +1,88 @@
#include "behaviour_control/status_vector.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "sensors/data_source_ball.h"
#include "strategy_roles/precision_shooter.h"
#include "vars.h"
#include "math.h"
PrecisionShooter::PrecisionShooter() : Game()
{
init();
}
PrecisionShooter::PrecisionShooter(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
{
init();
}
void PrecisionShooter::init()
{
atk_speed = 0;
atk_direction = 0;
atk_tilt = 0;
cstorc = 0;
gotta_tilt = false;
}
void PrecisionShooter::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
this->striker();
else
ps->goCenter();
}
unsigned long t3 = 0;
void PrecisionShooter::striker(){
#ifdef MAX_VEL
#undef MAX_VEL
#define MAX_VEL 100
#endif
//seguo palla
int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = PS_PLUSANG;
if(ball_deg >= 346 || ball_deg <= 16) plusang = PS_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->speed(ROLLER_DEFAULT_SPEED);
else roller->speed(roller->MIN);
if(ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 85 && CURRENT_DATA_READ.posy >= 32 && (CURRENT_DATA_READ.posx >= 15 || CURRENT_DATA_READ.posx <= -15) ) {
t3 = millis();
}
if(millis() - t3 < 1000){
roller->speed(1800);
ps->goCenter();
}
}
int PrecisionShooter::tilt() {
if (ball->isInMouth() /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
else gotta_tilt = false;
if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {
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;
}

2
src/strategy_roles/striker.cpp
View File

@ -39,7 +39,7 @@ void Striker::striker(){
//seguo palla
int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG;
if(ball_deg >= 346 || 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 >= 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;

100
src/strategy_roles/striker_test.cpp
View File

@ -1,100 +0,0 @@
#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 ps->goCenter();
}
void StrikerTest::striker()
{
/*First implementation of "orbital striker", a new way to approach the problem with less lines.
It works with robot's positions, calculating the final one using the angle shift over
and over again until we have an acceptable result. That result is used to drive->prepareDrive with the speed.*/
// if (CURRENT_DATA_READ.ballDistance > )
// drive->prepareDrive(CURRENT_DATA_READ.ballAngle, STRIKER_SPD);
// else
// {
// if (CURRENT_DATA_READ.ballAngle > 340 && CURRENT_DATA_READ.ballAngle < 20)
// {
// drive->prepareDrive(0, 100, 0);
// }
// else
// {
// int ballAngle = (90 - CURRENT_DATA_READ.ballAngle + 360) % 360;
// robotAngle = (ballAngle - 180 + 360) % 360;
// DEBUG.print("Ball is at angle (goniometric circle): ");
// DEBUG.println(ballAngle);
// DEBUG.print("Robot is at angle ");
// DEBUG.println(robotAngle);
// float robotAngle_rad = robotAngle*3.14 / 180;
// robotX = CURRENT_DATA_READ.ballDistance * cos(robotAngle_rad);
// robotY = CURRENT_DATA_READ.ballDistance * sin(robotAngle_rad);
// DEBUG.print("Coords of the robot relative to the ball: (");
// DEBUG.print(robotX);
// DEBUG.print(", ");
// DEBUG.print(robotY);
// DEBUG.println(")");
// // angleDiff = min(((-robotAngle + 360) % 360), ((robotAngle + 360) % 360));
// // angleShift = min(angleDiff, ANGLE_SHIFT_STEP);
// angleShift = ANGLE_SHIFT_STEP;
// if (robotAngle >= 0 && robotAngle <= 180)
// newAngle = robotAngle - angleShift;
// else
// newAngle = robotAngle + angleShift;
// DEBUG.print("New ball-robot angle: ");
// DEBUG.println(newAngle);
// float newAngle_rad = (newAngle)*3.14 / 180;
// robotX_new = TARGET_DIST * cos(newAngle_rad);
// robotY_new = TARGET_DIST * sin(newAngle_rad);
// DEBUG.print("New coords of the robot relative to the ball: (");
// DEBUG.print(robotX_new);
// DEBUG.print(", ");
// DEBUG.print(robotY_new);
// DEBUG.println(")");
// moveAngle = (atan2((robotX_new - robotX), (robotY_new - robotY))) * 180 / 3.14;
// moveAngle = (moveAngle + 360) % 360;
// DEBUG.print("Direction to move in: ");
// DEBUG.println(moveAngle);
// drive->prepareDrive(moveAngle, STRIKER_SPD);
// }
// }
// delay(1000);
// DEBUG.println("==========");
}
void StrikerTest::ballBack() {
}

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

@ -60,7 +60,9 @@ void PositionSysCamera::update(){
//IMPORTANT STEP: or the direction of the plane will be flipped
posx *= -1;
posy *= -1;
}else{
}
if(abs(CURRENT_DATA_READ.posx-CURRENT_DATA_WRITE.posx)>MAX_X || abs(CURRENT_DATA_READ.posy-CURRENT_DATA_WRITE.posy)>MAX_Y|| (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false) ) {
// Go back in time until we found a valid status, when we saw at least one goal
int i = 1;
do{

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

@ -15,9 +15,9 @@ void PositionSystemEmpty::test(){
}
void PositionSystemEmpty::goCenter(){
drive->prepareDrive(0,0,0);
// drive->prepareDrive(0,0,0);
}
void PositionSystemEmpty::centerGoal(){
drive->prepareDrive(0,0,0);
// drive->prepareDrive(0,0,0);
}

10
utility/OpenMV/conic_eff_h7.py
View File

@ -45,11 +45,11 @@ blue_led.on()
##############################################################################
thresholds = [ (61, 100, -11, 18, 60, 127), # thresholds yellow goalz
(34, 52, -14, 21, -67, -20)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (66, 88, -12, 26, 50, 79), # thresholds yellow goalz
(40, 61, -9, 19, -61, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (80, 0, 220, 200)
roi = (80, 0, 240, 200)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
@ -71,7 +71,7 @@ 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, -0.707413))
sensor.set_auto_whitebal(True, (-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)
@ -94,7 +94,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=60, area_threshold=80, merge = True):
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())

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

@ -36,8 +36,7 @@ def isInRightSide(img, x):
# LED Setup ##################################################################
red_led = pyb.LED(1)
green_led
= pyb.LED(2)
green_led = pyb.LED(2)
blue_led = pyb.LED(3)
red_led.off()
@ -46,11 +45,11 @@ blue_led.on()
##############################################################################
thresholds = [ (61, 100, -11, 18, 60, 127), # thresholds yellow goalz
(34, 52, -14, 21, -67, -20)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (66, 88, -12, 26, 50, 79), # thresholds yellow goalz
(40, 61, -9, 19, -61, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (80, 0, 220, 200)
roi = (80, 0, 240, 200)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
@ -59,6 +58,7 @@ 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 )
@ -72,7 +72,7 @@ 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, -0.707413))
sensor.set_auto_whitebal(True, (-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)
@ -95,7 +95,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=60, area_threshold=80, merge = True):
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())