Added precise heading mode.
Change-Id: Ic06cdc4fefdbe39b9d1040b9da2ac2f5c8e1bf81
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index c28a0ff..da09eb1 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -35,6 +35,9 @@
const ButtonLocation kShiftHigh(2, 3), kShiftHigh2(2, 2), kShiftLow(2, 1);
const ButtonLocation kQuickTurn(1, 5);
+const ButtonLocation kTurn1(1, 7);
+const ButtonLocation kTurn2(1, 11);
+
// Buttons on the lexan driver station to get things running on bring-up day.
const ButtonLocation kTest1(3, 6);
const ButtonLocation kTest2(3, 2);
@@ -73,19 +76,31 @@
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
bool is_control_loop_driving = false;
- double left_goal = 0.0;
- double right_goal = 0.0;
+ static double left_goal = 0.0;
+ static double right_goal = 0.0;
+
const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
+ if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get()) {
+ const double delta = data.PosEdge(kTurn2) ? 0.1 : -0.1;
+ left_goal = drivetrain_queue.status->estimated_left_position + delta;
+ right_goal = drivetrain_queue.status->estimated_right_position - delta;
+ }
+ }
+ if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
+ is_control_loop_driving = true;
+ }
if (!drivetrain_queue.goal.MakeWithBuilder()
.steering(wheel)
.throttle(throttle)
.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
.control_loop_driving(is_control_loop_driving)
- .left_goal(left_goal)
- .right_goal(right_goal)
+ .left_goal(left_goal + wheel * 0.5 + throttle * 0.3)
+ .right_goal(right_goal - wheel * 0.5 + throttle * 0.3)
.left_velocity_goal(0)
.right_velocity_goal(0)
.Send()) {