Very basic auto mode code.
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index e9c787e..45083ec 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -15,6 +15,7 @@
'../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_csv',
'../control_loops/shooter/shooter.gyp:shooter_lib_test',
'../control_loops/shooter/shooter.gyp:shooter',
+ '../autonomous/autonomous.gyp:auto',
'../input/input.gyp:JoystickReader',
'../input/input.gyp:SensorReader',
'../input/input.gyp:GyroReader',
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
new file mode 100644
index 0000000..99d0598
--- /dev/null
+++ b/frc971/autonomous/auto.cc
@@ -0,0 +1,78 @@
+#include "stdio.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/wrist/wrist_motor.q.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace autonomous {
+
+void SetShooterVelocity(double velocity) {
+ control_loops::shooter.goal.MakeWithBuilder()
+ .velocity(velocity).Send();
+}
+
+void StartIntake() {
+ control_loops::intake.goal.MakeWithBuilder()
+ .goal_state(2).Send();
+}
+
+void PreloadIntake() {
+ control_loops::intake.goal.MakeWithBuilder()
+ .goal_state(3).Send();
+}
+
+void ShootIntake() {
+ control_loops::intake.goal.MakeWithBuilder()
+ .goal_state(4).Send();
+}
+
+void WaitForShooter() {
+ control_loops::shooter.status.FetchLatest();
+ while (!control_loops::shooter.status->ready) {
+ control_loops::shooter.status.FetchNextBlocking();
+ }
+}
+
+// TODO(aschuh): Send the number of discs shot in the status message.
+void ShootNDiscs(int n) {
+ control_loops::intake.status.FetchLatest();
+ while (!control_loops::intake.status->ready) {
+ control_loops::intake.status.FetchNextBlocking();
+ }
+}
+
+bool ShouldExitAuto() {
+ ::frc971::autonomous::autonomous.FetchLatest();
+ return !::frc971::autonomous::autonomous->run_auto;
+}
+
+// TODO(aschuh): Drivetrain needs a profile somewhere.
+// Here isn't the best spot. It should be in another thread/process
+
+void HandleAuto() {
+ SetShooterVelocity(200.0);
+ PreloadIntake();
+
+ WaitForShooter();
+ ShootIntake();
+
+ // Wait for the wrist and angle adjust to finish zeroing before shooting.
+ // Sigh, constants go where?
+
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .left_goal(0.0)
+ .right_goal(0.0).Send();
+}
+
+} // namespace autonomous
+} // namespace frc971
diff --git a/frc971/autonomous/auto.h b/frc971/autonomous/auto.h
new file mode 100644
index 0000000..00d2d72
--- /dev/null
+++ b/frc971/autonomous/auto.h
@@ -0,0 +1,11 @@
+#ifndef FRC971_AUTONOMOUS_AUTO_H_
+#define FRC971_AUTONOMOUS_AUTO_H_
+
+namespace frc971 {
+namespace autonomous {
+
+void HandleAuto();
+
+} // namespace autonomous
+} // namespace frc971
+#endif // FRC971_AUTONOMOUS_AUTO_H_
diff --git a/frc971/autonomous/auto.q b/frc971/autonomous/auto.q
new file mode 100644
index 0000000..5791034
--- /dev/null
+++ b/frc971/autonomous/auto.q
@@ -0,0 +1,8 @@
+package frc971.autonomous;
+
+message AutoControl {
+ // True if auto mode should be running, false otherwise.
+ bool run_auto;
+};
+
+queue AutoControl autonomous;
diff --git a/frc971/autonomous/auto_main.cc b/frc971/autonomous/auto_main.cc
new file mode 100644
index 0000000..56b98fb
--- /dev/null
+++ b/frc971/autonomous/auto_main.cc
@@ -0,0 +1,32 @@
+#include "stdio.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/auto.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ ::frc971::autonomous::autonomous.FetchLatest();
+ while (!::frc971::autonomous::autonomous.get()) {
+ ::frc971::autonomous::autonomous.FetchNextBlocking();
+ }
+
+ while (true) {
+ while (!::frc971::autonomous::autonomous->run_auto) {
+ ::frc971::autonomous::autonomous.FetchNextBlocking();
+ }
+ ::frc971::autonomous::HandleAuto();
+
+ while (::frc971::autonomous::autonomous->run_auto) {
+ ::frc971::autonomous::autonomous.FetchNextBlocking();
+ }
+ }
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
new file mode 100644
index 0000000..4892975
--- /dev/null
+++ b/frc971/autonomous/autonomous.gyp
@@ -0,0 +1,56 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'auto_queue',
+ 'type': 'static_library',
+ 'sources': ['auto.q'],
+ 'variables': {
+ 'header_path': 'frc971/autonomous',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'auto_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'auto.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'auto_queue',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
+ '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:controls',
+ ],
+ },
+ {
+ 'target_name': 'auto',
+ 'type': 'executable',
+ 'sources': [
+ 'auto_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'auto_queue',
+ 'auto_lib',
+ ],
+ },
+ ],
+}