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

camera: coords filtering and better route calculation

pull/1/head
EmaMaker 2021-04-19 19:16:31 +02:00
parent 3ebe64555b
commit e71c49efd1
6 changed files with 59 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
include/sensors/data_source_camera_conicmirror.h
View File

@ -1,6 +1,7 @@
#pragma once
#include "behaviour_control/data_source.h"
#include "behaviour_control/complementary_filter.h"
#define startp 105
#define endp 115
@ -11,6 +12,12 @@
//Imu To Camera Angle Mult
#define IMUTOC_AMULT 1
#define FILTER_DEFAULT_COEFF 0.6
#define FILTER_BY_COEFF FILTER_DEFAULT_COEFF
#define FILTER_BX_COEFF FILTER_DEFAULT_COEFF
#define FILTER_YY_COEFF FILTER_DEFAULT_COEFF
#define FILTER_YX_COEFF FILTER_DEFAULT_COEFF
class DataSourceCameraConic : public DataSource{
public:
@ -30,4 +37,6 @@ class DataSourceCameraConic : public DataSource{
int goalOrientation, pAtk, pDef;
int value;
ComplementaryFilter *filter_yy, *filter_xy, *filter_yb, *filter_xb, *filter_yy_fix, *filter_xy_fix, *filter_yb_fix, *filter_xb_fix;
};

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

@ -7,8 +7,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 8
#define CAMERA_TRANSLATION_Y 10
#define CAMERA_TRANSLATION_X 10
#define CAMERA_TRANSLATION_Y 0
//Camera center: those setpoints correspond to what we consider the center of the field
#define CAMERA_CENTER_X 0
#define CAMERA_CENTER_Y 0
@ -24,7 +24,7 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/
//Actually it's ± MAX_VAL
#define MAX_X 50
#define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
#define DIST_MULT 1.4
#define DIST_MULT 1.7
#define Kpx 1
#define Kix 0

26
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -1,6 +1,9 @@
#include "behaviour_control/status_vector.h"
#include "sensors/data_source_camera_conicmirror.h"
//Comment out to disable complementary filters on angles
#define CAMERA_CONIC_FILTER_POINTS
DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud) {
true_xb = 0;
true_yb = 0;
@ -27,6 +30,15 @@ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : D
true_yb_fixed = 0;
true_xy_fixed = 0;
true_yy_fixed = 0;
filter_yy = new ComplementaryFilter(FILTER_YY_COEFF);
filter_xy = new ComplementaryFilter(FILTER_YX_COEFF);
filter_yb = new ComplementaryFilter(FILTER_BY_COEFF);
filter_xb = new ComplementaryFilter(FILTER_BX_COEFF);
filter_yy_fix = new ComplementaryFilter(FILTER_YY_COEFF);
filter_xy_fix = new ComplementaryFilter(FILTER_YX_COEFF);
filter_yb_fix = new ComplementaryFilter(FILTER_BY_COEFF);
filter_xb_fix = new ComplementaryFilter(FILTER_BX_COEFF);
}
void DataSourceCameraConic ::readSensor() {
@ -76,6 +88,13 @@ void DataSourceCameraConic ::computeCoordsAngles() {
true_xy = 50 - true_xy;
true_yy = true_yy - 50;
#ifdef CAMERA_CONIC_FILTER_POINTS
true_xb = filter_xb->calculate(true_xb);
true_yb = filter_yb->calculate(true_yb);
true_xy = filter_xy->calculate(true_xy);
true_yy = filter_yy->calculate(true_yy);
#endif
//-90 + to account for phase shifting with goniometric circle
yAngle = -90 + (atan2(true_yy, true_xy) * 180 / 3.14);
bAngle = -90 + (atan2(true_yb, true_xb) * 180 / 3.14);
@ -104,6 +123,13 @@ void DataSourceCameraConic ::computeCoordsAngles() {
yAngleFix = 90 - (atan2(true_yy_fixed, true_xy_fixed) * 180 / 3.14);
bAngleFix = 90 - (atan2(true_yb_fixed, true_xb_fixed) * 180 / 3.14);
#ifdef CAMERA_CONIC_FILTER_POINTS
true_xb_fixed = filter_xb_fix->calculate(true_xb_fixed);
true_yb_fixed = filter_yb_fix->calculate(true_yb_fixed);
true_xy_fixed = filter_xy_fix->calculate(true_xy_fixed);
true_yy_fixed = filter_yy_fix->calculate(true_yy_fixed);
#endif
//Important: update status vector
CURRENT_INPUT_WRITE.cameraByte = value;
CURRENT_DATA_WRITE.cam_xb = true_xb;

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

@ -2,6 +2,7 @@
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "vars.h"
#include "math.h"
PositionSysCamera::PositionSysCamera() {
MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
@ -112,17 +113,18 @@ void PositionSysCamera::CameraPID(){
Setpointy += axisy;
X->Compute();
Y->Compute();
Y->Compute();
//Compute an X and Y to give to the PID later
//There's surely a better way to do this
int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
dir = (dir+360) % 360;
int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
int dist = sqrt( ( (CURRENT_DATA_WRITE.posx-Setpointx)*(CURRENT_DATA_WRITE.posx-Setpointx) ) + (CURRENT_DATA_WRITE.posy-Setpointy)*(CURRENT_DATA_WRITE.posy-Setpointy) );
// int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL);
speed = filterSpeed->calculate(speed);
speed = speed > 35 ? speed : 0;
speed = speed > 40 ? speed : 0;
dir = filterDir->calculate(dir);
// drive->prepareDrive(dir, speed, 0);

12
src/test_menu.cpp
View File

@ -13,6 +13,8 @@
#include "behaviour_control/data_source.h"
#include "behaviour_control/status_vector.h"
unsigned long timer = 0;
void TestMenu::testMenu()
{
if (!(DEBUG.available() || flagtest))
@ -91,6 +93,16 @@ void TestMenu::testMenu()
DEBUG.println(robot_indentifier);
delay(50);
break;
case 'p':
if(millis() - timer > 150){
timer = millis();
DEBUG.println("Position on the field (camera)");
DEBUG.print("X: ");
DEBUG.print(CURRENT_DATA_READ.posx);
DEBUG.print(" , Y: ");
DEBUG.println(CURRENT_DATA_READ.posy);
}
break;
case 'u':
DEBUG.println("32u4 receive Test");
while (Serial2.available())

8
utility/OpenMV/conic_eff_h7.py
View File

@ -45,11 +45,11 @@ blue_led.on()
##############################################################################
thresholds = [ (48, 73, -15, 15, 29, 79), # thresholds yellow goal
(12, 43, -15, 15, -41, -9)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (54, 73, -4, 17, 18, 73), # thresholds yellow goal
(36, 45, -7, 7, -35, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (50,5,250, 230)
roi = (80, 0, 240, 230)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
@ -69,7 +69,7 @@ sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing(roi)
sensor.set_contrast(0)
sensor.set_saturation(3)
sensor.set_saturation(0)
sensor.set_brightness(1)
sensor.set_auto_whitebal(True)
sensor.set_auto_exposure(False, 6576)