Convert y2014 auto and joystick_reader to new frameworks
This gives us an event loop so we can move hot goal over.
Change-Id: Ifc2783fce0aac22d54ee85f2633466e10824a3c7
diff --git a/y2014/BUILD b/y2014/BUILD
index bf8f689..ee60c7c 100644
--- a/y2014/BUILD
+++ b/y2014/BUILD
@@ -29,7 +29,7 @@
":constants",
"//aos:init",
"//aos/actions:action_lib",
- "//aos/input:joystick_input",
+ "//aos/input:action_joystick_input",
"//aos/logging",
"//aos/time",
"//aos/util:log_interval",
@@ -38,6 +38,7 @@
"//frc971/queues:gyro",
"//y2014/actors:shoot_action_lib",
"//y2014/control_loops/claw:claw_queue",
+ "//y2014/control_loops/drivetrain:drivetrain_base",
"//y2014/control_loops/shooter:shooter_queue",
],
)
@@ -50,7 +51,6 @@
"//y2014/control_loops/drivetrain:drivetrain",
"//y2014/control_loops/claw:claw",
"//y2014/control_loops/shooter:shooter",
- "//y2014/autonomous:auto",
"//y2014/actors:binaries",
],
)
diff --git a/y2014/actors/BUILD b/y2014/actors/BUILD
index 09eb3b0..cb4a8b7 100644
--- a/y2014/actors/BUILD
+++ b/y2014/actors/BUILD
@@ -1,107 +1,101 @@
-package(default_visibility = ['//visibility:public'])
-
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
filegroup(
- name = 'binaries',
- srcs = [
- ':drivetrain_action',
- ':shoot_action',
- ],
+ name = "binaries",
+ srcs = [
+ ":autonomous_action",
+ ":shoot_action",
+ ],
+ visibility = ["//visibility:public"],
)
filegroup(
- name = 'binaries.stripped',
- srcs = [
- ':drivetrain_action.stripped',
- ':shoot_action.stripped',
- ],
+ name = "binaries.stripped",
+ srcs = [
+ ":autonomous_action.stripped",
+ ":shoot_action.stripped",
+ ],
+ visibility = ["//visibility:public"],
)
queue_library(
- name = 'shoot_action_queue',
- srcs = [
- 'shoot_action.q',
- ],
- deps = [
- '//aos/actions:action_queue',
- ],
+ name = "shoot_action_queue",
+ srcs = [
+ "shoot_action.q",
+ ],
+ deps = [
+ "//aos/actions:action_queue",
+ ],
)
cc_library(
- name = 'shoot_action_lib',
- srcs = [
- 'shoot_actor.cc',
- ],
- hdrs = [
- 'shoot_actor.h',
- ],
- deps = [
- ':shoot_action_queue',
- '//aos/actions:action_lib',
- '//y2014/queues:profile_params',
- '//aos/logging',
- '//y2014/control_loops/shooter:shooter_queue',
- '//y2014/control_loops/claw:claw_queue',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//y2014:constants',
- ],
+ name = "shoot_action_lib",
+ srcs = [
+ "shoot_actor.cc",
+ ],
+ hdrs = [
+ "shoot_actor.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":shoot_action_queue",
+ "//aos/actions:action_lib",
+ "//aos/logging",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//y2014:constants",
+ "//y2014/control_loops/claw:claw_queue",
+ "//y2014/control_loops/shooter:shooter_queue",
+ "//y2014/queues:profile_params",
+ ],
)
cc_binary(
- name = 'shoot_action',
- srcs = [
- 'shoot_actor_main.cc',
- ],
- deps = [
- '//aos:init',
- ':shoot_action_lib',
- ':shoot_action_queue',
- ],
-)
-
-queue_library(
- name = 'drivetrain_action_queue',
- srcs = [
- 'drivetrain_action.q',
- ],
- deps = [
- '//aos/actions:action_queue',
- ],
+ name = "shoot_action",
+ srcs = [
+ "shoot_actor_main.cc",
+ ],
+ deps = [
+ ":shoot_action_lib",
+ ":shoot_action_queue",
+ "//aos:init",
+ ],
)
cc_library(
- name = 'drivetrain_action_lib',
- srcs = [
- 'drivetrain_actor.cc',
- ],
- hdrs = [
- 'drivetrain_actor.h',
- ],
- deps = [
- ':drivetrain_action_queue',
- '//y2014:constants',
- '//aos/time:time',
- '//aos:math',
- '//aos/util:phased_loop',
- '//aos/logging',
- '//aos/actions:action_lib',
- '//aos/logging:queue_logging',
- '//third_party/eigen',
- '//aos/util:trapezoid_profile',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "autonomous_action_lib",
+ srcs = [
+ "autonomous_actor.cc",
+ ],
+ hdrs = [
+ "autonomous_actor.h",
+ ],
+ deps = [
+ "//aos/actions:action_lib",
+ "//aos/events:event-loop",
+ "//aos/logging",
+ "//aos/util:phased_loop",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops/drivetrain:drivetrain_config",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//y2014/actors:shoot_action_lib",
+ "//y2014/control_loops/claw:claw_queue",
+ "//y2014/control_loops/drivetrain:drivetrain_base",
+ "//y2014/control_loops/shooter:shooter_queue",
+ "//y2014/queues:auto_mode",
+ "//y2014/queues:hot_goal",
+ "//y2014/queues:profile_params",
+ ],
)
cc_binary(
- name = 'drivetrain_action',
- srcs = [
- 'drivetrain_actor_main.cc',
- ],
- deps = [
- '//aos:init',
- ':drivetrain_action_lib',
- ':drivetrain_action_queue',
- ],
+ name = "autonomous_action",
+ srcs = [
+ "autonomous_actor_main.cc",
+ ],
+ deps = [
+ ":autonomous_action_lib",
+ "//aos:init",
+ "//aos/events:shm-event-loop",
+ "//frc971/autonomous:auto_queue",
+ ],
)
diff --git a/y2014/actors/autonomous_actor.cc b/y2014/actors/autonomous_actor.cc
new file mode 100644
index 0000000..b8f48af
--- /dev/null
+++ b/y2014/actors/autonomous_actor.cc
@@ -0,0 +1,407 @@
+#include "y2014/actors/autonomous_actor.h"
+
+#include <stdio.h>
+
+#include <chrono>
+#include <memory>
+
+#include "aos/actions/actions.h"
+#include "aos/logging/logging.h"
+#include "aos/logging/queue_logging.h"
+#include "aos/time/time.h"
+#include "aos/util/phased_loop.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014/actors/shoot_actor.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/drivetrain/drivetrain_base.h"
+#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/queues/auto_mode.q.h"
+#include "y2014/queues/hot_goal.q.h"
+
+namespace y2014 {
+namespace actors {
+
+namespace chrono = ::std::chrono;
+namespace this_thread = ::std::this_thread;
+using ::aos::monotonic_clock;
+using ::frc971::ProfileParameters;
+
+AutonomousActor::AutonomousActor(
+ ::aos::EventLoop *event_loop,
+ ::frc971::autonomous::AutonomousActionQueueGroup *s)
+ : frc971::autonomous::BaseAutonomousActor(
+ event_loop, s, control_loops::GetDrivetrainConfig()) {}
+
+void AutonomousActor::PositionClawVertically(double intake_power,
+ double centering_power) {
+ if (!control_loops::claw_queue.goal.MakeWithBuilder()
+ .bottom_angle(0.0)
+ .separation_angle(0.0)
+ .intake(intake_power)
+ .centering(centering_power)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
+
+void AutonomousActor::PositionClawBackIntake() {
+ if (!control_loops::claw_queue.goal.MakeWithBuilder()
+ .bottom_angle(-2.273474)
+ .separation_angle(0.0)
+ .intake(12.0)
+ .centering(12.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
+
+void AutonomousActor::PositionClawUpClosed() {
+ // Move the claw to where we're going to shoot from but keep it closed until
+ // it gets there.
+ if (!control_loops::claw_queue.goal.MakeWithBuilder()
+ .bottom_angle(0.86)
+ .separation_angle(0.0)
+ .intake(4.0)
+ .centering(1.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
+
+void AutonomousActor::PositionClawForShot() {
+ if (!control_loops::claw_queue.goal.MakeWithBuilder()
+ .bottom_angle(0.86)
+ .separation_angle(0.10)
+ .intake(4.0)
+ .centering(1.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
+
+void AutonomousActor::SetShotPower(double power) {
+ LOG(INFO, "Setting shot power to %f\n", power);
+ if (!control_loops::shooter_queue.goal.MakeWithBuilder()
+ .shot_power(power)
+ .shot_requested(false)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ }
+}
+
+const ProfileParameters kFastDrive = {3.0, 2.5};
+const ProfileParameters kSlowDrive = {2.5, 2.5};
+const ProfileParameters kFastWithBallDrive = {3.0, 2.0};
+const ProfileParameters kSlowWithBallDrive = {2.5, 2.0};
+const ProfileParameters kFastTurn = {3.0, 10.0};
+
+void AutonomousActor::Shoot() {
+ // Shoot.
+ auto shoot_action = actors::MakeShootAction();
+ shoot_action->Start();
+ WaitUntilDoneOrCanceled(::std::move(shoot_action));
+}
+
+bool AutonomousActor::WaitUntilClawDone() {
+ while (true) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(10),
+ ::std::chrono::milliseconds(10) / 2);
+ // Poll the running bit and auto done bits.
+ phased_loop.SleepUntilNext();
+ control_loops::claw_queue.status.FetchLatest();
+ control_loops::claw_queue.goal.FetchLatest();
+ if (ShouldCancel()) {
+ return false;
+ }
+ if (control_loops::claw_queue.status.get() == nullptr ||
+ control_loops::claw_queue.goal.get() == nullptr) {
+ continue;
+ }
+ bool ans =
+ control_loops::claw_queue.status->zeroed &&
+ (::std::abs(control_loops::claw_queue.status->bottom_velocity) < 1.0) &&
+ (::std::abs(control_loops::claw_queue.status->bottom -
+ control_loops::claw_queue.goal->bottom_angle) < 0.10) &&
+ (::std::abs(control_loops::claw_queue.status->separation -
+ control_loops::claw_queue.goal->separation_angle) < 0.4);
+ if (ans) {
+ return true;
+ }
+ }
+}
+
+class HotGoalDecoder {
+ public:
+ HotGoalDecoder() { ResetCounts(); }
+
+ void ResetCounts() {
+ hot_goal.FetchLatest();
+ if (hot_goal.get()) {
+ start_counts_ = *hot_goal;
+ LOG_STRUCT(INFO, "counts reset to", start_counts_);
+ start_counts_valid_ = true;
+ } else {
+ LOG(WARNING, "no hot goal message. ignoring\n");
+ start_counts_valid_ = false;
+ }
+ }
+
+ void Update(bool block = false) {
+ if (block) {
+ hot_goal.FetchAnother();
+ } else {
+ hot_goal.FetchLatest();
+ }
+ if (hot_goal.get()) LOG_STRUCT(INFO, "new counts", *hot_goal);
+ }
+
+ bool left_triggered() const {
+ if (!start_counts_valid_ || !hot_goal.get()) return false;
+ return (hot_goal->left_count - start_counts_.left_count) > kThreshold;
+ }
+
+ bool right_triggered() const {
+ if (!start_counts_valid_ || !hot_goal.get()) return false;
+ return (hot_goal->right_count - start_counts_.right_count) > kThreshold;
+ }
+
+ bool is_left() const {
+ if (!start_counts_valid_ || !hot_goal.get()) return false;
+ const uint64_t left_difference =
+ hot_goal->left_count - start_counts_.left_count;
+ const uint64_t right_difference =
+ hot_goal->right_count - start_counts_.right_count;
+ if (left_difference > kThreshold) {
+ if (right_difference > kThreshold) {
+ // We've seen a lot of both, so pick the one we've seen the most of.
+ return left_difference > right_difference;
+ } else {
+ // We've seen enough left but not enough right, so go with it.
+ return true;
+ }
+ } else {
+ // We haven't seen enough left, so it's not left.
+ return false;
+ }
+ }
+
+ bool is_right() const {
+ if (!start_counts_valid_ || !hot_goal.get()) return false;
+ const uint64_t left_difference =
+ hot_goal->left_count - start_counts_.left_count;
+ const uint64_t right_difference =
+ hot_goal->right_count - start_counts_.right_count;
+ if (right_difference > kThreshold) {
+ if (left_difference > kThreshold) {
+ // We've seen a lot of both, so pick the one we've seen the most of.
+ return right_difference > left_difference;
+ } else {
+ // We've seen enough right but not enough left, so go with it.
+ return true;
+ }
+ } else {
+ // We haven't seen enough right, so it's not right.
+ return false;
+ }
+ }
+
+ private:
+ static const uint64_t kThreshold = 5;
+
+ ::y2014::HotGoal start_counts_;
+ bool start_counts_valid_;
+};
+
+bool AutonomousActor::RunAction(
+ const ::frc971::autonomous::AutonomousActionParams & /*params*/) {
+ enum class AutoVersion : uint8_t {
+ kStraight,
+ kDoubleHot,
+ kSingleHot,
+ };
+
+ // The front of the robot is 1.854 meters from the wall
+ static const double kShootDistance = 3.15;
+ static const double kPickupDistance = 0.5;
+ static const double kTurnAngle = 0.3;
+
+ monotonic_clock::time_point start_time = monotonic_clock::now();
+ LOG(INFO, "Handling auto mode\n");
+
+ AutoVersion auto_version;
+ ::y2014::sensors::auto_mode.FetchLatest();
+ if (!::y2014::sensors::auto_mode.get()) {
+ LOG(WARNING, "not sure which auto mode to use\n");
+ auto_version = AutoVersion::kStraight;
+ } else {
+ static const double kSelectorMin = 0.2, kSelectorMax = 4.4;
+
+ const double kSelectorStep = (kSelectorMax - kSelectorMin) / 3.0;
+ if (::y2014::sensors::auto_mode->voltage < kSelectorStep + kSelectorMin) {
+ auto_version = AutoVersion::kSingleHot;
+ } else if (::y2014::sensors::auto_mode->voltage <
+ 2 * kSelectorStep + kSelectorMin) {
+ auto_version = AutoVersion::kStraight;
+ } else {
+ auto_version = AutoVersion::kDoubleHot;
+ }
+ }
+ LOG(INFO, "running auto %" PRIu8 "\n", static_cast<uint8_t>(auto_version));
+
+ const ProfileParameters &drive_params =
+ (auto_version == AutoVersion::kStraight) ? kFastDrive : kSlowDrive;
+ const ProfileParameters &drive_with_ball_params =
+ (auto_version == AutoVersion::kStraight) ? kFastWithBallDrive
+ : kSlowWithBallDrive;
+
+ HotGoalDecoder hot_goal_decoder;
+ // True for left, false for right.
+ bool first_shot_left, second_shot_left_default, second_shot_left;
+
+ Reset();
+
+ // Turn the claw on, keep it straight up until the ball has been grabbed.
+ LOG(INFO, "Claw going up at %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
+ PositionClawVertically(12.0, 4.0);
+ SetShotPower(115.0);
+
+ // Wait for the ball to enter the claw.
+ this_thread::sleep_for(chrono::milliseconds(250));
+ if (ShouldCancel()) return true;
+ LOG(INFO, "Readying claw for shot at %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
+
+ if (ShouldCancel()) return true;
+ // Drive to the goal.
+ StartDrive(-kShootDistance, 0.0, drive_params, kFastTurn);
+ this_thread::sleep_for(chrono::milliseconds(750));
+ PositionClawForShot();
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitForDriveProfileDone();
+ if (ShouldCancel()) return true;
+
+ hot_goal_decoder.Update();
+ if (hot_goal_decoder.is_left()) {
+ LOG(INFO, "first shot left\n");
+ first_shot_left = true;
+ second_shot_left_default = false;
+ } else if (hot_goal_decoder.is_right()) {
+ LOG(INFO, "first shot right\n");
+ first_shot_left = false;
+ second_shot_left_default = true;
+ } else {
+ LOG(INFO, "first shot defaulting left\n");
+ first_shot_left = true;
+ second_shot_left_default = true;
+ }
+ if (auto_version == AutoVersion::kDoubleHot) {
+ if (ShouldCancel()) return true;
+ StartDrive(0, first_shot_left ? kTurnAngle : -kTurnAngle,
+ drive_with_ball_params, kFastTurn);
+ WaitForDriveProfileDone();
+ if (ShouldCancel()) return true;
+ } else if (auto_version == AutoVersion::kSingleHot) {
+ do {
+ // TODO(brians): Wait for next message with timeout or something.
+ this_thread::sleep_for(chrono::milliseconds(3));
+ hot_goal_decoder.Update(false);
+ if (ShouldCancel()) return true;
+ } while (!hot_goal_decoder.left_triggered() &&
+ (monotonic_clock::now() - start_time) < chrono::seconds(9));
+ } else if (auto_version == AutoVersion::kStraight) {
+ this_thread::sleep_for(chrono::milliseconds(400));
+ }
+
+ // Shoot.
+ LOG(INFO, "Shooting at %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
+ Shoot();
+ this_thread::sleep_for(chrono::milliseconds(50));
+
+ if (auto_version == AutoVersion::kDoubleHot) {
+ if (ShouldCancel()) return true;
+ StartDrive(0, first_shot_left ? -kTurnAngle : kTurnAngle,
+ drive_with_ball_params, kFastTurn);
+ WaitForDriveProfileDone();
+ if (ShouldCancel()) return true;
+ } else if (auto_version == AutoVersion::kSingleHot) {
+ LOG(INFO, "auto done at %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
+ PositionClawVertically(0.0, 0.0);
+ return true;
+ }
+
+ {
+ if (ShouldCancel()) return true;
+ // Intake the new ball.
+ LOG(INFO, "Claw ready for intake at %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
+ PositionClawBackIntake();
+ StartDrive(kShootDistance + kPickupDistance, 0.0, drive_params, kFastTurn);
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitForDriveProfileDone();
+ if (ShouldCancel()) return true;
+ LOG(INFO, "Wait for the claw at %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
+ if (!WaitUntilClawDone()) return true;
+ }
+
+ // Drive back.
+ {
+ LOG(INFO, "Driving back at %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
+ StartDrive(-(kShootDistance + kPickupDistance), 0.0, drive_params,
+ kFastTurn);
+ this_thread::sleep_for(chrono::milliseconds(300));
+ hot_goal_decoder.ResetCounts();
+ if (ShouldCancel()) return true;
+ PositionClawUpClosed();
+ if (!WaitUntilClawDone()) return true;
+ PositionClawForShot();
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitForDriveProfileDone();
+ if (ShouldCancel()) return true;
+ if (!WaitUntilClawDone()) return true;
+ }
+
+ hot_goal_decoder.Update();
+ if (hot_goal_decoder.is_left()) {
+ LOG(INFO, "second shot left\n");
+ second_shot_left = true;
+ } else if (hot_goal_decoder.is_right()) {
+ LOG(INFO, "second shot right\n");
+ second_shot_left = false;
+ } else {
+ LOG(INFO, "second shot defaulting %s\n",
+ second_shot_left_default ? "left" : "right");
+ second_shot_left = second_shot_left_default;
+ }
+ if (auto_version == AutoVersion::kDoubleHot) {
+ if (ShouldCancel()) return true;
+ StartDrive(0, second_shot_left ? kTurnAngle : -kTurnAngle, drive_params,
+ kFastTurn);
+ WaitForDriveProfileDone();
+ if (ShouldCancel()) return true;
+ } else if (auto_version == AutoVersion::kStraight) {
+ this_thread::sleep_for(chrono::milliseconds(400));
+ }
+
+ LOG(INFO, "Shooting at %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
+ // Shoot
+ Shoot();
+ if (ShouldCancel()) return true;
+
+ // Get ready to zero when we come back up.
+ this_thread::sleep_for(chrono::milliseconds(50));
+ PositionClawVertically(0.0, 0.0);
+ return true;
+}
+
+} // namespace actors
+} // namespace y2014
diff --git a/y2014/actors/autonomous_actor.h b/y2014/actors/autonomous_actor.h
new file mode 100644
index 0000000..fe38a02
--- /dev/null
+++ b/y2014/actors/autonomous_actor.h
@@ -0,0 +1,43 @@
+#ifndef Y2014_ACTORS_AUTONOMOUS_ACTOR_H_
+#define Y2014_ACTORS_AUTONOMOUS_ACTOR_H_
+
+#include <chrono>
+#include <memory>
+
+#include "aos/actions/actions.h"
+#include "aos/actions/actor.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2014 {
+namespace actors {
+
+class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+ public:
+ explicit AutonomousActor(::aos::EventLoop *event_loop,
+ ::frc971::autonomous::AutonomousActionQueueGroup *s);
+
+ bool RunAction(
+ const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+
+ private:
+ void Reset() {
+ InitializeEncoders();
+ ResetDrivetrain();
+ }
+
+ void PositionClawVertically(double intake_power, double centering_power);
+ void PositionClawBackIntake();
+ void PositionClawUpClosed();
+ void PositionClawForShot();
+ void SetShotPower(double power);
+ void Shoot();
+
+ bool WaitUntilClawDone();
+};
+
+} // namespace actors
+} // namespace y2014
+
+#endif // Y2014_ACTORS_AUTONOMOUS_ACTOR_H_
diff --git a/y2014/actors/autonomous_actor_main.cc b/y2014/actors/autonomous_actor_main.cc
new file mode 100644
index 0000000..6f8f66c
--- /dev/null
+++ b/y2014/actors/autonomous_actor_main.cc
@@ -0,0 +1,18 @@
+#include <stdio.h>
+
+#include "aos/events/shm-event-loop.h"
+#include "aos/init.h"
+#include "frc971/autonomous/auto.q.h"
+#include "y2014/actors/autonomous_actor.h"
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init(-1);
+
+ ::aos::ShmEventLoop event_loop;
+ ::y2014::actors::AutonomousActor autonomous(
+ &event_loop, &::frc971::autonomous::autonomous_action);
+ autonomous.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2014/actors/drivetrain_action.q b/y2014/actors/drivetrain_action.q
deleted file mode 100644
index 458a3e5..0000000
--- a/y2014/actors/drivetrain_action.q
+++ /dev/null
@@ -1,29 +0,0 @@
-package y2014.actors;
-
-import "aos/actions/actions.q";
-
-// Parameters to send with start.
-struct DrivetrainActionParams {
- double left_initial_position;
- double right_initial_position;
- double y_offset;
- double theta_offset;
- double maximum_velocity;
- double maximum_acceleration;
- double maximum_turn_velocity;
- double maximum_turn_acceleration;
-};
-
-queue_group DrivetrainActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- DrivetrainActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
-
-queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
deleted file mode 100644
index 7275d0d..0000000
--- a/y2014/actors/drivetrain_actor.cc
+++ /dev/null
@@ -1,176 +0,0 @@
-#include "y2014/actors/drivetrain_actor.h"
-
-#include <functional>
-#include <numeric>
-
-#include <Eigen/Dense>
-
-#include "aos/util/phased_loop.h"
-#include "aos/logging/logging.h"
-#include "aos/util/trapezoid_profile.h"
-#include "aos/commonmath.h"
-#include "aos/time/time.h"
-
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/actors/drivetrain_actor.h"
-#include "y2014/constants.h"
-#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-
-namespace y2014 {
-namespace actors {
-
-namespace chrono = ::std::chrono;
-
-DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
- : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
- loop_(constants::GetValues().make_drivetrain_loop()) {
- loop_.set_index(3);
-}
-
-bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto &K = loop_.controller().K();
-
- const double yoffset = params.y_offset;
- const double turn_offset =
- params.theta_offset * control_loops::drivetrain::kRobotRadius;
- LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
-
- // Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(chrono::milliseconds(5));
- ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(5));
- const double goal_velocity = 0.0;
- const double epsilon = 0.01;
- ::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
-
- profile.set_maximum_acceleration(params.maximum_acceleration);
- profile.set_maximum_velocity(params.maximum_velocity);
- turn_profile.set_maximum_acceleration(
- params.maximum_turn_acceleration *
- control_loops::drivetrain::kRobotRadius);
- turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
- control_loops::drivetrain::kRobotRadius);
-
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
- ::std::chrono::milliseconds(5) / 2);
- while (true) {
- phased_loop.SleepUntilNext();
-
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
- if (::frc971::control_loops::drivetrain_queue.status.get()) {
- const auto &status = *::frc971::control_loops::drivetrain_queue.status;
- if (::std::abs(status.uncapped_left_voltage -
- status.uncapped_right_voltage) > 24) {
- LOG(DEBUG, "spinning in place\n");
- // They're more than 24V apart, so stop moving forwards and let it deal
- // with spinning first.
- profile.SetGoal(
- (status.estimated_left_position + status.estimated_right_position -
- params.left_initial_position - params.right_initial_position) /
- 2.0);
- } else {
- static const double divisor = K(0, 0) + K(0, 2);
- double dx_left, dx_right;
- if (status.uncapped_left_voltage > 12.0) {
- dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
- } else if (status.uncapped_left_voltage < -12.0) {
- dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
- } else {
- dx_left = 0;
- }
- if (status.uncapped_right_voltage > 12.0) {
- dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
- } else if (status.uncapped_right_voltage < -12.0) {
- dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
- } else {
- dx_right = 0;
- }
- double dx;
- if (dx_left == 0 && dx_right == 0) {
- dx = 0;
- } else if (dx_left != 0 && dx_right != 0 &&
- ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
- // Both saturating in opposite directions. Don't do anything.
- LOG(DEBUG, "Saturating opposite ways, not adjusting\n");
- dx = 0;
- } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
- dx = dx_left;
- } else {
- dx = dx_right;
- }
- if (dx != 0) {
- LOG(DEBUG, "adjusting goal by %f\n", dx);
- profile.MoveGoal(-dx);
- }
- }
- } else {
- // If we ever get here, that's bad and we should just give up
- LOG(ERROR, "no drivetrain status!\n");
- return false;
- }
-
- const auto drive_profile_goal_state =
- profile.Update(yoffset, goal_velocity);
- const auto turn_profile_goal_state = turn_profile.Update(turn_offset, 0.0);
- left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
- right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
-
- if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
- ::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
- break;
- }
-
- if (ShouldCancel()) return true;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- left_goal_state(0, 0) + params.left_initial_position,
- right_goal_state(0, 0) + params.right_initial_position);
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .controller_type(1)
- .highgear(true)
- .left_goal(left_goal_state(0, 0) + params.left_initial_position)
- .right_goal(right_goal_state(0, 0) + params.right_initial_position)
- .Send();
- }
- if (ShouldCancel()) return true;
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
- while (!::frc971::control_loops::drivetrain_queue.status.get()) {
- LOG(WARNING,
- "No previous drivetrain status packet, trying to fetch again\n");
- ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
- if (ShouldCancel()) return true;
- }
- while (true) {
- if (ShouldCancel()) return true;
- const double kPositionThreshold = 0.05;
-
- const double left_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->estimated_left_position -
- (left_goal_state(0, 0) + params.left_initial_position));
- const double right_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->estimated_right_position -
- (right_goal_state(0, 0) + params.right_initial_position));
- const double velocity_error = ::std::abs(
- ::frc971::control_loops::drivetrain_queue.status->robot_speed);
- if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
- velocity_error < 0.2) {
- break;
- } else {
- LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
- velocity_error);
- }
- ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
- }
- LOG(INFO, "Done moving\n");
- return true;
-}
-
-::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2014::actors::DrivetrainActionParams ¶ms) {
- return ::std::unique_ptr<DrivetrainAction>(
- new DrivetrainAction(&::y2014::actors::drivetrain_action, params));
-}
-
-} // namespace actors
-} // namespace y2014
diff --git a/y2014/actors/drivetrain_actor.h b/y2014/actors/drivetrain_actor.h
deleted file mode 100644
index 454a41e..0000000
--- a/y2014/actors/drivetrain_actor.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef Y2014_ACTIONS_DRIVETRAIN_ACTION_H_
-#define Y2014_ACTIONS_DRIVETRAIN_ACTION_H_
-
-#include <memory>
-
-#include "aos/actions/actor.h"
-#include "aos/actions/actions.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-
-#include "y2014/actors/drivetrain_action.q.h"
-
-namespace y2014 {
-namespace actors {
-
-class DrivetrainActor
- : public ::aos::common::actions::ActorBase<DrivetrainActionQueueGroup> {
- public:
- explicit DrivetrainActor(DrivetrainActionQueueGroup* s);
-
- bool RunAction(const actors::DrivetrainActionParams ¶ms) override;
-
- private:
- StateFeedbackLoop<4, 2, 2> loop_;
-};
-
-typedef ::aos::common::actions::TypedAction<DrivetrainActionQueueGroup>
- DrivetrainAction;
-
-// Makes a new DrivetrainActor action.
-::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2014::actors::DrivetrainActionParams& params);
-
-} // namespace actors
-} // namespace y2014
-
-#endif
diff --git a/y2014/actors/drivetrain_actor_main.cc b/y2014/actors/drivetrain_actor_main.cc
deleted file mode 100644
index 8d33390..0000000
--- a/y2014/actors/drivetrain_actor_main.cc
+++ /dev/null
@@ -1,16 +0,0 @@
-#include <stdio.h>
-
-#include "aos/init.h"
-#include "y2014/actors/drivetrain_action.q.h"
-#include "y2014/actors/drivetrain_actor.h"
-
-int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init(-1);
-
- ::y2014::actors::DrivetrainActor drivetrain(
- &::y2014::actors::drivetrain_action);
- drivetrain.Run();
-
- ::aos::Cleanup();
- return 0;
-}
diff --git a/y2014/autonomous/BUILD b/y2014/autonomous/BUILD
deleted file mode 100644
index d76e910..0000000
--- a/y2014/autonomous/BUILD
+++ /dev/null
@@ -1,42 +0,0 @@
-package(default_visibility = ['//visibility:public'])
-
-cc_library(
- name = 'auto_lib',
- srcs = [
- 'auto.cc',
- ],
- hdrs = [
- 'auto.h',
- ],
- deps = [
- '//frc971/autonomous:auto_queue',
- '//aos/controls:control_loop',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//y2014/control_loops/shooter:shooter_queue',
- '//y2014/control_loops/claw:claw_queue',
- '//y2014:constants',
- '//aos/time:time',
- '//aos/util:phased_loop',
- '//aos/util:trapezoid_profile',
- '//aos/logging',
- '//aos/actions:action_lib',
- '//y2014/actors:shoot_action_lib',
- '//y2014/actors:drivetrain_action_lib',
- '//y2014/queues:hot_goal',
- '//aos/logging:queue_logging',
- '//y2014/queues:profile_params',
- '//y2014/queues:auto_mode',
- ],
-)
-
-cc_binary(
- name = 'auto',
- srcs = [
- 'auto_main.cc',
- ],
- deps = [
- '//aos:init',
- '//frc971/autonomous:auto_queue',
- ':auto_lib',
- ],
-)
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
deleted file mode 100644
index 18e490e..0000000
--- a/y2014/autonomous/auto.cc
+++ /dev/null
@@ -1,526 +0,0 @@
-#include <stdio.h>
-
-#include <chrono>
-#include <memory>
-
-#include "aos/actions/actions.h"
-#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/time/time.h"
-#include "aos/util/phased_loop.h"
-#include "aos/util/trapezoid_profile.h"
-
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/actors/drivetrain_actor.h"
-#include "y2014/actors/shoot_actor.h"
-#include "y2014/constants.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/queues/auto_mode.q.h"
-
-#include "y2014/queues/hot_goal.q.h"
-#include "y2014/queues/profile_params.q.h"
-
-namespace y2014 {
-namespace autonomous {
-namespace chrono = ::std::chrono;
-namespace this_thread = ::std::this_thread;
-using ::aos::monotonic_clock;
-
-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");
- frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .controller_type(1)
- .highgear(true)
- .left_goal(left_initial_position)
- .right_goal(right_initial_position)
- .quickturn(false)
- .Send();
-}
-
-void ResetDrivetrain() {
- LOG(INFO, "resetting the drivetrain\n");
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .controller_type(0)
- .highgear(true)
- .wheel(0.0)
- .throttle(0.0)
- .left_goal(left_initial_position)
- .right_goal(right_initial_position)
- .Send();
-}
-
-void WaitUntilDoneOrCanceled(
- ::std::unique_ptr<aos::common::actions::Action> action) {
- if (!action) {
- LOG(ERROR, "No action, not waiting\n");
- return;
- }
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(10),
- ::std::chrono::milliseconds(10) / 2);
- while (true) {
- // Poll the running bit and auto done bits.
- phased_loop.SleepUntilNext();
- if (!action->Running() || ShouldExitAuto()) {
- return;
- }
- }
-}
-
-void StepDrive(double distance, double theta) {
- double left_goal = (left_initial_position + distance -
- theta * control_loops::drivetrain::kRobotRadius);
- double right_goal = (right_initial_position + distance +
- theta * control_loops::drivetrain::kRobotRadius);
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .controller_type(1)
- .highgear(true)
- .left_goal(left_goal)
- .right_goal(right_goal)
- .Send();
- left_initial_position = left_goal;
- right_initial_position = right_goal;
-}
-
-void PositionClawVertically(double intake_power = 0.0,
- double centering_power = 0.0) {
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.0)
- .separation_angle(0.0)
- .intake(intake_power)
- .centering(centering_power)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
-}
-
-void PositionClawBackIntake() {
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(-2.273474)
- .separation_angle(0.0)
- .intake(12.0)
- .centering(12.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
-}
-
-void PositionClawUpClosed() {
- // Move the claw to where we're going to shoot from but keep it closed until
- // it gets there.
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.86)
- .separation_angle(0.0)
- .intake(4.0)
- .centering(1.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
-}
-
-void PositionClawForShot() {
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.86)
- .separation_angle(0.10)
- .intake(4.0)
- .centering(1.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
-}
-
-void SetShotPower(double power) {
- LOG(INFO, "Setting shot power to %f\n", power);
- if (!control_loops::shooter_queue.goal.MakeWithBuilder()
- .shot_power(power)
- .shot_requested(false)
- .unload_requested(false)
- .load_requested(false)
- .Send()) {
- LOG(WARNING, "sending shooter goal failed\n");
- }
-}
-
-void WaitUntilNear(double distance) {
- while (true) {
- if (ShouldExitAuto()) return;
- ::frc971::control_loops::drivetrain_queue.status.FetchAnother();
- double left_error = ::std::abs(left_initial_position -
- ::frc971::control_loops::drivetrain_queue
- .status->estimated_left_position);
- double right_error = ::std::abs(right_initial_position -
- ::frc971::control_loops::drivetrain_queue
- .status->estimated_right_position);
- const double kPositionThreshold = 0.05 + distance;
- if (right_error < kPositionThreshold && left_error < kPositionThreshold) {
- LOG(INFO, "At the goal\n");
- return;
- }
- }
-}
-
-const ProfileParams kFastDrive = {3.0, 2.5};
-const ProfileParams kSlowDrive = {2.5, 2.5};
-const ProfileParams kFastWithBallDrive = {3.0, 2.0};
-const ProfileParams kSlowWithBallDrive = {2.5, 2.0};
-const ProfileParams kFastTurn = {3.0, 10.0};
-
-::std::unique_ptr<::y2014::actors::DrivetrainAction> SetDriveGoal(
- double distance, const ProfileParams drive_params, double theta = 0,
- const ProfileParams &turn_params = kFastTurn) {
- LOG(INFO, "Driving to %f\n", distance);
-
- ::y2014::actors::DrivetrainActionParams params;
- params.left_initial_position = left_initial_position;
- params.right_initial_position = right_initial_position;
- params.y_offset = distance;
- params.theta_offset = theta;
- params.maximum_turn_acceleration = turn_params.acceleration;
- params.maximum_turn_velocity = turn_params.velocity;
- params.maximum_velocity = drive_params.velocity;
- params.maximum_acceleration = drive_params.acceleration;
- auto drivetrain_action = actors::MakeDrivetrainAction(params);
- drivetrain_action->Start();
- left_initial_position +=
- distance - theta * control_loops::drivetrain::kRobotRadius;
- right_initial_position +=
- distance + theta * control_loops::drivetrain::kRobotRadius;
- return drivetrain_action;
-}
-
-void Shoot() {
- // Shoot.
- auto shoot_action = actors::MakeShootAction();
- shoot_action->Start();
- WaitUntilDoneOrCanceled(::std::move(shoot_action));
-}
-
-void InitializeEncoders() {
- ::frc971::control_loops::drivetrain_queue.status.FetchAnother();
- left_initial_position =
- ::frc971::control_loops::drivetrain_queue.status->estimated_left_position;
- right_initial_position = ::frc971::control_loops::drivetrain_queue.status
- ->estimated_right_position;
-}
-
-void WaitUntilClawDone() {
- while (true) {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(10),
- ::std::chrono::milliseconds(10) / 2);
- // Poll the running bit and auto done bits.
- phased_loop.SleepUntilNext();
- control_loops::claw_queue.status.FetchLatest();
- control_loops::claw_queue.goal.FetchLatest();
- if (ShouldExitAuto()) {
- return;
- }
- if (control_loops::claw_queue.status.get() == nullptr ||
- control_loops::claw_queue.goal.get() == nullptr) {
- continue;
- }
- bool ans =
- control_loops::claw_queue.status->zeroed &&
- (::std::abs(control_loops::claw_queue.status->bottom_velocity) < 1.0) &&
- (::std::abs(control_loops::claw_queue.status->bottom -
- control_loops::claw_queue.goal->bottom_angle) < 0.10) &&
- (::std::abs(control_loops::claw_queue.status->separation -
- control_loops::claw_queue.goal->separation_angle) < 0.4);
- if (ans) {
- return;
- }
- if (ShouldExitAuto()) return;
- }
-}
-
-class HotGoalDecoder {
- public:
- HotGoalDecoder() { ResetCounts(); }
-
- void ResetCounts() {
- hot_goal.FetchLatest();
- if (hot_goal.get()) {
- start_counts_ = *hot_goal;
- LOG_STRUCT(INFO, "counts reset to", start_counts_);
- start_counts_valid_ = true;
- } else {
- LOG(WARNING, "no hot goal message. ignoring\n");
- start_counts_valid_ = false;
- }
- }
-
- void Update(bool block = false) {
- if (block) {
- hot_goal.FetchAnother();
- } else {
- hot_goal.FetchLatest();
- }
- if (hot_goal.get()) LOG_STRUCT(INFO, "new counts", *hot_goal);
- }
-
- bool left_triggered() const {
- if (!start_counts_valid_ || !hot_goal.get()) return false;
- return (hot_goal->left_count - start_counts_.left_count) > kThreshold;
- }
-
- bool right_triggered() const {
- if (!start_counts_valid_ || !hot_goal.get()) return false;
- return (hot_goal->right_count - start_counts_.right_count) > kThreshold;
- }
-
- bool is_left() const {
- if (!start_counts_valid_ || !hot_goal.get()) return false;
- const uint64_t left_difference =
- hot_goal->left_count - start_counts_.left_count;
- const uint64_t right_difference =
- hot_goal->right_count - start_counts_.right_count;
- if (left_difference > kThreshold) {
- if (right_difference > kThreshold) {
- // We've seen a lot of both, so pick the one we've seen the most of.
- return left_difference > right_difference;
- } else {
- // We've seen enough left but not enough right, so go with it.
- return true;
- }
- } else {
- // We haven't seen enough left, so it's not left.
- return false;
- }
- }
-
- bool is_right() const {
- if (!start_counts_valid_ || !hot_goal.get()) return false;
- const uint64_t left_difference =
- hot_goal->left_count - start_counts_.left_count;
- const uint64_t right_difference =
- hot_goal->right_count - start_counts_.right_count;
- if (right_difference > kThreshold) {
- if (left_difference > kThreshold) {
- // We've seen a lot of both, so pick the one we've seen the most of.
- return right_difference > left_difference;
- } else {
- // We've seen enough right but not enough left, so go with it.
- return true;
- }
- } else {
- // We haven't seen enough right, so it's not right.
- return false;
- }
- }
-
- private:
- static const uint64_t kThreshold = 5;
-
- ::y2014::HotGoal start_counts_;
- bool start_counts_valid_;
-};
-
-void HandleAuto() {
- enum class AutoVersion : uint8_t {
- kStraight,
- kDoubleHot,
- kSingleHot,
- };
-
- // The front of the robot is 1.854 meters from the wall
- static const double kShootDistance = 3.15;
- static const double kPickupDistance = 0.5;
- static const double kTurnAngle = 0.3;
-
- monotonic_clock::time_point start_time = monotonic_clock::now();
- LOG(INFO, "Handling auto mode\n");
-
- AutoVersion auto_version;
- ::y2014::sensors::auto_mode.FetchLatest();
- if (!::y2014::sensors::auto_mode.get()) {
- LOG(WARNING, "not sure which auto mode to use\n");
- auto_version = AutoVersion::kStraight;
- } else {
- static const double kSelectorMin = 0.2, kSelectorMax = 4.4;
-
- const double kSelectorStep = (kSelectorMax - kSelectorMin) / 3.0;
- if (::y2014::sensors::auto_mode->voltage < kSelectorStep + kSelectorMin) {
- auto_version = AutoVersion::kSingleHot;
- } else if (::y2014::sensors::auto_mode->voltage <
- 2 * kSelectorStep + kSelectorMin) {
- auto_version = AutoVersion::kStraight;
- } else {
- auto_version = AutoVersion::kDoubleHot;
- }
- }
- LOG(INFO, "running auto %" PRIu8 "\n", static_cast<uint8_t>(auto_version));
-
- const ProfileParams &drive_params =
- (auto_version == AutoVersion::kStraight) ? kFastDrive : kSlowDrive;
- const ProfileParams &drive_with_ball_params =
- (auto_version == AutoVersion::kStraight) ? kFastWithBallDrive
- : kSlowWithBallDrive;
-
- HotGoalDecoder hot_goal_decoder;
- // True for left, false for right.
- bool first_shot_left, second_shot_left_default, second_shot_left;
-
- ResetDrivetrain();
-
- this_thread::sleep_for(chrono::milliseconds(100));
- if (ShouldExitAuto()) return;
- InitializeEncoders();
-
- // Turn the claw on, keep it straight up until the ball has been grabbed.
- LOG(INFO, "Claw going up at %f\n",
- ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
- PositionClawVertically(12.0, 4.0);
- SetShotPower(115.0);
-
- // Wait for the ball to enter the claw.
- this_thread::sleep_for(chrono::milliseconds(250));
- if (ShouldExitAuto()) return;
- LOG(INFO, "Readying claw for shot at %f\n",
- ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
-
- {
- if (ShouldExitAuto()) return;
- // Drive to the goal.
- auto drivetrain_action = SetDriveGoal(-kShootDistance, drive_params);
- this_thread::sleep_for(chrono::milliseconds(750));
- PositionClawForShot();
- LOG(INFO, "Waiting until drivetrain is finished\n");
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- }
-
- hot_goal_decoder.Update();
- if (hot_goal_decoder.is_left()) {
- LOG(INFO, "first shot left\n");
- first_shot_left = true;
- second_shot_left_default = false;
- } else if (hot_goal_decoder.is_right()) {
- LOG(INFO, "first shot right\n");
- first_shot_left = false;
- second_shot_left_default = true;
- } else {
- LOG(INFO, "first shot defaulting left\n");
- first_shot_left = true;
- second_shot_left_default = true;
- }
- if (auto_version == AutoVersion::kDoubleHot) {
- if (ShouldExitAuto()) return;
- auto drivetrain_action = SetDriveGoal(
- 0, drive_with_ball_params, first_shot_left ? kTurnAngle : -kTurnAngle);
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- } else if (auto_version == AutoVersion::kSingleHot) {
- do {
- // TODO(brians): Wait for next message with timeout or something.
- this_thread::sleep_for(chrono::milliseconds(3));
- hot_goal_decoder.Update(false);
- if (ShouldExitAuto()) return;
- } while (!hot_goal_decoder.left_triggered() &&
- (monotonic_clock::now() - start_time) < chrono::seconds(9));
- } else if (auto_version == AutoVersion::kStraight) {
- this_thread::sleep_for(chrono::milliseconds(400));
- }
-
- // Shoot.
- LOG(INFO, "Shooting at %f\n",
- ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
- Shoot();
- this_thread::sleep_for(chrono::milliseconds(50));
-
- if (auto_version == AutoVersion::kDoubleHot) {
- if (ShouldExitAuto()) return;
- auto drivetrain_action = SetDriveGoal(
- 0, drive_with_ball_params, first_shot_left ? -kTurnAngle : kTurnAngle);
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- } else if (auto_version == AutoVersion::kSingleHot) {
- LOG(INFO, "auto done at %f\n",
- ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
- PositionClawVertically(0.0, 0.0);
- return;
- }
-
- {
- if (ShouldExitAuto()) return;
- // Intake the new ball.
- LOG(INFO, "Claw ready for intake at %f\n",
- ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
- PositionClawBackIntake();
- auto drivetrain_action =
- SetDriveGoal(kShootDistance + kPickupDistance, drive_params);
- LOG(INFO, "Waiting until drivetrain is finished\n");
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- LOG(INFO, "Wait for the claw at %f\n",
- ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
- WaitUntilClawDone();
- if (ShouldExitAuto()) return;
- }
-
- // Drive back.
- {
- LOG(INFO, "Driving back at %f\n",
- ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
- auto drivetrain_action =
- SetDriveGoal(-(kShootDistance + kPickupDistance), drive_params);
- this_thread::sleep_for(chrono::milliseconds(300));
- hot_goal_decoder.ResetCounts();
- if (ShouldExitAuto()) return;
- PositionClawUpClosed();
- WaitUntilClawDone();
- if (ShouldExitAuto()) return;
- PositionClawForShot();
- LOG(INFO, "Waiting until drivetrain is finished\n");
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- WaitUntilClawDone();
- if (ShouldExitAuto()) return;
- }
-
- hot_goal_decoder.Update();
- if (hot_goal_decoder.is_left()) {
- LOG(INFO, "second shot left\n");
- second_shot_left = true;
- } else if (hot_goal_decoder.is_right()) {
- LOG(INFO, "second shot right\n");
- second_shot_left = false;
- } else {
- LOG(INFO, "second shot defaulting %s\n",
- second_shot_left_default ? "left" : "right");
- second_shot_left = second_shot_left_default;
- }
- if (auto_version == AutoVersion::kDoubleHot) {
- if (ShouldExitAuto()) return;
- auto drivetrain_action = SetDriveGoal(
- 0, drive_params, second_shot_left ? kTurnAngle : -kTurnAngle);
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- } else if (auto_version == AutoVersion::kStraight) {
- this_thread::sleep_for(chrono::milliseconds(400));
- }
-
- LOG(INFO, "Shooting at %f\n",
- ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
- // Shoot
- Shoot();
- if (ShouldExitAuto()) return;
-
- // Get ready to zero when we come back up.
- this_thread::sleep_for(chrono::milliseconds(50));
- PositionClawVertically(0.0, 0.0);
-}
-
-} // namespace autonomous
-} // namespace y2014
diff --git a/y2014/autonomous/auto.h b/y2014/autonomous/auto.h
deleted file mode 100644
index dd00bba..0000000
--- a/y2014/autonomous/auto.h
+++ /dev/null
@@ -1,12 +0,0 @@
-#ifndef Y2014_AUTONOMOUS_AUTO_H_
-#define Y2014_AUTONOMOUS_AUTO_H_
-
-namespace y2014 {
-namespace autonomous {
-
-void HandleAuto();
-
-} // namespace autonomous
-} // namespace y2014
-
-#endif // Y2014_AUTONOMOUS_AUTO_H_
diff --git a/y2014/autonomous/auto_main.cc b/y2014/autonomous/auto_main.cc
deleted file mode 100644
index f125cc2..0000000
--- a/y2014/autonomous/auto_main.cc
+++ /dev/null
@@ -1,40 +0,0 @@
-#include <stdio.h>
-
-#include "aos/init.h"
-#include "aos/logging/logging.h"
-#include "aos/time/time.h"
-#include "frc971/autonomous/auto.q.h"
-#include "y2014/autonomous/auto.h"
-
-int main(int /*argc*/, char * /*argv*/ []) {
- ::aos::Init(-1);
-
- 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::monotonic_clock::time_point start_time =
- ::aos::monotonic_clock::now();
- ::y2014::autonomous::HandleAuto();
-
- auto elapsed_time = ::aos::monotonic_clock::now() - start_time;
- LOG(INFO, "Auto mode exited in %f, waiting for it to finish.\n",
- ::aos::time::DurationInSeconds(elapsed_time));
- 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/joystick_reader.cc b/y2014/joystick_reader.cc
index 9c32aa4..99c11b4 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -3,21 +3,22 @@
#include <unistd.h>
#include <math.h>
+#include "aos/actions/actions.h"
#include "aos/init.h"
-#include "aos/input/joystick_input.h"
+#include "aos/input/action_joystick_input.h"
#include "aos/input/driver_station_data.h"
#include "aos/logging/logging.h"
-#include "aos/util/log_interval.h"
#include "aos/time/time.h"
-#include "aos/actions/actions.h"
+#include "aos/util/log_interval.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/constants.h"
-#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/queues/gyro.q.h"
#include "y2014/actors/shoot_actor.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/drivetrain/drivetrain_base.h"
+#include "y2014/control_loops/shooter/shooter.q.h"
using ::frc971::control_loops::drivetrain_queue;
using ::frc971::sensors::gyro_reading;
@@ -153,96 +154,17 @@
const ClawGoal k254PassGoal = {-1.95, kGrabSeparation};
const ClawGoal kFlipped254PassGoal = {1.96, kGrabSeparation};
-class Reader : public ::aos::input::JoystickInput {
+class Reader : public ::aos::input::ActionJoystickInput {
public:
Reader(::aos::EventLoop *event_loop)
- : ::aos::input::JoystickInput(event_loop),
- is_high_gear_(false),
+ : ::aos::input::ActionJoystickInput(
+ event_loop, control_loops::GetDrivetrainConfig(),
+ ::aos::input::DrivetrainInputReader::InputType::kSteeringWheel, {}),
shot_power_(80.0),
goal_angle_(0.0),
separation_angle_(kGrabSeparation),
velocity_compensation_(0.0),
- intake_power_(0.0),
- was_running_(false) {}
-
- void RunIteration(const ::aos::input::driver_station::Data &data) override {
- bool last_auto_running = auto_running_;
- auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
- data.GetControlBit(ControlBit::kEnabled);
- if (auto_running_ != last_auto_running) {
- if (auto_running_) {
- StartAuto();
- } else {
- StopAuto();
- }
- }
-
- if (!data.GetControlBit(ControlBit::kAutonomous)) {
- HandleDrivetrain(data);
- HandleTeleop(data);
- }
- }
-
- void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
- bool is_control_loop_driving = false;
- double left_goal = 0.0;
- double right_goal = 0.0;
- const double wheel = -data.GetAxis(kSteeringWheel);
- const double throttle = -data.GetAxis(kDriveThrottle);
- const double kThrottleGain = 1.0 / 2.5;
- if (false && (data.IsPressed(kDriveControlLoopEnable1) ||
- data.IsPressed(kDriveControlLoopEnable2))) {
- if (data.PosEdge(kDriveControlLoopEnable1) ||
- data.PosEdge(kDriveControlLoopEnable2)) {
- if (drivetrain_queue.position.FetchLatest() &&
- gyro_reading.FetchLatest()) {
- distance_ = (drivetrain_queue.position->left_encoder +
- drivetrain_queue.position->right_encoder) /
- 2.0 -
- throttle * kThrottleGain / 2.0;
- angle_ = gyro_reading->angle;
- filtered_goal_distance_ = distance_;
- }
- }
- is_control_loop_driving = true;
-
- // const double gyro_angle = Gyro.View().angle;
- const double goal_theta = angle_ - wheel * 0.27;
- const double goal_distance = distance_ + throttle * kThrottleGain;
- const double robot_width = 22.0 / 100.0 * 2.54;
- const double kMaxVelocity = 0.6;
- if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance_) {
- filtered_goal_distance_ += kMaxVelocity * 0.03;
- } else if (goal_distance <
- -kMaxVelocity * 0.02 + filtered_goal_distance_) {
- filtered_goal_distance_ -= kMaxVelocity * 0.02;
- } else {
- filtered_goal_distance_ = goal_distance;
- }
- left_goal = filtered_goal_distance_ - robot_width * goal_theta / 2.0;
- right_goal = filtered_goal_distance_ + robot_width * goal_theta / 2.0;
- is_high_gear_ = false;
-
- LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
- }
- if (!drivetrain_queue.goal.MakeWithBuilder()
- .wheel(wheel)
- .throttle(throttle)
- .highgear(is_high_gear_)
- .quickturn(data.IsPressed(kQuickTurn))
- .controller_type(is_control_loop_driving ? 1 : 0)
- .left_goal(left_goal)
- .right_goal(right_goal)
- .Send()) {
- LOG(WARNING, "sending stick values failed\n");
- }
- if (data.PosEdge(kShiftLow)) {
- is_high_gear_ = false;
- }
- if (data.PosEdge(kShiftHigh)) {
- is_high_gear_ = true;
- }
- }
+ intake_power_(0.0) {}
void SetGoal(ClawGoal goal) {
goal_angle_ = goal.angle;
@@ -262,11 +184,7 @@
intake_power_ = goal.intake_power;
}
- void HandleTeleop(const ::aos::input::driver_station::Data &data) {
- if (!data.GetControlBit(ControlBit::kEnabled)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- }
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) override {
if (data.IsPressed(kRollersIn) || data.IsPressed(kRollersOut)) {
intake_power_ = 0.0;
separation_angle_ = kGrabSeparation;
@@ -304,120 +222,119 @@
claw_goal_adjust *= -1;
if (data.IsPressed(kIntakeOpenPosition)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFlippedIntakeOpenGoal);
} else if (data.IsPressed(kIntakePosition)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFlippedIntakeGoal);
} else if (data.IsPressed(kVerticalTuck)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kVerticalTuckGoal);
} else if (data.IsPressed(kTuck)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFlippedTuckGoal);
} else if (data.PosEdge(kLongShot)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFlippedLongShotGoal);
} else if (data.PosEdge(kCloseShot)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFlippedMediumShotGoal);
} else if (data.PosEdge(kFenderShot)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFlippedShortShotGoal);
} else if (data.PosEdge(kHumanPlayerShot)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFlippedHumanShotGoal);
} else if (data.PosEdge(kUserLeft)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFlipped254PassGoal);
} else if (data.PosEdge(kUserRight)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFlippedDemoShotGoal);
} else if (data.PosEdge(kTrussShot)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFlippedTrussShotGoal);
}
} else {
if (data.IsPressed(kIntakeOpenPosition)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kIntakeOpenGoal);
} else if (data.IsPressed(kIntakePosition)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kIntakeGoal);
} else if (data.IsPressed(kVerticalTuck)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kVerticalTuckGoal);
} else if (data.IsPressed(kTuck)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kTuckGoal);
} else if (data.PosEdge(kLongShot)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kLongShotGoal);
} else if (data.PosEdge(kCloseShot)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kCloseShotGoal);
} else if (data.PosEdge(kFenderShot)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kFenderShotGoal);
} else if (data.PosEdge(kHumanPlayerShot)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kHumanShotGoal);
} else if (data.PosEdge(kUserLeft)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(k254PassGoal);
} else if (data.PosEdge(kUserRight)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kDemoShotGoal);
} else if (data.PosEdge(kTrussShot)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kTrussShotGoal);
}
}
if (data.PosEdge(kFire)) {
- action_queue_.EnqueueAction(actors::MakeShootAction());
+ EnqueueAction(actors::MakeShootAction());
} else if (data.NegEdge(kFire)) {
- action_queue_.CancelCurrentAction();
+ CancelCurrentAction();
}
- action_queue_.Tick();
if (data.IsPressed(kUnload) || data.IsPressed(kReload)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
intake_power_ = 0.0;
velocity_compensation_ = 0.0;
}
// Send out the claw and shooter goals if no actions are running.
- if (!action_queue_.Running()) {
+ if (!ActionRunning()) {
goal_angle_ += claw_goal_adjust;
separation_angle_ += claw_separation_adjust;
// If the action just ended, turn the intake off and stop velocity
// compensating.
- if (was_running_) {
+ if (was_running_action()) {
intake_power_ = 0.0;
velocity_compensation_ = 0.0;
}
@@ -473,7 +390,6 @@
LOG(WARNING, "sending shooter goal failed\n");
}
}
- was_running_ = action_queue_.Running();
}
double SpeedToAngleOffset(double speed) {
@@ -494,23 +410,13 @@
::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
}
- bool is_high_gear_;
double shot_power_;
double goal_angle_;
double separation_angle_, shot_separation_angle_;
double velocity_compensation_;
- // Distance, angle, and filtered goal for closed loop driving.
- double distance_;
- double angle_;
- double filtered_goal_distance_;
double intake_power_;
- bool was_running_;
bool moving_for_shot_ = false;
- bool auto_running_ = false;
-
- ::aos::common::actions::ActionQueue action_queue_;
-
::aos::util::SimpleLogInterval no_drivetrain_status_ =
::aos::util::SimpleLogInterval(::std::chrono::milliseconds(200), WARNING,
"no drivetrain status");