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

preparation for romecup: refine striker

pull/1/head
EmaMaker 2021-05-10 18:28:41 +02:00
parent 9321e6ff0b
commit 082e7effcf
12 changed files with 39 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/motors_movement/roller.h
View File

@ -2,7 +2,7 @@
#include "ESC.h"
#define ROLLER_DEFAULT_SPEED 1300
#define ROLLER_DEFAULT_SPEED 1100
class Roller{
public:

2
include/strategy_roles/striker.h
View File

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

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

@ -16,7 +16,7 @@
#define S4I A9
#define S4O A8
#define LINE_THRESH_CAM 350
#define LINE_THRESH_CAM 325
#define EXIT_TIME 250
#define LINES_EXIT_SPD 350

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

@ -8,7 +8,7 @@
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 4
#define CAMERA_TRANSLATION_Y 4
#define CAMERA_TRANSLATION_Y 12
//Camera center: those setpoints correspond to what we consider the center of the field
#define CAMERA_CENTER_X 0
#define CAMERA_CENTER_Y 0

2
include/vars.h
View File

@ -21,4 +21,6 @@ in the 32U4 code*/
#define ROLLER_INA 34
#define ROLLER_INB 35
#define BALL_32U4 Serial2
extr float sins[360], cosins[360];

8
src/motors_movement/drivecontroller.cpp
View File

@ -68,11 +68,11 @@ void DriveController::drive(int dir, int speed, int tilt){
//TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?
// Disable vector sum because calculations are a bitty crappy imho. Will have to test if it's what makes the robot act strange with lines
// Re enabling the below lines requires to comment out drive->prepareDrive and uncommenting the lines relative to vector sum inside positionsys_camera and comment out the other lines here
vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
//vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
//vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
// vx = ((speed * cosins[dir]));
// vy = ((-speed * sins[dir]));
vx = ((speed * cosins[dir]));
vy = ((-speed * sins[dir]));
// if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) {
// vxn = 0;

3
src/motors_movement/roller.cpp
View File

@ -91,5 +91,6 @@ void Roller::update(){
}
void Roller::speed(int speed){
roller->speed(speed);
if(roller_armed)
roller->speed(speed);
}

6
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -1,5 +1,6 @@
#include "behaviour_control/status_vector.h"
#include "sensors/data_source_camera_conicmirror.h"
#include "vars.h"
//Comment out to disable complementary filters on angles
#define CAMERA_CONIC_FILTER_POINTS
@ -170,6 +171,11 @@ void DataSourceCameraConic ::computeCoordsAngles() {
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
}
byte to_32u4 = 0;
to_32u4 |= (CURRENT_DATA_READ.ySeen);
to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1;
BALL_32U4.write(to_32u4);
}
void DataSourceCameraConic::test(){

2
src/sensors/sensors.cpp
View File

@ -8,7 +8,7 @@ void initSensors(){
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));
compass = new DataSourceBNO055();
ball = new DataSourceBall(&Serial2, 57600);
ball = new DataSourceBall(&BALL_32U4, 57600);
camera = new DataSourceCameraConic(&Serial3, 19200);
bt = new DataSourceBT(&Serial1, 115200);
roller = new Roller(30, 31, 1000, 2000, 500);

34
src/strategy_roles/striker.cpp
View File

@ -35,38 +35,24 @@ void Striker::realPlay()
ps->goCenter();
}
void Striker::striker()
{
int plusang = STRIKER_PLUSANG, ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle;
void Striker::striker(){
//seguo palla
int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG;
if (CURRENT_DATA_READ.ballDistance > STRIKER_ATTACK_DISTANCE)
{
drive->prepareDrive(ball_deg > 180 ? CURRENT_DATA_READ.ballAngle - 20 : CURRENT_DATA_READ.ballAngle + 20, MAX_VEL_HALF, 0);
return;
}
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 > 180) ball_degrees2 = ball_deg - 360; //ragiono in +180 -180
else ball_degrees2 = ball_deg;
if (ball_deg > 340 || ball_deg < 20)
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;
if(ball_degrees2 > 0) dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo
else dir = ball_deg - plusang; //se sto nel negativo sottraggo
if (ball_degrees2 > 0)
dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo
else
dir = ball_deg - plusang; //se sto nel negativo sottraggo
if (dir < 0)
dir = dir + 360; //se sto nel quadrante negativo ricappotto
else
dir = dir;
dir = (dir + 360) % 360;
drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
}
int Striker::tilt() {
if (ball->isInMouth() || (ball->isInMouthMaxDistance() && gotta_tilt)) gotta_tilt = true;
if (ball->isInMouth() /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
else gotta_tilt = false;
if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {

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

@ -126,18 +126,18 @@ void PositionSysCamera::CameraPID(){
speed = filterSpeed->calculate(speed);
speed = speed > 40 ? speed : 0;
dir = filterDir->calculate(dir);
// drive->prepareDrive(dir, speed, 0);
drive->prepareDrive(dir, speed, 0);
//Disable below lines for now because they probably result in unexpected behaviour on lines. Re enabling them requires to comment out the drive->prepareDrive above
//and check the notes in drivecontroller for the other stuff to comment and uncomment
//TODO: add complementary filter on this speed if we keep using it
vx = ((speed * cosins[dir]));
vy = ((-speed * sins[dir]));
// vx = ((speed * cosins[dir]));
// vy = ((-speed * sins[dir]));
CURRENT_DATA_WRITE.addvx = vx;
CURRENT_DATA_WRITE.addvy = vy;
// CURRENT_DATA_WRITE.addvx = vx;
// CURRENT_DATA_WRITE.addvy = vy;
}
}

8
utility/OpenMV/conic_eff_h7.py
View File

@ -45,11 +45,11 @@ blue_led.on()
##############################################################################
thresholds = [ (48, 83, -9, 18, 38, 82), # thresholds yellow goal
(45, 61, -11, 14, -42, -22)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (55, 92, -3, 24, 60, 90), # thresholds yellow goal
(33, 49, -9, 12, -52, -12)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (20, 0, 300, 220)
roi = (30, 0, 290, 240)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
@ -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=50, area_threshold=70, merge = True):
for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=90, merge = True):
img.draw_rectangle(blob.rect())
#img.draw_cross(blob.cx(), blob.cy())