Fixed control loop driving in drivetrain_input
Change-Id: I9de8c94e5378a8db2299d5d5b3d2c6c121c7d8dc
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 02957a8..f9cc45d 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -62,12 +62,13 @@
}
}
- if (is_control_loop_driving) {
- if (drivetrain_queue.status.get()) {
+ if (drivetrain_queue.status.get()) {
+ if (is_control_loop_driving && !last_is_control_loop_driving_) {
left_goal_ = drivetrain_queue.status->estimated_left_position;
right_goal_ = drivetrain_queue.status->estimated_right_position;
}
}
+
const double current_left_goal =
left_goal_ - wheel * wheel_multiplier_ + throttle * 0.3;
const double current_right_goal =
@@ -92,6 +93,8 @@
if (!new_drivetrain_goal.Send()) {
LOG(WARNING, "sending stick values failed\n");
}
+
+ last_is_control_loop_driving_ = is_control_loop_driving;
}
DrivetrainInputReader::WheelAndThrottle
@@ -242,10 +245,13 @@
// TODO(james): Make a copy assignment operator for ButtonLocation so we don't
// have to shoehorn in these ternary operators.
- const ButtonLocation kTurn1 =
+ const ButtonLocation kTurn1 = (top_button_use == TopButtonUse::kLineFollow)
+ ? SecondButton
+ : DummyButton;
+ // Turn2 does closed loop driving.
+ const ButtonLocation kTurn2 =
(top_button_use == TopButtonUse::kLineFollow) ? TopButton : DummyButton;
- // Turn2 currently does nothing on the pistol grip, ever.
- const ButtonLocation kTurn2 = DummyButton;
+
const ButtonLocation kShiftHigh =
(top_button_use == TopButtonUse::kShift) ? TopButton : DummyButton;
const ButtonLocation kShiftLow =
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index 80046ee..f4b0212 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -96,6 +96,8 @@
const driver_station::ButtonLocation turn2_;
const TurnButtonUse turn2_use_;
+ bool last_is_control_loop_driving_ = false;
+
// Structure containing the (potentially adjusted) steering and throttle
// values from the joysticks.
struct WheelAndThrottle {
@@ -183,7 +185,7 @@
driver_station::ButtonLocation turn2)
: DrivetrainInputReader(wheel_high, throttle_high, quick_turn, turn1,
TurnButtonUse::kLineFollow, turn2,
- TurnButtonUse::kLineFollow),
+ TurnButtonUse::kControlLoopDriving),
wheel_low_(wheel_low),
wheel_velocity_high_(wheel_velocity_high),
wheel_velocity_low_(wheel_velocity_low),
diff --git a/y2019/constants.cc b/y2019/constants.cc
index dfd3c69..b2e4f3d 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -186,8 +186,9 @@
wrist_params->zeroing_constants.measured_absolute_position = 0.163840;
wrist->potentiometer_offset = -4.257454;
- stilts_params->zeroing_constants.measured_absolute_position = 0.030049;
- stilts->potentiometer_offset = -0.015760 + 0.011604;
+ stilts_params->zeroing_constants.measured_absolute_position = 0.072829;
+ stilts->potentiometer_offset = -0.015760 + 0.011604 - 0.061213;
+ FillCameraPoses(vision::CompBotTeensyId(), &r->cameras);
break;
case kPracticeTeamNumber:
diff --git a/y2019/vision/constants.h b/y2019/vision/constants.h
index faa16e1..b61bff7 100644
--- a/y2019/vision/constants.h
+++ b/y2019/vision/constants.h
@@ -82,6 +82,7 @@
// Serial number of the teensy for each robot.
constexpr uint32_t CodeBotTeensyId() { return 0xffff322e; }
constexpr uint32_t PracticeBotTeensyId() { return 0xffff3215; }
+constexpr uint32_t CompBotTeensyId() { return 0xffff3210; }
// Get the IDs of the cameras in each port for a particular teensy board.
// inlined so that we don't have to deal with including it in the autogenerated
@@ -92,6 +93,8 @@
return {{0, 0, 0, 16, 19}};
case PracticeBotTeensyId():
return {{14, 15, 18, 17, 1}};
+ case CompBotTeensyId():
+ return {{6, 7, 8, 9, 10}};
default:
return {{0, 0, 0, 0, 0}};
}