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

Corrected a few major bugs in camera data receiving and using. Still no lock trying to directly fix goals' coordinates using axis rotation formula.

pull/1/head
EmaMaker 2020-03-02 20:45:48 +01:00
parent 8299d82dff
commit dddbbc1810
7 changed files with 54 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/position/positionsys_camera.h
View File

@ -2,8 +2,8 @@
#include "position/systems.h"
#define CAMERA_CENTER_X 0
#define CAMERA_CENTER_Y 0
#define CAMERA_CENTER_X 10
#define CAMERA_CENTER_Y -13
#define CAMERA_CENTER_Y_ABS_SUM 72
//Actually it's ± MAX_VAL
#define MAX_X 25

2
include/sensors/data_source_camera_conicmirror.h
View File

@ -8,6 +8,8 @@
//Coords are mapped from 0 up to this value
#define MAP_MAX 100
#define HALF_MAP_MAX 50
//Imu To Camera Angle Mult
#define IMUTOC_AMULT 1
class DataSourceCameraConic : public DataSource{

2
include/vars.h
View File

@ -1,5 +1,5 @@
#pragma once
#define DEBUG Serial
#define DEBUG Serial3
#define GLOBAL_SPD_MULT 1.0

5
src/main.cpp
View File

@ -19,10 +19,9 @@ void setup() {
void loop() {
updateSensors();
camera->test();
/* goalie->play(role==1);
keeper->play(role==0); */
goalie->play(role==1);
keeper->play(role==0);
// Last thing to do: movement and update status vector
drive->drivePrepared();

7
src/position/positionsys_camera.cpp
View File

@ -89,6 +89,13 @@ void PositionSysCamera :: CameraPID(){
}else{
//TODO: no goal seen
}
//To have the axis corresponding to the real cartesian plane Inputx and InputY must be multiplied by -1
//This is because the coordinates given as input are relative to the robot, while the coordinates used in the setpoint
//are on an absolute cartesian plane with it's center in the center of the field
Inputx *= -1;
Inputy *= -1;
Setpointx = CAMERA_CENTER_X;
Setpointy = CAMERA_CENTER_Y;

59
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -23,6 +23,10 @@ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : D
bAngle = 0;
bAngleFix = 0;
bDist = 0;
true_xb_fixed = 0;
true_yb_fixed = 0;
true_xy_fixed = 0;
true_yy_fixed = 0;
}
void DataSourceCameraConic ::readSensor() {
@ -37,24 +41,35 @@ void DataSourceCameraConic ::readSensor() {
if ((count = 4) && (start == true)) {
data_received = true;
true_xb = xb - 50;
true_yb = 50 - yb;
true_xy = xy - 50;
true_yy = 50 - yy;
true_xb_fixed = true_xb*(cos(CURRENT_DATA_READ.IMUAngle)) - true_yb*(sin(CURRENT_DATA_READ.IMUAngle));
true_xy_fixed = true_xy*(cos(CURRENT_DATA_READ.IMUAngle)) - true_yy*(sin(CURRENT_DATA_READ.IMUAngle));
true_yb_fixed = true_xb*(sin(CURRENT_DATA_READ.IMUAngle)) + true_yb*(cos(CURRENT_DATA_READ.IMUAngle));
true_yy_fixed = true_xy*(sin(CURRENT_DATA_READ.IMUAngle)) + true_yy*(cos(CURRENT_DATA_READ.IMUAngle));
true_xb = xb;
true_yb = yb;
true_xy = xy;
true_yy = yy;
//Remap from [0,100] to [-50, +49] to correctly compute angles and distances and calculate them
yAngle = -90 - (atan2(true_yy, true_xy) * 180 / 3.14);
bAngle = -90 - (atan2(true_yb, true_xb) * 180 / 3.14);
//Where are the goals relative to the robot?
true_xb = 50 - true_xb;
true_yb = true_yb - 50;
true_xy = 50 - true_xy;
true_yy = true_yy - 50;
//Remap from [0,100] to [-50, +50] to correctly compute angles and distances and calculate them
yAngle = 90 - (atan2(true_yy, true_xy) * 180 / 3.14);
bAngle = 90 - (atan2(true_yb, true_xb) * 180 / 3.14);
//Now cast angles to [0, 359] domain angle flip them
yAngle = (yAngle + 360) % 360;
bAngle = (bAngle + 360) % 360;
yDist = sqrt((true_yy) * (true_yy) + (true_xy) * (true_xy));
bDist = sqrt((true_yb) * (true_yb) + (true_xb) * (true_xb));
//Rotate the point on the cartesian plane to compensate the robot tilt
// float angleFix = CURRENT_DATA_READ.IMUAngle*IMUTOC_AMULT;
// angleFix = (angleFix * 3.14) / 180;
// float angleFix = CURRENT_DATA_READ.IMUAngle*3.14/180;
// true_xb_fixed = (true_xb*(cos(angleFix))) - (true_yb*(sin(angleFix)));
// true_yb_fixed = (true_xb*(sin(angleFix))) + (true_yb*(cos(angleFix)));
// true_xy_fixed = (true_xy*(cos(angleFix))) - (true_yy*(sin(angleFix)));
// true_yy_fixed = (true_xy*(sin(angleFix))) + (true_yy*(cos(angleFix)));
int angleFix = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle;
@ -62,8 +77,6 @@ void DataSourceCameraConic ::readSensor() {
yAngleFix = ((int)((yAngle + angleFix * 0.8)) + 360) % 360;
bAngleFix = ((int)((bAngle + angleFix * 0.8)) + 360) % 360;
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;
@ -71,6 +84,10 @@ void DataSourceCameraConic ::readSensor() {
CURRENT_DATA_WRITE.cam_yb = true_yb;
CURRENT_DATA_WRITE.cam_xy = true_xy;
CURRENT_DATA_WRITE.cam_yy = true_yy;
CURRENT_DATA_WRITE.cam_xb_fixed = true_xb_fixed;
CURRENT_DATA_WRITE.cam_yb_fixed = true_yb_fixed;
CURRENT_DATA_WRITE.cam_xy_fixed = true_xy_fixed;
CURRENT_DATA_WRITE.cam_yy_fixed = true_yy_fixed;
CURRENT_DATA_WRITE.yAngle = yAngle;
CURRENT_DATA_WRITE.bAngle = bAngle;
CURRENT_DATA_WRITE.yAngleFix = yAngleFix;
@ -145,10 +162,11 @@ void DataSourceCameraConic::test(){
DEBUG.print(CURRENT_DATA_READ.cam_yb);
DEBUG.print(")");
DEBUG.print(" | Fixed coords: (");
DEBUG.print(CURRENT_DATA_READ.cam_xy_fixed);
DEBUG.print(CURRENT_DATA_READ.cam_xb_fixed);
DEBUG.print(",");
DEBUG.println(CURRENT_DATA_READ.cam_yy_fixed);
DEBUG.println(")---------------");
DEBUG.print(CURRENT_DATA_READ.cam_yb_fixed);
DEBUG.println(")");
DEBUG.println("----------------");
DEBUG.print("Yellow: Angle: ");
DEBUG.print(yAngle);
DEBUG.print(" | Fixed Angle: ");
@ -165,7 +183,8 @@ void DataSourceCameraConic::test(){
DEBUG.print(" | Fixed coords: (");
DEBUG.print(CURRENT_DATA_READ.cam_xy_fixed);
DEBUG.print(",");
DEBUG.println(CURRENT_DATA_READ.cam_yy_fixed);
DEBUG.println(")---------------");
DEBUG.print(CURRENT_DATA_READ.cam_yy_fixed);
DEBUG.println(")");
DEBUG.println("---------------");
delay(150);
}

2
utility/OpenMV/conic_eff.py
View File

@ -64,7 +64,7 @@ sensor.set_saturation(1)
sensor.set_brightness(0)
sensor.set_quality(0)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False, 4500)
sensor.set_auto_exposure(False, 6500)
sensor.set_auto_gain(True)
sensor.skip_frames(time = 300)