Improve intaking.
Add a stuck box detector (box intake velocity), and better handling for
boxes coming in sideways.
Change-Id: I5fa4a5b00f7dd1288f8fed33375953bdfe710559
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index b644ca1..f204605 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -725,8 +725,10 @@
virtual void Write() override {
auto &queue = ::frc971::control_loops::drivetrain_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left_victor_->SetSpeed(queue->left_voltage / 12.0);
- drivetrain_right_victor_->SetSpeed(-queue->right_voltage / 12.0);
+ drivetrain_left_victor_->SetSpeed(
+ ::aos::Clip(queue->left_voltage, -12.0, 12.0) / 12.0);
+ drivetrain_right_victor_->SetSpeed(
+ ::aos::Clip(-queue->right_voltage, -12.0, 12.0) / 12.0);
}
virtual void Stop() override {