diff --git a/include/position/positionsys_camera.h b/include/position/positionsys_camera.h index 1cf8b3f..2877bc4 100644 --- a/include/position/positionsys_camera.h +++ b/include/position/positionsys_camera.h @@ -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 diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 6c3e842..cd36257 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -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{ diff --git a/include/vars.h b/include/vars.h index 3c31965..4c4e00f 100755 --- a/include/vars.h +++ b/include/vars.h @@ -1,5 +1,5 @@ #pragma once -#define DEBUG Serial +#define DEBUG Serial3 #define GLOBAL_SPD_MULT 1.0 diff --git a/src/main.cpp b/src/main.cpp index e84ab13..52a14e5 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -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(); diff --git a/src/position/positionsys_camera.cpp b/src/position/positionsys_camera.cpp index f417fe0..ba28ab8 100644 --- a/src/position/positionsys_camera.cpp +++ b/src/position/positionsys_camera.cpp @@ -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; diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index 16d3f57..911048f 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -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); } diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index bca8390..e1bdc5c 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -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)