EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

now the robot can use the camera to recenter even when one the goals can't be seen

code_midgen
EmaMaker 2020-02-27 17:30:10 +01:00
parent 752866de32
commit 00f6192255
5 changed files with 48 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/positionsys_camera.h
View File

@ -2,9 +2,8 @@
#include "systems.h"
#define CAMERA_CENTER_X 0
#define CAMERA_CENTER_Y_BOTH 3
#define CAMERA_CENTER_Y_BLUE -60
#define CAMERA_CENTER_Y_YELLOW -30
#define CAMERA_CENTER_Y -13
#define CAMERA_CENTER_Y_ABS_SUM 72
#define Kpx 1
#define Kix 0
@ -22,6 +21,7 @@ class PositionSysCamera : public PositionSystem{
void test() override;
void setCameraPID();
void CameraPID();
int calcOtherGoalY(int goalY);
double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;

24
src/data_source_camera_conicmirror.cpp
View File

@ -52,8 +52,8 @@ void DataSourceCameraConic ::readSensor() {
yAngleFix = ((int)((yAngle + angleFix * 0.8)) + 360) % 360;
bAngleFix = ((int)((bAngle + angleFix * 0.8)) + 360) % 360;
yDist = sqrt((true_yy - 50) * (true_yy - 50) + (50 - true_xy) * (50 - true_xy));
bDist = sqrt((true_yb - 50) * (true_yb - 50) + (50 - true_xb) * (50 - true_xb));
yDist = sqrt((true_yy) * (true_yy) + (true_xy) * (true_xy));
bDist = sqrt((true_yb) * (true_yb) + (true_xb) * (true_xb));
//Important: update status vector
CURRENT_INPUT_WRITE.cameraByte = value;
@ -68,14 +68,10 @@ void DataSourceCameraConic ::readSensor() {
CURRENT_DATA_WRITE.yDist = yDist;
CURRENT_DATA_WRITE.bDist = bDist;
if (xb == unkn || yb == unkn)
CURRENT_DATA_WRITE.bSeen = false;
else
CURRENT_DATA_WRITE.bSeen = true;
if (xy == unkn || yy == unkn)
CURRENT_DATA_WRITE.ySeen = false;
else
CURRENT_DATA_WRITE.ySeen = true;
if (xb == unkn || yb == unkn) CURRENT_DATA_WRITE.bSeen = false;
else CURRENT_DATA_WRITE.bSeen = true;
if (xy == unkn || yy == unkn) CURRENT_DATA_WRITE.ySeen = false;
else CURRENT_DATA_WRITE.ySeen = true;
if (goalOrientation == HIGH) {
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle;
@ -132,7 +128,9 @@ void DataSourceCameraConic::test()
DEBUG.print(" | ");
DEBUG.print(bAngleFix);
DEBUG.print(" | ");
DEBUG.println(bDist);
DEBUG.print(bDist);
DEBUG.print(" | ");
DEBUG.println(CURRENT_DATA_READ.bSeen);
DEBUG.println(" --- ");
DEBUG.print("Yellow: ");
@ -140,7 +138,9 @@ void DataSourceCameraConic::test()
DEBUG.print(" | ");
DEBUG.print(yAngleFix);
DEBUG.print(" | ");
DEBUG.println(yDist);
DEBUG.print(yDist);
DEBUG.print(" | ");
DEBUG.println(CURRENT_DATA_READ.ySeen);
DEBUG.println("---------------");
DEBUG.print("Data: ");
DEBUG.print(true_xb);

2
src/main.cpp
View File

@ -24,8 +24,6 @@ void loop() {
keeper->play(role==0);
// Last thing to do: movement and update status vector
// drive->prepareDrive(0,0, CURRENT_DATA_READ.angleAtkFix);
drive->drivePrepared();
updateStatusVector();
}

43
src/positionsys_camera.cpp
View File

@ -63,36 +63,47 @@ void PositionSysCamera :: setCameraPID(){
Y->SetSampleTime(2);
}
/*Knowing the sum of the absolute values of the y position of the goals, it calculates the missing goal y knowing the other one
We know the sum of the absolute values is a fixed number.
By subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y
The sign of the goal y we found is simply the reverse of the one we got
*/
int PositionSysCamera::calcOtherGoalY(int goalY){
int otherGoalY = CAMERA_CENTER_Y_ABS_SUM - abs(goalY);
otherGoalY = goalY < 0 ? otherGoalY : -otherGoalY;
return otherGoalY;
}
void PositionSysCamera :: CameraPID(){
if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){
Inputx = (CURRENT_DATA_READ.cam_xy + CURRENT_DATA_READ.cam_xb) / 2;
Inputy = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
Setpointx = CAMERA_CENTER_X;
Setpointy = CAMERA_CENTER_Y_BOTH;
}
if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){
}else if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){
Inputx = CURRENT_DATA_READ.cam_xb;
Inputy = CURRENT_DATA_READ.cam_yb;
Setpointx = CAMERA_CENTER_X;
Setpointy = CAMERA_CENTER_Y_BLUE;
}
if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){
Inputy = CURRENT_DATA_READ.cam_yb + calcOtherGoalY(CURRENT_DATA_READ.cam_yb);
}else if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){
Inputx = CURRENT_DATA_READ.cam_xy;
Inputy = CURRENT_DATA_READ.cam_yy;
Setpointx = CAMERA_CENTER_X;
Setpointy = CAMERA_CENTER_Y_YELLOW;
Inputy = CURRENT_DATA_READ.cam_yy + calcOtherGoalY(CURRENT_DATA_READ.cam_yy);
//Setpointy todo
}else{
}
Setpointx = CAMERA_CENTER_X;
Setpointy = CAMERA_CENTER_Y;
//TODO: no goal seen
X->Compute();
Y->Compute();
// DEBUG.println(Outputx);
// DEBUG.print(CURRENT_DATA_READ.cam_yb);
// DEBUG.print(" ");
// DEBUG.println(calcOtherGoalY(CURRENT_DATA_READ.cam_yb));
int dir = -90-(atan2(-Outputy,-Outputx)*180/3.14);
dir = (dir+360) % 360;
drive->prepareDrive(dir, 100, 0);
if(abs(Outputx) <= 1 && abs(Outputy) <= 1){
drive->prepareDrive(0,0,0);
}else{
int dir = -90-(atan2(-Outputy,-Outputx)*180/3.14);
dir = (dir+360) % 360;
drive->prepareDrive(dir, 100, 0);
}
}

12
utility/OpenMV/conic_eff.py
View File

@ -38,8 +38,8 @@ blue_led.on()
##############################################################################
thresholds = [ (23, 38, -34, -8, -12, 22), # thresholds yellow goal
(69, 98, -14, 30, 66, 113)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (50, 98, -2, 30, 57, 113), # thresholds yellow goal
(17, 30, -28, -8, -8, 12)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152)
@ -59,12 +59,12 @@ clock = time.clock()'''
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_contrast(+3)
sensor.set_saturation(2)
sensor.set_contrast(1)
sensor.set_saturation(1)
sensor.set_brightness(0)
sensor.set_quality(0)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False, 5000)
sensor.set_auto_exposure(False, 4500)
sensor.set_auto_gain(True)
sensor.skip_frames(time = 300)
@ -84,7 +84,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=80, 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())