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

superteam challenges: challenge 3: spinner

pull/1/head
EmaMaker 2021-06-23 23:31:20 +02:00
parent e8f45faff6
commit 0fceb85428
11 changed files with 137 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/motors_movement/drivecontroller.h
View File

@ -20,7 +20,9 @@
//Max possible vel 310
#define MAX_VEL 110
// #define MAX_POSSIBLE_VEL 310
#define MAX_POSSIBLE_VEL 280
#define MAX_VEL 100
#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)

6
include/sensors/data_source_camera_conicmirror.h
View File

@ -12,7 +12,7 @@
//Imu To Camera Angle Mult
#define IMUTOC_AMULT 1
#define FILTER_DEFAULT_COEFF 0.6
#define FILTER_DEFAULT_COEFF 0.25
#define FILTER_BY_COEFF FILTER_DEFAULT_COEFF
#define FILTER_BX_COEFF FILTER_DEFAULT_COEFF
#define FILTER_YY_COEFF FILTER_DEFAULT_COEFF
@ -27,8 +27,8 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/
// #define CAMERA_TRANSLATION_Y 7
//Robot with roller
#define CAMERA_TRANSLATION_X 0
#define CAMERA_TRANSLATION_Y 12
#define CAMERA_TRANSLATION_X -3
#define CAMERA_TRANSLATION_Y -3
class DataSourceCameraConic : public DataSource{

4
include/strategy_roles/games.h
View File

@ -13,6 +13,7 @@
#include "strategy_roles/precision_shooter.h"
#include "strategy_roles/pass_and_shoot.h"
#include "strategy_roles/spot_finder.h"
#include "strategy_roles/the_spinner.h"
// #include "strategy_roles/keeper.h"
void initGames();
@ -24,4 +25,5 @@ g_extr Game* pass_and_shoot;
// g_extr Game* keeper;
g_extr Game* tc1;
g_extr Game* tc2;
g_extr Game* st_tc1;
g_extr Game* st_tc3;

56
include/strategy_roles/the_spinner.h Normal file
View File

@ -0,0 +1,56 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
#define X_COORD 10
#define Y_COORD 15
class Spinner : public Game{
public:
Spinner();
Spinner(LineSystem* ls, PositionSystem* ps);
private:
void realPlay() override;
void init() override;
void circle();
bool doingCircle = false;
bool firstCircle = true;
bool flag = false;
unsigned long t =0;
int step = 0;
typedef struct v{
v(){
x = 0;
y = 0;
}
v(int x_, int y_){
x = x_;
y = y_;
}
int x, y;
} spot;
/*
spot(13, 15), //top right spot
spot(-13, 15), //top left spot
spot(-13, -15) //bottom left spot
spot(13, -15), //bottom right spot
*/
vector<spot> spots = {
/*START: centre*/
spot(0,0),
// /*1ST BOTTLE*/
spot(20, 15),
};
int current_spot_i = 0;
spot current_spot;
};

3
include/systems/position/positionsys_camera.h
View File

@ -20,7 +20,8 @@
//Actually it's ± MAX_VAL
#define MAX_X 50
#define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
#define DIST_MULT 5
#define MAX_DIST_EXPERIMENTAL 70
#define DIST_MULT 8
#define VICINITY_DIST_TRESH 2
#define ROUGH_VICINITY_DIST_TRESH 10

3
src/main.cpp
View File

@ -54,8 +54,7 @@ void loop() {
// keeper_condition = role == LOW;
if(robot_indentifier){
tc2->play(1);
// if(roller->roller_armed) roller->speed(roller->MAX);
striker->play(1);
// Last thing to do: movement and update status vector
drive->drivePrepared();
}else{

3
src/strategy_roles/games.cpp
View File

@ -17,6 +17,7 @@ void initGames(){
precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera());
striker_roller = new StrikerRoller(new LineSysCamera(lIn, lOut), new PositionSysCamera());
tc1 = new StrikerRoller(new LineSystemEmpty(), new PositionSysCamera());
tc2 = new SpotFinder(new LineSystemEmpty(), new PositionSysCamera());
st_tc1 = new SpotFinder(new LineSystemEmpty(), new PositionSysCamera());
st_tc3 = new Spinner(new LineSystemEmpty(), new PositionSysCamera());
// keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
}

3
src/strategy_roles/striker.cpp
View File

@ -72,7 +72,8 @@ void Striker::striker(){
else dir = ball_deg - plusang; //se sto nel negativo sottraggo
dir = (dir + 360) % 360;
drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
// drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
drive->prepareDrive(dir, MAX_VEL_HALF, 0);
if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
else roller->speed(roller->MIN);

48
src/strategy_roles/the_spinner.cpp Normal file
View File

@ -0,0 +1,48 @@
#include "strategy_roles/the_spinner.h"
#include "systems/position/positionsys_camera.h"
#include "vars.h"
#include "math.h"
Spinner::Spinner() : Game() {}
Spinner::Spinner(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) {}
void Spinner::init() {}
void Spinner::realPlay() {
if(current_spot_i >= spots.size()) return;
current_spot = spots[current_spot_i];
if(doingCircle){
circle();
return;
}
if (((PositionSysCamera*)ps)->isInTheVicinityOf(current_spot.x, current_spot.y)) {
if(current_spot.x == 0 && current_spot.y == 0) {
current_spot_i++;
return;
}
doingCircle = true;
firstCircle = true;
step = 0;
}
else (((PositionSysCamera*)ps)->setMoveSetpoints(current_spot.x, current_spot.y));
}
void Spinner::circle(){
if(millis() - t < 1000){
if(step == 0) drive->prepareDrive(45, 50, 0);
else if(step == 1) drive->prepareDrive(315, 50, 0);
else if(step == 2) drive->prepareDrive(225, 50, 0);
else if(step == 3) {
if(firstCircle){
drive->prepareDrive(135, 80, 0);
firstCircle = false;
}else doingCircle = false;
}
}else {
step++;
t = millis();
}
}

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

@ -7,6 +7,7 @@
PositionSysCamera::PositionSysCamera() {
MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
Inputx = 0;
Outputx = 0;
Setpointx = 0;
@ -52,7 +53,7 @@ void PositionSysCamera::update(){
//IMPORTANT STEP: or the direction of the plane will be flipped
posx *= -1;
posy *= -1;
//Filtering error in calculation like this is a dirty hack, I know
if(posx < -MAX_X || posx > MAX_X || posy < -MAX_Y || 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
@ -150,11 +151,17 @@ void PositionSysCamera::CameraPID(){
int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
dir = (dir+360) % 360;
// int dist = sqrt( ( pow(CURRENT_DATA_WRITE.posx-Setpointx,2) ) + pow(CURRENT_DATA_WRITE.posy-Setpointy, 2) );
// int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL);
int speed = hypot(Outputx, Outputy) * DIST_MULT;
// speed = speed > 10 ? speed : 0;
dir = filterDir->calculate(dir);
float distance = hypot(Outputx, Outputy);
float speed = distance > 3 ? 20 + map(distance, 0, MAX_DIST_EXPERIMENTAL, 0, MAX_POSSIBLE_VEL) : 0;
// DEBUG.print("x: ");
// DEBUG.print(Outputx);
// DEBUG.print(" y:");
// DEBUG.print(Outputy);
// DEBUG.print(" Hypot:");
// DEBUG.print(hypot(Outputx, Outputy));
// DEBUG.print(" Speed:");
// DEBUG.println(speed);
#ifdef DRIVE_VECTOR_SUM
vx = ((speed * cosins[dir]));

8
utility/OpenMV/conic_eff_h7.py
View File

@ -45,8 +45,8 @@ blue_led.on()
##############################################################################
thresholds = [ (84, 100, -29, 1, 36, 127), # thresholds yellow goalz
(36, 100, -34, 12, -59, -19)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (68, 100, -16, 26, 24, 85), # thresholds yellow goalz
(36, 70, -3, 27, -75, -27)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (70, 0, 250, 200)
@ -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, -1.804))
sensor.set_auto_whitebal(True, (-6.02073, -5.119987, -1.006964))
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=80, area_threshold=100, merge = True):
for blob in img.find_blobs(thresholds, pixels_threshold=100, area_threshold=100, merge = True):
img.draw_rectangle(blob.rect())
#img.draw_cross(blob.cx(), blob.cy())