From 79a5c84432f383a2f8e1fe9dea3743b8183ad206 Mon Sep 17 00:00:00 2001 From: dziedada Date: Mon, 9 Dec 2019 19:03:00 -0500 Subject: [PATCH] Goalie functionality works. --- config/camera-config.yaml | 8 ++++---- include/planner/arm_planner.hpp | 6 +++--- include/planner/ball.hpp | 6 +++--- src/planner/TrackingVis.cpp | 2 +- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/config/camera-config.yaml b/config/camera-config.yaml index ce8c7e0..651c506 100644 --- a/config/camera-config.yaml +++ b/config/camera-config.yaml @@ -88,10 +88,10 @@ ball_detector: x_max: 2.5 y_min: -0.6 y_max: 0.6 - z_min: 0.02 - z_max: 0.11 - ball_radius_min: 0.07 - ball_radius_max: 0.11 + z_min: 0.000 + z_max: 0.06 + ball_radius_min: 0.014 + ball_radius_max: 0.06 fake_ball_detector: plane_mask: true diff --git a/include/planner/arm_planner.hpp b/include/planner/arm_planner.hpp index 9a53578..94fe7c5 100644 --- a/include/planner/arm_planner.hpp +++ b/include/planner/arm_planner.hpp @@ -153,7 +153,7 @@ class ArmPlanner double innerRadius = 0.13; // TODO i thought we might need this to restrict noisy velocities if ( x > 0 && y != 0 && bestBall->reachPrediction.ball_inrange_position_.norm() > innerRadius ) { - std::cout << "x: " << x << " y: " << y << std::endl; + std::cout << "x: " << y << " y: " << x << std::endl; publishPlan( bestBall->reachPrediction.ball_inrange_position_ ); } } @@ -271,8 +271,8 @@ class ArmPlanner std::vector > waypoints( 1, std::vector(3, 0)); // Flip X and Y for Arm Coordinate system - waypoints[0][0] = endpoint[0]; - waypoints[0][1] = endpoint[1]; + waypoints[0][0] = endpoint[1]; + waypoints[0][1] = endpoint[0]; waypoints[0][2] = 1; path.waypoints = waypoints; path.speed = 1.0; diff --git a/include/planner/ball.hpp b/include/planner/ball.hpp index bb03353..ce0f361 100644 --- a/include/planner/ball.hpp +++ b/include/planner/ball.hpp @@ -207,7 +207,7 @@ class Ball reachPrediction.ball_in_range_time_ = utime + seconds_to_microsends(interval); //cout << "reachPrediction time " << reachPrediction.ball_in_range_time_ << std::endl; reachPrediction.ball_inrange_position_ = position; - reachPrediction.ball_inrange_velocity_ = velocity; + reachPrediction.ball_inrange_velocity_ = velocity_coord_avg; reachPrediction.goal_ = position; return; } @@ -243,7 +243,7 @@ class Ball velocityEstimate /= (double)HISTORY_SIZE; //std::cout << "vel est final: " << velocityEstimate << std::endl; - Vector2d pointEstimate = coordinate + (velocityEstimate * dT); + Vector2d pointEstimate = coordinate + (velocity_coord_avg * dT); // std::cout << "prediction: "; // std::cout << "dT " << dT << " dx " << velocityEstimate.x() << " dy " << velocityEstimate.y() << endl; //return Vector4d(predState.at(0), predState.at(1), @@ -252,7 +252,7 @@ class Ball // TODO: IS this correct? return Vector4d( pointEstimate.x(), pointEstimate.y(), - velocityEstimate.x(), velocityEstimate.y()); + velocity_coord_avg.x(), velocity_coord_avg.y()); } // check to see if the ball will reach radius of us within diff --git a/src/planner/TrackingVis.cpp b/src/planner/TrackingVis.cpp index bc770f6..39ba16d 100644 --- a/src/planner/TrackingVis.cpp +++ b/src/planner/TrackingVis.cpp @@ -25,7 +25,7 @@ constexpr int DISPLAY_Y_ZERO = 250; constexpr int DISPLAY_X_ZERO = 250; constexpr int X_AXIS = 1; constexpr int Y_AXIS = 0; -constexpr float PREDICTION_TIME = 0.1; // Units are seconds +constexpr float PREDICTION_TIME = 0.5; // Units are seconds constexpr int LINE_THICKNESS = 1; constexpr int CIRCLE_RADIUS = 3; constexpr int64_t MAX_PRED_TIME_DIFF = 400; // Units are milliseconds