From 827e063155e3a7e964f0a3c2d102af5bb97a21e2 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Mon, 21 Jun 2021 11:17:33 +0200 Subject: [PATCH] precision shooter,camera: improve, fix math error in translation alongside with better speed calculation and utility methods --- include/motors_movement/drivecontroller.h | 2 +- include/sensors/data_source_ball_presence.h | 2 +- .../sensors/data_source_camera_conicmirror.h | 2 +- include/strategy_roles/precision_shooter.h | 4 +- include/systems/position/positionsys_camera.h | 4 +- src/main.cpp | 2 + src/sensors/data_source_ball_presence.cpp | 2 +- .../data_source_camera_conicmirror.cpp | 8 +-- src/sensors/sensors.cpp | 2 +- src/strategy_roles/precision_shooter.cpp | 64 +++++++++++-------- src/system/positions/positionsys_camera.cpp | 8 +-- src/test_menu.cpp | 4 +- utility/OpenMV/conic_eff_h7.py | 4 +- 13 files changed, 62 insertions(+), 46 deletions(-) diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index c65a51b..6bd356a 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -20,7 +20,7 @@ //Max possible vel 310 -#define MAX_VEL 120 +#define MAX_VEL 50 #define MAX_VEL_EIGTH ((int)MAX_VEL*0.8) #define MAX_VEL_HALF ((int)MAX_VEL*0.5) #define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75) diff --git a/include/sensors/data_source_ball_presence.h b/include/sensors/data_source_ball_presence.h index 2190d17..d90a55d 100644 --- a/include/sensors/data_source_ball_presence.h +++ b/include/sensors/data_source_ball_presence.h @@ -1,6 +1,6 @@ #pragma once -#define BALL_PRESENCE_TRESH 500 +#define BALL_PRESENCE_TRESH 600 #include #include "behaviour_control/data_source.h" diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 747ec32..32ae0ac 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -27,7 +27,7 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/ // #define CAMERA_TRANSLATION_Y 7 //Robot with roller -#define CAMERA_TRANSLATION_X 0 +#define CAMERA_TRANSLATION_X 4 #define CAMERA_TRANSLATION_Y 7 class DataSourceCameraConic : public DataSource{ diff --git a/include/strategy_roles/precision_shooter.h b/include/strategy_roles/precision_shooter.h index 0a5f946..eef60dd 100644 --- a/include/strategy_roles/precision_shooter.h +++ b/include/strategy_roles/precision_shooter.h @@ -11,7 +11,7 @@ #define PS_PLUSANG_VISIONCONE 10 // There needs to be a little bit of space between the target point and the spot to be in -#define PS_SPINNER_OVERHEAD 10 +#define PS_SPINNER_OVERHEAD 6 class PrecisionShooter : public Game{ @@ -23,7 +23,7 @@ class PrecisionShooter : public Game{ void realPlay() override; void init() override; void catchBall(); - void spinner(); + void spinner(int); int tilt(); private: diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 7abae74..f842e99 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -20,9 +20,9 @@ //Actually it's ± MAX_VAL #define MAX_X 50 #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2) -#define DIST_MULT 1.7 +#define DIST_MULT 3.5 -#define VICINITY_DIST_TRESH 3 +#define VICINITY_DIST_TRESH 2 #define Kpx 1 #define Kix 0 diff --git a/src/main.cpp b/src/main.cpp index 47a56bc..152342e 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -57,6 +57,8 @@ void loop() { testmenu->testMenu(); + // if(roller->roller_armed) roller->speed(roller->MAX); + // Last thing to do: movement and update status vector drive->drivePrepared(); updateStatusVector(); diff --git a/src/sensors/data_source_ball_presence.cpp b/src/sensors/data_source_ball_presence.cpp index 1ad3e09..ffcb2cd 100644 --- a/src/sensors/data_source_ball_presence.cpp +++ b/src/sensors/data_source_ball_presence.cpp @@ -25,5 +25,5 @@ void DataSourceBallPresence::test(){ DEBUG.print(" -> "); DEBUG.print(CURRENT_DATA_READ.ballPresent); DEBUG.print(" | Ball in mouth: "); - DEBUG.print(isInMouth()); + DEBUG.println(isInMouth()); } \ No newline at end of file diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index 6f5c911..4b6ab24 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -87,10 +87,10 @@ void DataSourceCameraConic ::readSensor() { void DataSourceCameraConic ::computeCoordsAngles() { //Where are the goals relative to the robot? //Remap from [0,100] to [-50, +50] to correctly compute angles and distances and calculate them, getting the original coords calculated by the camera - true_xb = 50 - true_xb + CAMERA_TRANSLATION_X; - true_yb = true_yb - 50 + CAMERA_TRANSLATION_Y; - true_xy = 50 - true_xy + CAMERA_TRANSLATION_X; - true_yy = true_yy - 50 + CAMERA_TRANSLATION_Y; + true_xb = 50 - true_xb + CAMERA_TRANSLATION_X*0.5; + true_yb = true_yb - 50 + CAMERA_TRANSLATION_Y*0.5; + true_xy = 50 - true_xy + CAMERA_TRANSLATION_X*0.5; + true_yy = true_yy - 50 + CAMERA_TRANSLATION_Y*0.5; #ifdef CAMERA_CONIC_FILTER_POINTS true_xb = filter_xb->calculate(true_xb); diff --git a/src/sensors/sensors.cpp b/src/sensors/sensors.cpp index b77f61b..3f437d9 100644 --- a/src/sensors/sensors.cpp +++ b/src/sensors/sensors.cpp @@ -20,7 +20,7 @@ void initSensors(){ // delay(350); bt = new DataSourceBT(&Serial1, 9600); roller = new Roller(30, 31, 1000, 2000, 500); - ballPresence = new DataSourceBallPresence(A22, true); + ballPresence = new DataSourceBallPresence(A13, true); } void updateSensors(){ diff --git a/src/strategy_roles/precision_shooter.cpp b/src/strategy_roles/precision_shooter.cpp index 1b3173c..4022aef 100644 --- a/src/strategy_roles/precision_shooter.cpp +++ b/src/strategy_roles/precision_shooter.cpp @@ -32,7 +32,7 @@ void PrecisionShooter::init() void PrecisionShooter::realPlay() { if (CURRENT_DATA_READ.ballSeen) - this->catchBall(); + this->spinner(0); else ps->goCenter(); } @@ -51,61 +51,75 @@ unsigned long spinner_timer = 0; float spinner_tilt = 0; void PrecisionShooter::spinner(int targetx){ - if(!ballPresence->isInMouth()) { - spinner_state=0; - spinner_flag = false; - } + // if(!ballPresence->isInMouth()) { + // spinner_state=0; + // spinner_flag = false; + // } if(spinner_state == 0){ - if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED); + if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); + if(ballPresence->isInMouth() && !spinner_flag){ spinner_flag = true; spinner_timer = millis(); } - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 250) { + if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { spinner_state++; - spinner_tilt = CURRENT_DATA_READ.IMUAngle; + spinner_flag = false; } }else if(spinner_state == 1){ - int spotx; + roller->speed(roller->MAX); + int spotx; if(targetx > 0) spotx = targetx-PS_SPINNER_OVERHEAD; else spotx = targetx+PS_SPINNER_OVERHEAD; - ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0); - if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)){ - ball_catch_state++; + if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 5)) { + + if( !spinner_flag){ + spinner_flag = true; + spinner_timer = millis(); + } + + if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 750) { + spinner_state++; + spinner_flag = false; + spinner_tilt = CURRENT_DATA_READ.IMUAngle; + } if(CURRENT_DATA_READ.posx > targetx){ - tilt1 = -0.1; - tilt2 = 0.35; + tilt1 = -0.075; + tilt2 = 0.3; }else{ - tilt1 = 0.1; - tilt2 = -0.35; + tilt1 = 0.075; + tilt2 = -0.3; } - } + }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 5); }else if(spinner_state == 2){ + roller->speed(roller->MAX); + spinner_tilt += tilt1; - drive(0,0,spinner_tilt); + drive->prepareDrive(0,0,spinner_tilt); if(CURRENT_DATA_READ.IMUAngle > 175 && CURRENT_DATA_READ.IMUAngle < 185) { spinner_state++; spinner_tilt = CURRENT_DATA_READ.IMUAngle; + + spinner_timer = millis(); } }else if(spinner_state == 3){ + roller->speed(roller->MAX); + if(millis() - spinner_timer > 2000) spinner_state++; + }else if(spinner_state == 4){ spinner_tilt += tilt2; - drive(0,0,spinner_tilt); + drive->prepareDrive(0,0,spinner_tilt); - if(CURRENT_DATA_READ.IMUAngle > 225 || CURRENT_DATA_READ.IMUAngle < 135){ - roller->speed(roller->MIN); - spinner_state++; - } + if(CURRENT_DATA_READ.IMUAngle > 215 || CURRENT_DATA_READ.IMUAngle < 125) roller->speed(roller->MIN); + if(CURRENT_DATA_READ.IMUAngle > 355 || CURRENT_DATA_READ.IMUAngle < 10) spinner_state++; } - } - /* Ball catch state machine 0: go towards the ball, until it's been in robot's mouth for 250ms diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index be12d8c..88c808d 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -145,10 +145,10 @@ void PositionSysCamera::CameraPID(){ int dir = -90-(atan2(Outputy,Outputx)*180/3.14); dir = (dir+360) % 360; - int dist = sqrt( ( pow(CURRENT_DATA_WRITE.posx-Setpointx,2) ) + pow(CURRENT_DATA_WRITE.posy-Setpointy, 2) ); - int speed = dist*DIST_MULT; - - speed = speed > 10 ? speed : 0; + // int dist = sqrt( ( pow(CURRENT_DATA_WRITE.posx-Setpointx,2) ) + pow(CURRENT_DATA_WRITE.posy-Setpointy, 2) ); + // int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL); + int speed = hypot(Outputx, Outputy) * DIST_MULT; + // speed = speed > 10 ? speed : 0; dir = filterDir->calculate(dir); #ifdef DRIVE_VECTOR_SUM diff --git a/src/test_menu.cpp b/src/test_menu.cpp index 15bc19d..114ef8e 100644 --- a/src/test_menu.cpp +++ b/src/test_menu.cpp @@ -114,8 +114,8 @@ void TestMenu::testMenu() ballPresence->test(); break; case 'r': - drive->stopAll(); - flagtest = false; + DEBUG.println("Roller at default speed"); + roller->speed(roller->MAX); break; case 's': DEBUG.println("32u4 send Test"); diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index a2a2f13..d0c7a05 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -46,7 +46,7 @@ blue_led.on() thresholds = [ (70, 100, -36, 8, 38, 112), # thresholds yellow goalz - (50, 77, -23, 8, -60, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + (31, 74, -31, 15, -50, -9)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (50, 0, 250, 200) @@ -67,7 +67,7 @@ clock = time.clock()''' sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QVGA) -#sensor.set_windowing(roi) +sensor.set_windowing(roi) sensor.set_contrast(0) sensor.set_saturation(2) sensor.set_brightness(3)