Copy over 2014 bot code to the directory.
This is purely for the code review process. It does not compile.
Change-Id: I7617d245891b3218a98d3b21a3492763e22e0f88
diff --git a/y2014_bot3/autonomous/auto.cc b/y2014_bot3/autonomous/auto.cc
new file mode 100644
index 0000000..ab9ffa5
--- /dev/null
+++ b/y2014_bot3/autonomous/auto.cc
@@ -0,0 +1,177 @@
+#include <stdio.h>
+
+#include <memory>
+
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/time.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/logging/logging.h"
+
+#include "bot3/actions/drivetrain_action.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
+#include "bot3/control_loops/rollers/rollers.q.h"
+#include "frc971/actions/action_client.h"
+#include "frc971/actions/drivetrain_action.q.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/queues/other_sensors.q.h"
+
+using ::aos::time::Time;
+
+namespace bot3 {
+namespace autonomous {
+
+namespace time = ::aos::time;
+
+static double left_initial_position, right_initial_position;
+
+bool ShouldExitAuto() {
+ ::frc971::autonomous::autonomous.FetchLatest();
+ bool ans = !::frc971::autonomous::autonomous->run_auto;
+ if (ans) {
+ LOG(INFO, "Time to exit auto mode\n");
+ }
+ return ans;
+}
+
+void StopDrivetrain() {
+ LOG(INFO, "Stopping the drivetrain\n");
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .left_goal(left_initial_position)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position)
+ .right_velocity_goal(0)
+ .quickturn(false)
+ .Send();
+}
+
+void ResetDrivetrain() {
+ LOG(INFO, "resetting the drivetrain\n");
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .highgear(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 WaitUntilDoneOrCanceled(::frc971::Action *action) {
+ while (true) {
+ // Poll the running bit and auto done bits.
+ ::aos::time::PhasedLoop10MS(5000);
+ if (!action->Running() || ShouldExitAuto()) {
+ return;
+ }
+ }
+}
+
+::std::unique_ptr<::frc971::TypedAction
+ < ::frc971::actions::DrivetrainActionQueueGroup>>
+SetDriveGoal(double distance, bool slow_acceleration,
+ double maximum_velocity = 1.7, double theta = 0) {
+ LOG(INFO, "Driving to %f\n", distance);
+ auto drivetrain_action = actions::MakeDrivetrainAction();
+ drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
+ drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
+ drivetrain_action->GetGoal()->y_offset = distance;
+ drivetrain_action->GetGoal()->theta_offset = theta;
+ drivetrain_action->GetGoal()->maximum_velocity = maximum_velocity;
+ drivetrain_action->GetGoal()->maximum_acceleration =
+ slow_acceleration ? 2.5 : 3.0;
+ drivetrain_action->Start();
+ left_initial_position +=
+ distance - theta * control_loops::kBot3TurnWidth / 2.0;
+ right_initial_position +=
+ distance + theta * control_loops::kBot3TurnWidth / 2.0;
+ return ::std::move(drivetrain_action);
+}
+
+void InitializeEncoders() {
+ control_loops::drivetrain.status.FetchLatest();
+ while (!control_loops::drivetrain.status.get()) {
+ LOG(WARNING,
+ "No previous drivetrain position packet, trying to fetch again\n");
+ control_loops::drivetrain.status.FetchNextBlocking();
+ }
+ left_initial_position =
+ control_loops::drivetrain.status->filtered_left_position;
+ right_initial_position =
+ control_loops::drivetrain.status->filtered_right_position;
+
+}
+
+void StopRollers() {
+ control_loops::rollers.goal.MakeWithBuilder()
+ .intake(0)
+ .low_spit(0)
+ .human_player(false)
+ .Send();
+}
+
+void SpitBallFront() {
+ control_loops::rollers.goal.MakeWithBuilder()
+ .intake(0)
+ .low_spit(1)
+ .human_player(false)
+ .Send();
+ time::SleepFor(time::Time::InSeconds(1));
+ StopRollers();
+}
+
+void IntakeBallBack() {
+ control_loops::rollers.goal.MakeWithBuilder()
+ .intake(-1)
+ .low_spit(0)
+ .human_player(false)
+ .Send();
+ time::SleepFor(time::Time::InSeconds(1.5));
+ StopRollers();
+}
+
+void ScoreBall(const double distance, const double velocity) {
+ // Drive to the goal, score, and drive back.
+ {
+ // Drive forward.
+ auto drivetrain_action = SetDriveGoal(distance,
+ false, velocity);
+ LOG(INFO, "Waiting until drivetrain is finished.\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ time::SleepFor(time::Time::InSeconds(0.5));
+ if (ShouldExitAuto()) return;
+ }
+ {
+ LOG(INFO, "Spitting ball.\n");
+ SpitBallFront();
+ time::SleepFor(time::Time::InSeconds(0.5));
+ if (ShouldExitAuto()) return;
+ }
+ {
+ // Drive back.
+ LOG(INFO, "Driving back.\n");
+ auto drivetrain_action = SetDriveGoal(-distance,
+ false, velocity);
+ LOG(INFO, "Waiting until drivetrain is finished.\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+}
+
+void HandleAuto() {
+ constexpr double kDriveDistance = 4.86;
+ constexpr double kAutoVelocity = 2.5;
+
+ if (ShouldExitAuto()) return;
+ ResetDrivetrain();
+ InitializeEncoders();
+ if (ShouldExitAuto()) return;
+
+ ScoreBall(kDriveDistance, kAutoVelocity);
+}
+
+} // namespace autonomous
+} // namespace bot3
diff --git a/y2014_bot3/autonomous/auto.h b/y2014_bot3/autonomous/auto.h
new file mode 100644
index 0000000..896d22c
--- /dev/null
+++ b/y2014_bot3/autonomous/auto.h
@@ -0,0 +1,12 @@
+#ifndef BOT3_AUTONOMOUS_AUTO_H_
+#define BOT3_AUTONOMOUS_AUTO_H_
+
+namespace bot3 {
+namespace autonomous {
+
+void HandleAuto();
+
+} // namespace autonomous
+} // namespace bot3
+
+#endif // BOT3_AUTONOMOUS_AUTO_H_
diff --git a/y2014_bot3/autonomous/auto_main.cc b/y2014_bot3/autonomous/auto_main.cc
new file mode 100644
index 0000000..eb8f865
--- /dev/null
+++ b/y2014_bot3/autonomous/auto_main.cc
@@ -0,0 +1,42 @@
+#include <stdio.h>
+
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "bot3/autonomous/auto.h"
+#include "frc971/autonomous/auto.q.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ LOG(INFO, "Auto main started\n");
+ ::frc971::autonomous::autonomous.FetchLatest();
+ while (!::frc971::autonomous::autonomous.get()) {
+ ::frc971::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
+ }
+
+ while (true) {
+ while (!::frc971::autonomous::autonomous->run_auto) {
+ ::frc971::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
+ }
+ LOG(INFO, "Starting auto mode\n");
+ ::aos::time::Time start_time = ::aos::time::Time::Now();
+ ::bot3::autonomous::HandleAuto();
+
+ ::aos::time::Time elapsed_time = ::aos::time::Time::Now() - start_time;
+ LOG(INFO, "Auto mode exited in %f, waiting for it to finish.\n",
+ elapsed_time.ToSeconds());
+ while (::frc971::autonomous::autonomous->run_auto) {
+ ::frc971::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Got another auto packet\n");
+ }
+ LOG(INFO, "Waiting for auto to start back up.\n");
+ }
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/y2014_bot3/autonomous/autonomous.gyp b/y2014_bot3/autonomous/autonomous.gyp
new file mode 100644
index 0000000..01d95d0
--- /dev/null
+++ b/y2014_bot3/autonomous/autonomous.gyp
@@ -0,0 +1,50 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'auto_queue',
+ 'type': 'static_library',
+ 'sources': [
+ '<(DEPTH)/frc971/autonomous/auto.q',
+ ],
+ 'variables': {
+ 'header_path': 'frc971/autonomous',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'auto_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'auto.cc',
+ ],
+ 'dependencies': [
+ 'auto_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/rollers/rollers.gyp:rollers_loop',
+ '<(DEPTH)/bot3/actions/actions.gyp:action_client',
+ '<(DEPTH)/bot3/actions/actions.gyp:drivetrain_action_lib',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ },
+ {
+ 'target_name': 'auto',
+ 'type': 'executable',
+ 'sources': [
+ 'auto_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'auto_queue',
+ 'auto_lib',
+ ],
+ },
+ ],
+}