Goalie functionality works.

jhirsh · Dec 10, 2019 · 79a5c84 · 79a5c84
1 parent efa607b
commit 79a5c84
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 11 deletions.
diff --git 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
@@ -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<std::vector< double > > waypoints( 1, std::vector<double>(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
@@ -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<double>(0), predState.at<double>(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
@@ -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