Add roller velocity compensation
When we are moving faster forwards, we want the roller to be spinning
faster to compensate for the ground velocity. Otherwise we push balls
away. This also makes it so we don't throw them as hard when moving
slow.
Added tests for intake roller voltage
Change-Id: Ica015d124c7a192bcd001dbc02330f71d6616b04
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 5825475..1cd3295 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -79,6 +79,7 @@
double intake_pos = -0.89;
double turret_pos = 0.0;
float roller_speed = 0.0f;
+ float roller_speed_compensation = 0.0f;
double accelerator_speed = 0.0;
double finisher_speed = 0.0;
double climber_speed = 0.0;
@@ -107,24 +108,24 @@
}
} else if (data.IsPressed(kShootSlow)) {
accelerator_speed = 180.0;
- finisher_speed = 375.0;
+ finisher_speed = 300.0;
}
if (data.IsPressed(kIntakeExtend)) {
intake_pos = 1.2;
- roller_speed = 9.0f;
+ roller_speed = 7.0f;
+ roller_speed_compensation = 2.0f;
}
if (superstructure_status_fetcher_.get() &&
superstructure_status_fetcher_->intake()->position() > -0.5) {
roller_speed = std::max(roller_speed, 6.0f);
- }
-
- if (data.IsPressed(kFeed)) {
+ roller_speed_compensation = 2.0f;
}
if (data.IsPressed(kIntakeIn)) {
roller_speed = 6.0f;
+ roller_speed_compensation = 2.0f;
} else if (data.IsPressed(kSpit)) {
roller_speed = -6.0f;
}
@@ -163,6 +164,8 @@
superstructure_goal_builder.add_intake(intake_offset);
superstructure_goal_builder.add_turret(turret_offset);
superstructure_goal_builder.add_roller_voltage(roller_speed);
+ superstructure_goal_builder.add_roller_speed_compensation(
+ roller_speed_compensation);
superstructure_goal_builder.add_shooter(shooter_offset);
superstructure_goal_builder.add_shooting(data.IsPressed(kFeed));
superstructure_goal_builder.add_climber_voltage(climber_speed);