Added basic autonomous.
diff --git a/bot3/autonomous/auto.cc b/bot3/autonomous/auto.cc
index 3167770..8a3c1ad 100644
--- a/bot3/autonomous/auto.cc
+++ b/bot3/autonomous/auto.cc
@@ -8,6 +8,7 @@
#include "bot3/autonomous/auto.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
#include "frc971/constants.h"
using ::aos::time::Time;
@@ -15,11 +16,41 @@
namespace bot3 {
namespace autonomous {
+bool ShouldExitAuto() {
+ ::bot3::autonomous::autonomous.FetchLatest();
+ bool ans = !::bot3::autonomous::autonomous->run_auto;
+ if (ans) {
+ LOG(INFO, "Time to exit auto mode\n");
+ }
+ return ans;
+}
+
+void SetShooter(double velocity, bool push) {
+ LOG(INFO, "Setting shooter velocity to %f\n", velocity);
+ control_loops::shooter.goal.MakeWithBuilder()
+ .velocity(velocity).push(push).Send();
+}
+
+bool ShooterReady() {
+ control_loops::shooter.status.FetchLatest();
+ return control_loops::shooter.status->ready;
+}
+
// start with N discs in the indexer
void HandleAuto() {
- // TODO (danielp): Do something in auto.
- // (That's why I left all the includes.)
- LOG(INFO, "Auto mode is not currently implemented on this robot.\n");
+ double shooter_velocity = 250.0;
+ while (!ShouldExitAuto()) {
+ SetShooter(shooter_velocity, false);
+ while (!ShouldExitAuto() && !ShooterReady());
+ if (ShouldExitAuto()) return;
+ SetShooter(shooter_velocity, true);
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.5));
+ SetShooter(shooter_velocity, false);
+ // Waits because, until we have feedback,
+ // the shooter will always think it's ready.
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.5));
+ }
+ return;
}
} // namespace autonomous