Add can grabber auto.
Change-Id: I47b2c93916018dce3923a8a66c1ac2c663d1f7a1
diff --git a/bot3/autonomous/auto.cc b/bot3/autonomous/auto.cc
index 0a2f99a..3e770ba 100644
--- a/bot3/autonomous/auto.cc
+++ b/bot3/autonomous/auto.cc
@@ -10,7 +10,7 @@
#include "bot3/autonomous/auto.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/actors/drivetrain_actor.h"
+#include "bot3/control_loops/drivetrain/drivetrain.h"
using ::aos::time::Time;
using ::bot3::control_loops::drivetrain_queue;
@@ -25,6 +25,8 @@
namespace time = ::aos::time;
+static double left_initial_position, right_initial_position;
+
bool ShouldExitAuto() {
::bot3::autonomous::autonomous.FetchLatest();
bool ans = !::bot3::autonomous::autonomous->run_auto;
@@ -34,13 +36,90 @@
return ans;
}
+void ResetDrivetrain() {
+ LOG(INFO, "resetting the drivetrain\n");
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .steering(0.0)
+ .throttle(0.0)
+ .left_goal(left_initial_position)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position)
+ .right_velocity_goal(0)
+ .Send();
+}
+
+void InitializeEncoders() {
+ control_loops::drivetrain_queue.status.FetchAnother();
+ left_initial_position =
+ control_loops::drivetrain_queue.status->filtered_left_position;
+ right_initial_position =
+ control_loops::drivetrain_queue.status->filtered_right_position;
+}
+
+void GrabberForTime(double voltage, double wait_time) {
+ ::aos::time::Time now = ::aos::time::Time::Now();
+ ::aos::time::Time end_time = now + time::Time::InSeconds(wait_time);
+ LOG(INFO, "Starting to grab at %f for %f seconds\n", voltage, wait_time);
+
+ while (true) {
+ autonomous::can_grabber_control.MakeWithBuilder()
+ .can_grabber_voltage(voltage)
+ .Send();
+
+ // Poll the running bit and auto done bits.
+ if (ShouldExitAuto()) {
+ return;
+ }
+ if (::aos::time::Time::Now() > end_time) {
+ LOG(INFO, "Done grabbing\n");
+ return;
+ }
+ ::aos::time::PhasedLoopXMS(5, 2500);
+ }
+}
+
+// Auto methods.
+void CanGrabberAuto() {
+ ResetDrivetrain();
+
+ // Launch can grabbers.
+ GrabberForTime(12.0, 0.26);
+ if (ShouldExitAuto()) return;
+ InitializeEncoders();
+ ResetDrivetrain();
+ if (ShouldExitAuto()) return;
+
+ // Drive backwards, and pulse the can grabbers again to tip the cans.
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .steering(0.0)
+ .throttle(0.0)
+ .left_goal(left_initial_position + 1.75)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position + 1.75)
+ .right_velocity_goal(0)
+ .Send();
+ GrabberForTime(12.0, 0.02);
+ if (ShouldExitAuto()) return;
+
+ // We shouldn't need as much power at this point, so lower the can grabber
+ // voltages to avoid damaging the motors due to stalling.
+ GrabberForTime(4.0, 0.75);
+ if (ShouldExitAuto()) return;
+ GrabberForTime(-3.0, 0.25);
+ if (ShouldExitAuto()) return;
+ GrabberForTime(-12.0, 1.0);
+ if (ShouldExitAuto()) return;
+ GrabberForTime(-3.0, 12.0);
+}
+
void HandleAuto() {
::aos::time::Time start_time = ::aos::time::Time::Now();
LOG(INFO, "Starting auto mode at %f\n", start_time.ToSeconds());
- ::std::unique_ptr<::frc971::actors::DrivetrainAction> drive;
- LOG(INFO, "Doing nothing in auto.\n");
- if (ShouldExitAuto()) return;
+ // TODO(comran): Add various options for different autos down below.
+ CanGrabberAuto();
}
} // namespace autonomous