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

robocup preparation: correct pos-sys-camera bugs 

better use of roller
nice startup/debugging sounds
disable auto gain in H7
pull/1/head
EmaMaker 2021-05-12 16:44:17 +02:00
parent f11127bb40
commit 3631fe18ab
12 changed files with 81 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/behaviour_control/status_vector.h
View File

@ -38,7 +38,7 @@ typedef struct data{
int IMUAngle, ballAngle, ballAngleFix, ballDistance,
yAngle, bAngle, yAngleFix, bAngleFix,
yDist, bDist,
angleAtk, angleAtkFix, angleDef, angleDefFix,
angleAtk, angleAtkFix, angleDef, angleDefFix, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix,
cam_xb, cam_yb, cam_xy, cam_yy,
speed, tilt, dir, axisBlock[4],
USfr, USsx, USdx, USrr,

2
include/motors_movement/roller.h
View File

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

2
include/sensors/data_source_ball.h
View File

@ -4,7 +4,7 @@
#define MOUTH_MIN_ANGLE 340
#define MOUTH_MAX_ANGLE 20
#define MOUTH_DISTANCE 110
#define MOUTH_DISTANCE 115
#define MOUTH_MAX_DISTANCE 140
class DataSourceBall : public DataSource{

2
include/sensors/data_source_camera_conicmirror.h
View File

@ -22,7 +22,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 1
#define CAMERA_TRANSLATION_Y 4
#define CAMERA_TRANSLATION_Y -4
class DataSourceCameraConic : public DataSource{

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

@ -11,8 +11,8 @@
#define S1O A6
#define S2I A2
#define S2O A3
#define S3I A1
#define S3O A0
#define S3I A0
#define S3O A1
#define S4I A9
#define S4O A8

4
src/main.cpp
View File

@ -29,7 +29,7 @@ void setup() {
testmenu = new TestMenu();
tone(BUZZER, 240, 250);
initStatusVector();
delay(100);
delay(250);
tone(BUZZER, 260, 250);
initSensors();
@ -37,7 +37,7 @@ void setup() {
tone(BUZZER, 320, 250);
initGames();
delay(200);
delay(250);
//Startup sound
tone(BUZZER, 350.00, 250);

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;

22
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -160,16 +160,38 @@ void DataSourceCameraConic ::computeCoordsAngles() {
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle;
CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.ySeen;
CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yy;
CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yy_fixed;
CURRENT_DATA_WRITE.xAtk = CURRENT_DATA_WRITE.cam_xy;
CURRENT_DATA_WRITE.xAtkFix = CURRENT_DATA_WRITE.cam_xy_fixed;
CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.bAngle;
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix;
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.bSeen;
CURRENT_DATA_WRITE.yDef = CURRENT_DATA_WRITE.cam_yb;
CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yb_fixed;
CURRENT_DATA_WRITE.xDef = CURRENT_DATA_WRITE.cam_xb;
CURRENT_DATA_WRITE.xDefFix = CURRENT_DATA_WRITE.cam_xb_fixed;
} else {
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle;
CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.bAngleFix;
CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.bSeen;
CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yb;
CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yb_fixed;
CURRENT_DATA_WRITE.xAtk = CURRENT_DATA_WRITE.cam_xb;
CURRENT_DATA_WRITE.xAtkFix = CURRENT_DATA_WRITE.cam_xb_fixed;
CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle;
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
CURRENT_DATA_WRITE.yDef = CURRENT_DATA_WRITE.cam_yy;
CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yy_fixed;
CURRENT_DATA_WRITE.xDef = CURRENT_DATA_WRITE.cam_xy;
CURRENT_DATA_WRITE.xDefFix = CURRENT_DATA_WRITE.cam_xy_fixed;
}
byte to_32u4 = 0;

12
src/strategy_roles/striker.cpp
View File

@ -47,8 +47,15 @@ void Striker::striker(){
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->isInMouth() && ( (CURRENT_DATA_READ.posx <= -30 && CURRENT_DATA_READ.posy >= 35) || (CURRENT_DATA_READ.posx >= 30 && CURRENT_DATA_READ.posy >= 35))){
// ps->goCenter();
// }
}
int Striker::tilt() {
@ -62,8 +69,5 @@ int Striker::tilt() {
atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
}
if(ball->isInFront()) roller->speed(ROLLER_DEFAULT_SPEED);
else roller->speed(roller->MIN);
return atk_tilt;
}

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

@ -42,12 +42,24 @@ void PositionSysCamera::update(){
if(CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == true){
posx = (CURRENT_DATA_WRITE.cam_xy + CURRENT_DATA_WRITE.cam_xb) / 2;
posy = CURRENT_DATA_WRITE.cam_yb + CURRENT_DATA_WRITE.cam_yy;
//IMPORTANT STEP: or the direction of the plane will be flipped
posx *= -1;
posy *= -1;
}else if (CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == false){
posx = CURRENT_DATA_WRITE.cam_xb;
posy = CURRENT_DATA_WRITE.cam_yb + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yb);
//IMPORTANT STEP: or the direction of the plane will be flipped
posx *= -1;
posy *= -1;
}else if (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == true){
posx = CURRENT_DATA_WRITE.cam_xy;
posy = CURRENT_DATA_WRITE.cam_yy + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yy);
//IMPORTANT STEP: or the direction of the plane will be flipped
posx *= -1;
posy *= -1;
}else{
// Go back in time until we found a valid status, when we saw at least one goal
int i = 1;
@ -67,10 +79,6 @@ void PositionSysCamera::update(){
}
}
//IMPORTANT STEP: or the direction of the plane will be flipped
posx *= -1;
posy *= -1;
CURRENT_DATA_WRITE.posx = posx;
CURRENT_DATA_WRITE.posy = posy;
Inputx = posx;
@ -144,18 +152,18 @@ void PositionSysCamera::CameraPID(){
speed = speed > 30 ? speed : 0;
dir = filterDir->calculate(dir);;
//speed = filterSpeed->calculate(speed);
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;
}
}

18
utility/OpenMV/conic_eff_h7.py
View File

@ -45,11 +45,11 @@ blue_led.on()
##############################################################################
thresholds = [ (51, 74, -18, 12, 25, 70), # thresholds yellow goal
(27, 40, -18, 13, -29, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
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)
roi = (80, 0, 240, 220)
roi = (80, 0, 220, 200)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
@ -69,11 +69,11 @@ sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing(roi)
sensor.set_contrast(0)
sensor.set_saturation(1)
sensor.set_brightness(2)
sensor.set_auto_whitebal(True)
sensor.set_auto_exposure(True)
sensor.set_auto_gain(True)
sensor.set_saturation(2)
sensor.set_brightness(3)
sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -0.707413))
sensor.set_auto_exposure(False, 6576)
#sensor.set_auto_gain(False, gain_db=8.78)
sensor.skip_frames(time = 300)
clock = time.clock()
@ -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=40, area_threshold=50, merge = True):
for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=80, merge = True):
img.draw_rectangle(blob.rect())
#img.draw_cross(blob.cx(), blob.cy())

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

@ -36,7 +36,8 @@ 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()
@ -45,11 +46,11 @@ blue_led.on()
##############################################################################
thresholds = [ (51, 74, -18, 12, 25, 70), # thresholds yellow goalz
(27, 40, -18, 13, -29, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
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)
roi = (80, 0, 240, 220)
roi = (80, 0, 220, 200)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
@ -69,11 +70,11 @@ sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing(roi)
sensor.set_contrast(0)
sensor.set_saturation(1)
sensor.set_brightness(2)
sensor.set_auto_whitebal(True)
sensor.set_auto_exposure(True)
sensor.set_auto_gain(True)
sensor.set_saturation(2)
sensor.set_brightness(3)
sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -0.707413))
sensor.set_auto_exposure(False, 6576)
#sensor.set_auto_gain(False, gain_db=8.78)
sensor.skip_frames(time = 300)
clock = time.clock()
@ -94,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=40, area_threshold=50, merge = True):
for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=80, merge = True):
img.draw_rectangle(blob.rect())
#img.draw_cross(blob.cx(), blob.cy())