Merge "Extract common charuco detection code into charuco_lib"
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index cea1fdd..72d3297 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -588,6 +588,9 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
+ # Motor inertia in kg m^2
+ self.motor_inertia = 0.0001634
+
class NMotor(object):
def __init__(self, motor, n):
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 2e4e269..bf54f60 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -235,7 +235,6 @@
if self.points.getSplines():
self.draw_splines(cr)
for i, points in enumerate(self.points.getSplines()):
-
points = [
np.array([self.mToPx(x), self.mToPx(y)])
for (x, y) in points
@@ -332,6 +331,17 @@
self.queue_draw()
self.graph.schedule_recalculate(self.points)
+ def clear_graph(self):
+ self.points = Points()
+ #recalulate graph using new points
+ self.graph.axis.clear()
+ self.graph.queue_draw()
+ #allow placing again
+ self.mode = Mode.kPlacing
+ #redraw entire graph
+ self.queue_draw()
+ #TODO: Make a way to undo clear
+
def do_key_press_event(self, event):
keyval = Gdk.keyval_to_lower(event.keyval)
@@ -354,7 +364,6 @@
event.x, event.y)
if self.mode == Mode.kEditing:
if self.index_of_edit > -1 and self.held_x != self.mousex:
-
self.points.setSplines(self.spline_edit, self.index_of_edit,
self.pxToM(self.mousex),
self.pxToM(self.mousey))
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index 258dcfa..e07bc80 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -20,6 +20,9 @@
self.connect(event, handler)
+ def clear_clicked(self, button):
+ self.field.clear_graph()
+
def output_json_clicked(self, button):
self.field.export_json(self.file_name_box.get_text())
@@ -129,6 +132,9 @@
self.input_json.set_size_request(100, 40)
self.input_json.connect("clicked", self.input_json_clicked)
+ self.clear = Gtk.Button.new_with_label("Clear")
+ self.clear.set_size_request(100, 40)
+ self.clear.connect("clicked", self.clear_clicked)
#Dropdown feature
self.label = Gtk.Label()
self.label.set_text("Change Field:")
@@ -169,6 +175,7 @@
jsonControls.add(self.file_name_box)
jsonControls.add(self.output_json)
jsonControls.add(self.input_json)
+ jsonControls.add(self.clear)
container.attach(jsonControls, 1, 0, 1, 1)
container.attach(self.label, 4, 0, 1, 1)
diff --git a/y2020/actors/shooter_tuning_actor.cc b/y2020/actors/shooter_tuning_actor.cc
index a783c65..f9d4123 100644
--- a/y2020/actors/shooter_tuning_actor.cc
+++ b/y2020/actors/shooter_tuning_actor.cc
@@ -69,6 +69,13 @@
const auto finisher = tuning_params_->finisher();
const auto accelerator = tuning_params_->accelerator();
+ LOG(INFO) << "Tuning with fininisher from " << finisher->velocity_initial()
+ << " to " << finisher->velocity_final() << " by "
+ << finisher->velocity_increment();
+ LOG(INFO) << "Tuning with accelerator from "
+ << accelerator->velocity_initial() << " to "
+ << accelerator->velocity_final() << " by "
+ << accelerator->velocity_increment();
// Add the velocities for the sweep
for (velocity_finisher_ = finisher->velocity_initial();
velocity_finisher_ <= finisher->velocity_final();
@@ -84,12 +91,11 @@
// Randomize the ordering of the velocities
std::srand(std::time(nullptr));
std::random_shuffle(velocities_.begin(), velocities_.end());
-
- fout_.open("shooter_tuning_data.csv", std::ios_base::app);
}
bool ShooterTuningActor::RunAction(
const frc971::autonomous::AutonomousActionParams * /*params*/) {
+ fout_.open("shooter_tuning_data.csv", std::ios_base::app);
LOG(INFO) << "velocity_accelerator,velocity_finisher,velocity_ball";
shooting_ = true;
@@ -100,12 +106,24 @@
velocity_finisher_ = velocities_[i].second;
SendSuperstructureGoal();
WaitAndWriteBallData();
+
+ if (i % 100 == 0 && i != 0) {
+ LOG(INFO) << "Pausing to cool for 2 minutes";
+ shooting_ = false;
+ velocity_accelerator_ = 0.0;
+ velocity_finisher_ = 0.0;
+ SendSuperstructureGoal();
+ std::this_thread::sleep_for(std::chrono::seconds(120));
+ shooting_ = true;
+ }
}
shooting_ = false;
velocity_finisher_ = 0.0;
velocity_accelerator_ = 0.0;
SendSuperstructureGoal();
+ fout_.close();
+ LOG(INFO) << "Done!";
return true;
}
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 893613a..ea12b46 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -33,19 +33,32 @@
::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>
*const hood = &r->hood;
- constexpr double kFeetToMeters = 0.0254 * 12.0;
- // Approximate robot length, for converting estimates from the doc below.
- // Rounded up from exact estimate, since I'm not sure if the original estimate
- // includes bumpers.
- constexpr double kRobotLength = 0.9;
- // { distance_to_target, { hood_angle, accelerator_power, finisher_power }}
- // Current settings based on
- // https://docs.google.com/document/d/1NR9F-ntlSoqZ9LqDzLjn-c14t8ZrppawCCG7wQy47RU/edit
+ constexpr double kInchesToMeters = 0.0254;
+ // Approximate length from the front bumpers to the middle of the robot.
+ constexpr double kHalfRobotLength = (36.00 / 2) * kInchesToMeters;
+ // We found that the finisher velocity does not change ball velocity much, so
+ // keep it constant.
+ constexpr double kVelocityFinisher = 350.0;
r->shot_interpolation_table = InterpolationTable<Values::ShotParams>(
- {{7.6 * kFeetToMeters - kRobotLength, {0.115, 197.0, 175.0}},
- {7.6 * kFeetToMeters + kRobotLength, {0.31, 265.0, 235.0}},
- {12.6 * kFeetToMeters + kRobotLength, {0.4, 292.0, 260.0}},
- {17.6 * kFeetToMeters + kRobotLength, {0.52, 365.0, 325.0}}});
+ {{40.00 * kInchesToMeters + kHalfRobotLength, {0.1, 10.6}},
+ {113.5 * kInchesToMeters + kHalfRobotLength, {0.42, 13.2}},
+ {168.5 * kInchesToMeters + kHalfRobotLength, {0.51, 13.2}},
+ {231.3 * kInchesToMeters + kHalfRobotLength, {0.51, 13.2}},
+ {276.5 * kInchesToMeters + kHalfRobotLength, {0.53, 13.2}}});
+
+ r->flywheel_shot_interpolation_table =
+ InterpolationTable<Values::FlywheelShotParams>(
+ {{10.6, {250.0, kVelocityFinisher}},
+ {12.0, {275.0, kVelocityFinisher}},
+ {13.2, {300.0, kVelocityFinisher}},
+ {14.0, {325.0, kVelocityFinisher}},
+ {14.6, {350.0, kVelocityFinisher}},
+ {15.2, {375.0, kVelocityFinisher}},
+ {15.6, {400.0, kVelocityFinisher}},
+ {16.1, {425.0, kVelocityFinisher}},
+ {16.3, {450.0, kVelocityFinisher}},
+ {16.6, {475.0, kVelocityFinisher}},
+ {17.0, {500.0, kVelocityFinisher}}});
// Hood constants.
hood->zeroing_voltage = 2.0;
@@ -126,9 +139,11 @@
1.42977866919024 - Values::kIntakeZero();
turret->potentiometer_offset = 5.52519370141247 + 0.00853506822980376 +
- 0.0109413725126625 - 0.223719825811759;
+ 0.0109413725126625 - 0.223719825811759 +
+ 0.261356551915472;
+ ;
turret_params->zeroing_constants.measured_absolute_position =
- 0.547478339799516;
+ 0.588553036694566;
hood->zeroing_constants.measured_absolute_position = 0.0344482433884915;
hood->zeroing_constants.single_turn_measured_absolute_position =
diff --git a/y2020/constants.h b/y2020/constants.h
index 77f78f4..4483948 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -190,24 +190,37 @@
struct ShotParams {
// Measured in radians
double hood_angle;
- // Angular velocity in radians per second of the slowest (lowest) wheel in
- // the kicker. Positive is shooting the ball.
- double accelerator_power;
- // Angular velocity in radians per seconds of the flywheel. Positive is
- // shooting.
- double finisher_power;
+ // Muzzle velocity (m/s) of the ball as it is shot out of the shooter.
+ double velocity_ball;
static ShotParams BlendY(double coefficient, ShotParams a1, ShotParams a2) {
using ::frc971::shooter_interpolation::Blend;
- return ShotParams{
- Blend(coefficient, a1.hood_angle, a2.hood_angle),
- Blend(coefficient, a1.accelerator_power, a2.accelerator_power),
- Blend(coefficient, a1.finisher_power, a2.finisher_power)};
+ return ShotParams{Blend(coefficient, a1.hood_angle, a2.hood_angle),
+ Blend(coefficient, a1.velocity_ball, a2.velocity_ball)};
}
};
- // { distance_to_target, { hood_angle, accelerator_power, finisher_power }}
+ struct FlywheelShotParams {
+ // Angular velocity in radians per second of the slowest (lowest) wheel in
+ // the kicker. Positive is shooting the ball.
+ double velocity_accelerator;
+ // Angular velocity in radians per seconds of the flywheel. Positive is
+ // shooting.
+ double velocity_finisher;
+
+ static FlywheelShotParams BlendY(double coefficient, FlywheelShotParams a1,
+ FlywheelShotParams a2) {
+ using ::frc971::shooter_interpolation::Blend;
+ return FlywheelShotParams{
+ Blend(coefficient, a1.velocity_accelerator, a2.velocity_accelerator),
+ Blend(coefficient, a1.velocity_finisher, a2.velocity_finisher)};
+ }
+ };
+
+ // { distance_to_target, { hood_angle, velocity_ball }}
InterpolationTable<ShotParams> shot_interpolation_table;
+ // { velocity_ball, { velocity_accelerator, velocity_finisher }}
+ InterpolationTable<FlywheelShotParams> flywheel_shot_interpolation_table;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2020/control_loops/python/turret.py b/y2020/control_loops/python/turret.py
index acddae3..6f15057 100644
--- a/y2020/control_loops/python/turret.py
+++ b/y2020/control_loops/python/turret.py
@@ -20,14 +20,14 @@
kTurret = angular_system.AngularSystemParams(
name='Turret',
- motor=control_loop.Vex775Pro(),
- G=(6.0 / 60.0) * (26.0 / 150.0),
+ motor=control_loop.MiniCIM(),
+ G=(26.0 / 150.0) * (14.0 / 60.0) * (20.0 / 60.0),
J=0.20,
q_pos=0.30,
q_vel=4.5,
kalman_q_pos=0.12,
kalman_q_vel=10.0,
- kalman_q_voltage=12.0,
+ kalman_q_voltage=20.0,
kalman_r_position=0.05)
def main(argv):
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index f7ab223..36edf30 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -50,7 +50,7 @@
// Limit to the battery voltage and the current limit voltage.
mutable_U(0, 0) = std::clamp(U(0, 0), lower_limit, upper_limit);
- mutable_U(0, 0) = std::clamp(U(0, 0), -12.0, 12.0);
+ mutable_U(0, 0) = std::clamp(U(0, 0), 0.0, 12.0);
}
private:
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 98de31d..70ea319 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -72,15 +72,18 @@
aos::FlatbufferFixedAllocatorArray<ShooterGoal, 64> shooter_goal;
constants::Values::ShotParams shot_params;
+ constants::Values::FlywheelShotParams flywheel_shot_params;
if (constants::GetValues().shot_interpolation_table.GetInRange(
- distance_to_goal, &shot_params)) {
+ distance_to_goal, &shot_params) &&
+ constants::GetValues().flywheel_shot_interpolation_table.GetInRange(
+ shot_params.velocity_ball, &flywheel_shot_params)) {
hood_goal.Finish(frc971::control_loops::
CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*hood_goal.fbb(), shot_params.hood_angle));
- shooter_goal.Finish(CreateShooterGoal(*shooter_goal.fbb(),
- shot_params.accelerator_power,
- shot_params.finisher_power));
+ shooter_goal.Finish(CreateShooterGoal(
+ *shooter_goal.fbb(), flywheel_shot_params.velocity_accelerator,
+ flywheel_shot_params.velocity_finisher));
} else {
hood_goal.Finish(
frc971::control_loops::
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index e4a9cb6..59e976e 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -26,9 +26,9 @@
// Terms to control the velocity gain for the friction compensation, and the
// voltage cap.
- static constexpr double kTurretFrictionGain = 10.0;
+ static constexpr double kTurretFrictionGain = 0.0;
static constexpr double kTurretFrictionVoltageLimit = 1.5;
- static constexpr double kTurretDitherGain = 0.4;
+ static constexpr double kTurretDitherGain = 0.0;
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index f71fe17..28c4bf5 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -1085,6 +1085,7 @@
constants::Values::kHoodRange().upper, 0.001);
}
+// Test a value in range with auto tracking
TEST_P(SuperstructureAllianceTest, ShooterInterpolationInRange) {
SetEnabled(true);
const frc971::control_loops::Pose target = turret::OuterPortPose(GetParam());
@@ -1140,15 +1141,15 @@
EXPECT_GE(superstructure_status_fetcher_->shooter()
->accelerator_left()
->angular_velocity_goal(),
- 100.0);
+ 250.0);
EXPECT_GE(superstructure_status_fetcher_->shooter()
->accelerator_right()
->angular_velocity_goal(),
- 100.0);
- EXPECT_GE(superstructure_status_fetcher_->shooter()
- ->finisher()
- ->angular_velocity_goal(),
- 100.0);
+ 250.0);
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->angular_velocity_goal(),
+ 350.0);
EXPECT_GE(superstructure_status_fetcher_->hood()->position(),
constants::Values::kHoodRange().lower);
}
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index f5b0612..26f65b7 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -32,12 +32,12 @@
public:
CameraReader(aos::EventLoop *event_loop,
const sift::TrainingData *training_data, V4L2Reader *reader,
- cv::FlannBasedMatcher *matcher)
+ const cv::Ptr<cv::flann::IndexParams> &index_params,
+ const cv::Ptr<cv::flann::SearchParams> &search_params)
: event_loop_(event_loop),
training_data_(training_data),
camera_calibration_(FindCameraCalibration()),
reader_(reader),
- matcher_(matcher),
image_sender_(event_loop->MakeSender<CameraImage>("/camera")),
result_sender_(
event_loop->MakeSender<sift::ImageMatchResult>("/camera")),
@@ -46,10 +46,16 @@
read_image_timer_(event_loop->AddTimer([this]() { ReadImage(); })),
prev_R_camera_field_vec_(cv::Mat::zeros(3, 1, CV_32F)),
prev_T_camera_field_(cv::Mat::zeros(3, 1, CV_32F)) {
+
+ for (int ii = 0; ii < number_training_images(); ++ii) {
+ matchers_.push_back(cv::FlannBasedMatcher(index_params, search_params));
+ }
+
CopyTrainingFeatures();
- // Technically we don't need to do this, but doing it now avoids the first
- // match attempt being slow.
- matcher_->train();
+
+ for (auto &matcher : matchers_) {
+ matcher.train();
+ }
event_loop->OnRun(
[this]() { read_image_timer_->Setup(event_loop_->monotonic_now()); });
@@ -82,6 +88,7 @@
const std::vector<cv::Mat> &field_camera_list,
const std::vector<cv::Point2f> &target_point_vector,
const std::vector<float> &target_radius_vector,
+ const std::vector<int> &training_image_indices,
aos::Sender<sift::ImageMatchResult> *result_sender,
bool send_details);
@@ -154,7 +161,7 @@
const sift::TrainingData *const training_data_;
const sift::CameraCalibration *const camera_calibration_;
V4L2Reader *const reader_;
- cv::FlannBasedMatcher *const matcher_;
+ std::vector<cv::FlannBasedMatcher> matchers_;
aos::Sender<CameraImage> image_sender_;
aos::Sender<sift::ImageMatchResult> result_sender_;
aos::Sender<sift::ImageMatchResult> detailed_result_sender_;
@@ -188,6 +195,7 @@
}
void CameraReader::CopyTrainingFeatures() {
+ int training_image_index = 0;
for (const sift::TrainingImage *training_image : *training_data_->images()) {
cv::Mat features(training_image->features()->size(), 128, CV_32F);
for (size_t i = 0; i < training_image->features()->size(); ++i) {
@@ -206,7 +214,8 @@
const auto out_mat = features(cv::Range(i, i + 1), cv::Range(0, 128));
in_mat.convertTo(out_mat, CV_32F);
}
- matcher_->add(features);
+ matchers_[training_image_index].add(features);
+ ++training_image_index;
}
}
@@ -218,6 +227,7 @@
const std::vector<cv::Mat> &field_camera_list,
const std::vector<cv::Point2f> &target_point_vector,
const std::vector<float> &target_radius_vector,
+ const std::vector<int> &training_image_indices,
aos::Sender<sift::ImageMatchResult> *result_sender, bool send_details) {
auto builder = result_sender->MakeBuilder();
const auto camera_calibration_offset =
@@ -252,8 +262,8 @@
sift::CreateTransformationMatrix(*builder.fbb(), fc_data_offset);
const flatbuffers::Offset<sift::TransformationMatrix>
- field_to_target_offset =
- aos::RecursiveCopyFlatBuffer(FieldToTarget(i), builder.fbb());
+ field_to_target_offset = aos::RecursiveCopyFlatBuffer(
+ FieldToTarget(training_image_indices[i]), builder.fbb());
sift::CameraPose::Builder pose_builder(*builder.fbb());
pose_builder.add_camera_to_target(transform_offset);
@@ -302,14 +312,8 @@
sift_->detectAndCompute(image_mat, cv::noArray(), keypoints, descriptors);
}
- // Then, match those features against our training data.
- std::vector<std::vector<cv::DMatch>> matches;
- if (!FLAGS_skip_sift) {
- matcher_->knnMatch(/* queryDescriptors */ descriptors, matches, /* k */ 2);
- }
-
struct PerImageMatches {
- std::vector<const std::vector<cv::DMatch> *> matches;
+ std::vector<std::vector<cv::DMatch>> matches;
std::vector<cv::Point3f> training_points_3d;
std::vector<cv::Point2f> query_points;
std::vector<cv::Point2f> training_points;
@@ -317,32 +321,43 @@
};
std::vector<PerImageMatches> per_image_matches(number_training_images());
- // Pull out the good matches which we want for each image.
- // Discard the bad matches per Lowe's ratio test.
- // (Lowe originally proposed 0.7 ratio, but 0.75 was later proposed as a
- // better option. We'll go with the more conservative (fewer, better matches)
- // for now).
- for (const std::vector<cv::DMatch> &match : matches) {
- CHECK_EQ(2u, match.size());
- CHECK_LE(match[0].distance, match[1].distance);
- CHECK_LT(match[0].imgIdx, number_training_images());
- CHECK_LT(match[1].imgIdx, number_training_images());
- CHECK_EQ(match[0].queryIdx, match[1].queryIdx);
- if (!(match[0].distance < 0.7 * match[1].distance)) {
- continue;
+ for (int image_idx = 0; image_idx < number_training_images(); ++image_idx) {
+ // Then, match those features against our training data.
+ std::vector<std::vector<cv::DMatch>> matches;
+ if (!FLAGS_skip_sift) {
+ matchers_[image_idx].knnMatch(/* queryDescriptors */ descriptors, matches,
+ /* k */ 2);
}
- const int training_image = match[0].imgIdx;
- CHECK_LT(training_image, static_cast<int>(per_image_matches.size()));
- PerImageMatches *const per_image = &per_image_matches[training_image];
- per_image->matches.push_back(&match);
- per_image->training_points.push_back(
- Training2dPoint(training_image, match[0].trainIdx));
- per_image->training_points_3d.push_back(
- Training3dPoint(training_image, match[0].trainIdx));
+ // Pull out the good matches which we want for each image.
+ // Discard the bad matches per Lowe's ratio test.
+ // (Lowe originally proposed 0.7 ratio, but 0.75 was later proposed as a
+ // better option. We'll go with the more conservative (fewer, better
+ // matches) for now).
+ for (const std::vector<cv::DMatch> &match : matches) {
+ CHECK_EQ(2u, match.size());
+ CHECK_LE(match[0].distance, match[1].distance);
+ CHECK_EQ(match[0].imgIdx, 0);
+ CHECK_EQ(match[1].imgIdx, 0);
+ CHECK_EQ(match[0].queryIdx, match[1].queryIdx);
+ if (!(match[0].distance < 0.7 * match[1].distance)) {
+ continue;
+ }
- const cv::KeyPoint &keypoint = keypoints[match[0].queryIdx];
- per_image->query_points.push_back(keypoint.pt);
+ const int training_image = image_idx;
+ CHECK_LT(training_image, static_cast<int>(per_image_matches.size()));
+ PerImageMatches *const per_image = &per_image_matches[training_image];
+ per_image->matches.push_back(match);
+ per_image->matches.back()[0].imgIdx = image_idx;
+ per_image->matches.back()[1].imgIdx = image_idx;
+ per_image->training_points.push_back(
+ Training2dPoint(training_image, match[0].trainIdx));
+ per_image->training_points_3d.push_back(
+ Training3dPoint(training_image, match[0].trainIdx));
+
+ const cv::KeyPoint &keypoint = keypoints[match[0].queryIdx];
+ per_image->query_points.push_back(keypoint.pt);
+ }
}
// The minimum number of matches in a training image for us to use it.
@@ -355,12 +370,13 @@
// Build list of target point and radius for each good match
std::vector<cv::Point2f> target_point_vector;
std::vector<float> target_radius_vector;
+ std::vector<int> training_image_indices;
// Iterate through matches for each training image
for (size_t i = 0; i < per_image_matches.size(); ++i) {
const PerImageMatches &per_image = per_image_matches[i];
- VLOG(2) << "Number of matches to start: " << per_image_matches.size()
+ VLOG(2) << "Number of matches to start: " << per_image.matches.size()
<< "\n";
// If we don't have enough matches to start, skip this set of matches
if (per_image.matches.size() < kMinimumMatchCount) {
@@ -392,8 +408,7 @@
}
// Add this to our collection of all matches that passed our criteria
- all_good_matches.push_back(
- static_cast<std::vector<cv::DMatch>>(*per_image.matches[j]));
+ all_good_matches.push_back(per_image.matches[j]);
// Fill out the data for matches per image that made it past
// homography check, for later use
@@ -415,8 +430,7 @@
// Collect training target location, so we can map it to matched image
cv::Point2f target_point;
float target_radius;
- TargetLocation((*(per_image_good_match.matches[0]))[0].imgIdx, target_point,
- target_radius);
+ TargetLocation(i, target_point, target_radius);
// Store target_point in vector for use by perspectiveTransform
std::vector<cv::Point2f> src_target_pt;
@@ -494,6 +508,8 @@
const_cast<void *>(static_cast<const void *>(
FieldToTarget(i)->data()->data())));
+ training_image_indices.push_back(i);
+
const cv::Mat R_field_target =
H_field_target(cv::Range(0, 3), cv::Range(0, 3));
const cv::Mat T_field_target =
@@ -532,11 +548,11 @@
SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
camera_target_list, field_camera_list,
target_point_vector, target_radius_vector,
- &detailed_result_sender_, true);
+ training_image_indices, &detailed_result_sender_, true);
SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
camera_target_list, field_camera_list,
target_point_vector, target_radius_vector,
- &result_sender_, false);
+ training_image_indices, &result_sender_, false);
}
void CameraReader::ReadImage() {
@@ -656,7 +672,7 @@
V4L2Reader v4l2_reader(&event_loop, "/dev/video0");
CameraReader camera_reader(&event_loop, &training_data.message(),
- &v4l2_reader, &matcher);
+ &v4l2_reader, index_params, search_params);
event_loop.Run();
}
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index 6172ae7..727cf70 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -57,7 +57,7 @@
turret_cam_ext.R = np.array(
camera_pitch_matrix *
np.matrix([[0., 0., 1.], [-1, 0, 0], [0, -1., 0]]))
- turret_cam_ext.T = np.array([15.0 * 0.0254, 15.0 * 0.0254, 41.0 * 0.0254])
+ turret_cam_ext.T = np.array([7.5 * 0.0254, -5.5 * 0.0254, 41.0 * 0.0254])
default_cam_ext = CameraExtrinsics()
default_cam_ext.R = np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
[0.0, 0.0, 1.0]])
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index 2654d35..900c72a 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -354,10 +354,10 @@
]
# Populate the blue power port
- # ideal_power_port_blue.polygon_list.append(
- # power_port_blue_main_panel_polygon_points_2d)
- # ideal_power_port_blue.polygon_list_3d.append(
- # power_port_blue_main_panel_polygon_points_3d)
+ ideal_power_port_blue.polygon_list.append(
+ power_port_blue_main_panel_polygon_points_2d)
+ ideal_power_port_blue.polygon_list_3d.append(
+ power_port_blue_main_panel_polygon_points_3d)
# Including the wing panel
ideal_power_port_blue.polygon_list.append(
power_port_blue_wing_panel_polygon_points_2d)
@@ -375,7 +375,6 @@
training_target_power_port_blue = TargetData(
'test_images/train_power_port_blue.png')
- # 'test_images/image_from_ios-scaled.jpg')
training_target_power_port_blue.target_rotation = ideal_power_port_blue.target_rotation
training_target_power_port_blue.target_position = ideal_power_port_blue.target_position
training_target_power_port_blue.target_radius = target_radius_default
@@ -443,9 +442,9 @@
#training_target_list.append(training_target_loading_bay_red)
### Blue Power Port
- #glog.info("Adding blue power port to the model list")
- #ideal_target_list.append(ideal_power_port_blue)
- #training_target_list.append(training_target_power_port_blue)
+ glog.info("Adding blue power port to the model list")
+ ideal_target_list.append(ideal_power_port_blue)
+ training_target_list.append(training_target_power_port_blue)
### Blue Loading Bay
#glog.info("Adding blue loading bay to the model list")
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 3e91d1e..fb46d71 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -349,6 +349,7 @@
::std::unique_ptr<::frc::Encoder> finisher_encoder_,
left_accelerator_encoder_, right_accelerator_encoder_;
+
::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
frc971::wpilib::ADIS16470 *imu_ = nullptr;
@@ -415,6 +416,11 @@
}
}
+ void set_washing_machine_control_panel_victor(
+ ::std::unique_ptr<::frc::VictorSP> t) {
+ washing_machine_control_panel_victor_ = ::std::move(t);
+ }
+
void set_accelerator_left_falcon(::std::unique_ptr<::frc::TalonFX> t) {
accelerator_left_falcon_ = ::std::move(t);
}
@@ -456,7 +462,7 @@
kMaxBringupPower) /
12.0);
- turret_victor_->SetSpeed(std::clamp(-output.turret_voltage(),
+ turret_victor_->SetSpeed(std::clamp(output.turret_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
@@ -464,6 +470,12 @@
std::clamp(output.feeder_voltage(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
+ if (washing_machine_control_panel_victor_) {
+ washing_machine_control_panel_victor_->SetSpeed(
+ std::clamp(-output.washing_machine_spinner_voltage(),
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ }
accelerator_left_falcon_->SetSpeed(
std::clamp(-output.accelerator_left_voltage(), -kMaxBringupPower,
@@ -498,6 +510,9 @@
hood_victor_->SetDisabled();
intake_joint_victor_->SetDisabled();
turret_victor_->SetDisabled();
+ if (washing_machine_control_panel_victor_) {
+ washing_machine_control_panel_victor_->SetDisabled();
+ }
accelerator_left_falcon_->SetDisabled();
accelerator_right_falcon_->SetDisabled();
finisher_falcon0_->SetDisabled();
@@ -509,7 +524,7 @@
}
::std::unique_ptr<::frc::VictorSP> hood_victor_, intake_joint_victor_,
- turret_victor_;
+ turret_victor_, washing_machine_control_panel_victor_;
::std::unique_ptr<::frc::TalonFX> accelerator_left_falcon_,
accelerator_right_falcon_, finisher_falcon0_, finisher_falcon1_;
@@ -611,6 +626,8 @@
superstructure_writer.set_turret_victor(make_unique<frc::VictorSP>(7));
superstructure_writer.set_feeder_falcon(
make_unique<ctre::phoenix::motorcontrol::can::TalonFX>(1));
+ superstructure_writer.set_washing_machine_control_panel_victor(
+ make_unique<frc::VictorSP>(6));
superstructure_writer.set_accelerator_left_falcon(
make_unique<::frc::TalonFX>(5));
superstructure_writer.set_accelerator_right_falcon(