Convert 2014_bot3 to autonomous framework
Makes it easier to experiment with it.
Change-Id: I9478a1a60ee61585a4e1b955e329686aefa15aa4
diff --git a/y2014_bot3/BUILD b/y2014_bot3/BUILD
index ea79dac..f5c5979 100644
--- a/y2014_bot3/BUILD
+++ b/y2014_bot3/BUILD
@@ -6,15 +6,18 @@
"joystick_reader.cc",
],
deps = [
- "//aos/time:time",
- "//aos/actions:action_lib",
- "//aos/logging",
- "//aos/util:log_interval",
- "//aos/input:joystick_input",
"//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/input:drivetrain_input",
+ "//aos/input:joystick_input",
+ "//aos/logging",
+ "//aos/time",
+ "//aos/util:log_interval",
+ "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops/drivetrain:drivetrain_queue",
"//frc971/queues:gyro",
- "//y2014_bot3/autonomous:auto_queue",
+ "//y2014_bot3/control_loops/drivetrain:drivetrain_base",
"//y2014_bot3/control_loops/rollers:rollers_queue",
],
)
@@ -23,7 +26,7 @@
start_binaries = [
":joystick_reader",
":wpilib_interface",
- "//y2014_bot3/autonomous:auto",
+ "//y2014_bot3/actors:autonomous_action",
"//y2014_bot3/control_loops/drivetrain:drivetrain",
"//y2014_bot3/control_loops/rollers:rollers",
],
@@ -36,16 +39,16 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- "//aos/stl_mutex:stl_mutex",
- "//aos/time:time",
+ "//aos:init",
"//aos/controls:control_loop",
"//aos/logging",
"//aos/logging:queue_logging",
- "//aos/robot_state:robot_state",
+ "//aos/robot_state",
+ "//aos/stl_mutex",
+ "//aos/time",
"//aos/util:log_interval",
"//aos/util:phased_loop",
"//aos/util:wrapping_counter",
- "//aos:init",
"//frc971/control_loops:queues",
"//frc971/control_loops/drivetrain:drivetrain_queue",
"//frc971/wpilib:buffered_pcm",
@@ -58,7 +61,6 @@
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
- "//y2014_bot3/autonomous:auto_queue",
"//y2014_bot3/control_loops/drivetrain:drivetrain_base",
"//y2014_bot3/control_loops/rollers:rollers_lib",
],
diff --git a/y2014_bot3/actors/BUILD b/y2014_bot3/actors/BUILD
new file mode 100644
index 0000000..57f6d08
--- /dev/null
+++ b/y2014_bot3/actors/BUILD
@@ -0,0 +1,31 @@
+cc_library(
+ name = "autonomous_action_lib",
+ srcs = [
+ "autonomous_actor.cc",
+ ],
+ hdrs = [
+ "autonomous_actor.h",
+ ],
+ deps = [
+ "//aos/actions:action_lib",
+ "//aos/logging",
+ "//aos/util:phased_loop",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops/drivetrain:drivetrain_config",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//y2014_bot3/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+cc_binary(
+ name = "autonomous_action",
+ srcs = [
+ "autonomous_actor_main.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":autonomous_action_lib",
+ "//aos:init",
+ "//frc971/autonomous:auto_queue",
+ ],
+)
diff --git a/y2014_bot3/actors/autonomous_actor.cc b/y2014_bot3/actors/autonomous_actor.cc
new file mode 100644
index 0000000..681b7a0
--- /dev/null
+++ b/y2014_bot3/actors/autonomous_actor.cc
@@ -0,0 +1,58 @@
+#include "y2014_bot3/actors/autonomous_actor.h"
+
+#include <inttypes.h>
+#include <chrono>
+#include <cmath>
+
+#include "aos/logging/logging.h"
+#include "aos/util/phased_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2014_bot3 {
+namespace actors {
+using ::frc971::ProfileParameters;
+
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+
+namespace {
+
+double DoubleSeconds(monotonic_clock::duration duration) {
+ return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
+ .count();
+}
+
+const ProfileParameters kDrive = {5.0, 2.5};
+const ProfileParameters kTurn = {8.0, 3.0};
+
+} // namespace
+
+AutonomousActor::AutonomousActor(
+ ::frc971::autonomous::AutonomousActionQueueGroup *s)
+ : frc971::autonomous::BaseAutonomousActor(
+ s, control_loops::drivetrain::GetDrivetrainConfig()) {}
+
+bool AutonomousActor::RunAction(
+ const ::frc971::autonomous::AutonomousActionParams ¶ms) {
+ monotonic_clock::time_point start_time = monotonic_clock::now();
+ LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
+ Reset();
+
+ StartDrive(1.0, 0.0, kDrive, kTurn);
+ if (!WaitForDriveDone()) return true;
+
+ LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (!ShouldCancel()) {
+ phased_loop.SleepUntilNext();
+ }
+ LOG(DEBUG, "Done running\n");
+
+ return true;
+}
+
+} // namespace actors
+} // namespace y2014_bot3
diff --git a/y2014_bot3/actors/autonomous_actor.h b/y2014_bot3/actors/autonomous_actor.h
new file mode 100644
index 0000000..94bbbd7
--- /dev/null
+++ b/y2014_bot3/actors/autonomous_actor.h
@@ -0,0 +1,31 @@
+#ifndef Y2014_BOT3_ACTORS_AUTONOMOUS_ACTOR_H_
+#define Y2014_BOT3_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"
+
+namespace y2014_bot3 {
+namespace actors {
+
+class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+ public:
+ explicit AutonomousActor(::frc971::autonomous::AutonomousActionQueueGroup *s);
+
+ bool RunAction(
+ const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+
+ private:
+ void Reset() {
+ InitializeEncoders();
+ ResetDrivetrain();
+ }
+};
+
+} // namespace actors
+} // namespace y2014_bot3
+
+#endif // Y2014_BOT3_ACTORS_AUTONOMOUS_ACTOR_H_
diff --git a/y2014_bot3/actors/autonomous_actor_main.cc b/y2014_bot3/actors/autonomous_actor_main.cc
new file mode 100644
index 0000000..157d9ed
--- /dev/null
+++ b/y2014_bot3/actors/autonomous_actor_main.cc
@@ -0,0 +1,16 @@
+#include <stdio.h>
+
+#include "aos/init.h"
+#include "frc971/autonomous/auto.q.h"
+#include "y2014_bot3/actors/autonomous_actor.h"
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init(-1);
+
+ ::y2014_bot3::actors::AutonomousActor autonomous(
+ &::frc971::autonomous::autonomous_action);
+ autonomous.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2014_bot3/autonomous/BUILD b/y2014_bot3/autonomous/BUILD
deleted file mode 100644
index c4db824..0000000
--- a/y2014_bot3/autonomous/BUILD
+++ /dev/null
@@ -1,43 +0,0 @@
-package(default_visibility = ['//visibility:public'])
-
-load('//aos/build:queues.bzl', 'queue_library')
-
-queue_library(
- name = 'auto_queue',
- srcs = [
- 'auto.q',
- ],
-)
-
-cc_library(
- name = 'auto_lib',
- srcs = [
- 'auto.cc',
- ],
- hdrs = [
- 'auto.h',
- ],
- deps = [
- ':auto_queue',
- '//aos/controls:control_loop',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//y2014_bot3/control_loops/rollers:rollers_queue',
- '//aos/time:time',
- '//aos/util:phased_loop',
- '//aos/util:trapezoid_profile',
- '//aos/logging',
- '//aos/logging:queue_logging',
- ],
-)
-
-cc_binary(
- name = 'auto',
- srcs = [
- 'auto_main.cc',
- ],
- deps = [
- '//aos:init',
- ':auto_queue',
- ':auto_lib',
- ],
-)
diff --git a/y2014_bot3/autonomous/auto.cc b/y2014_bot3/autonomous/auto.cc
deleted file mode 100644
index 6cdaafd..0000000
--- a/y2014_bot3/autonomous/auto.cc
+++ /dev/null
@@ -1,94 +0,0 @@
-#include <stdio.h>
-
-#include <chrono>
-#include <memory>
-
-#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/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014_bot3/autonomous/auto.q.h"
-#include "y2014_bot3/control_loops/rollers/rollers.q.h"
-
-using ::frc971::control_loops::drivetrain_queue;
-using ::y2014_bot3::control_loops::rollers_queue;
-
-namespace y2014_bot3 {
-namespace autonomous {
-
-namespace chrono = ::std::chrono;
-using ::aos::monotonic_clock;
-
-static double left_initial_position, right_initial_position;
-
-bool ShouldExitAuto() {
- ::y2014_bot3::autonomous::autonomous.FetchLatest();
- bool ans = !::y2014_bot3::autonomous::autonomous->run_auto;
- if (ans) {
- LOG(INFO, "Time to exit auto mode\n");
- }
- return ans;
-}
-
-void ResetDrivetrain() {
- LOG(INFO, "resetting the drivetrain\n");
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(false)
- .wheel(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() {
- ::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 HandleAuto() {
- monotonic_clock::time_point start_time = monotonic_clock::now();
- LOG(INFO, "Starting auto mode at %f\n",
- chrono::duration_cast<chrono::duration<double>>(
- start_time.time_since_epoch()).count());
-
- // TODO(comran): Add various options for different autos down below.
- ResetDrivetrain();
- InitializeEncoders();
-
- LOG(INFO, "Driving\n");
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(false)
- .highgear(false)
- .quickturn(false)
- .wheel(0.0)
- .throttle(0.5)
- .left_goal(left_initial_position)
- .left_velocity_goal(0)
- .right_goal(right_initial_position)
- .right_velocity_goal(0)
- .Send();
- ::std::this_thread::sleep_for(chrono::seconds(2));
-
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(false)
- .highgear(false)
- .quickturn(false)
- .wheel(0.0)
- .throttle(0.0)
- .left_goal(left_initial_position)
- .left_velocity_goal(0)
- .right_goal(right_initial_position)
- .right_velocity_goal(0)
- .Send();
-}
-
-} // namespace autonomous
-} // namespace y2014_bot3
diff --git a/y2014_bot3/autonomous/auto.h b/y2014_bot3/autonomous/auto.h
deleted file mode 100644
index 387d2a4..0000000
--- a/y2014_bot3/autonomous/auto.h
+++ /dev/null
@@ -1,12 +0,0 @@
-#ifndef Y2014_BOT3_AUTONOMOUS_AUTO_H_
-#define Y2014_BOT3_AUTONOMOUS_AUTO_H_
-
-namespace y2014_bot3 {
-namespace autonomous {
-
-void HandleAuto();
-
-} // namespace autonomous
-} // namespace y2014_bot3
-
-#endif // Y2014_BOT3_AUTONOMOUS_AUTO_H_
diff --git a/y2014_bot3/autonomous/auto.q b/y2014_bot3/autonomous/auto.q
deleted file mode 100644
index 68fcd5f..0000000
--- a/y2014_bot3/autonomous/auto.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package y2014_bot3.autonomous;
-
-message AutoControl {
- // True if auto mode should be running, false otherwise.
- bool run_auto;
-};
-queue AutoControl autonomous;
diff --git a/y2014_bot3/autonomous/auto_main.cc b/y2014_bot3/autonomous/auto_main.cc
deleted file mode 100644
index 55c7c05..0000000
--- a/y2014_bot3/autonomous/auto_main.cc
+++ /dev/null
@@ -1,43 +0,0 @@
-#include <stdio.h>
-
-#include "aos/time/time.h"
-#include "aos/init.h"
-#include "aos/logging/logging.h"
-#include "y2014_bot3/autonomous/auto.q.h"
-#include "y2014_bot3/autonomous/auto.h"
-
-int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init(-1);
-
- LOG(INFO, "Auto main started.\n");
- ::y2014_bot3::autonomous::autonomous.FetchLatest();
- while (!::y2014_bot3::autonomous::autonomous.get()) {
- ::y2014_bot3::autonomous::autonomous.FetchNextBlocking();
- LOG(INFO, "Got another auto packet\n");
- }
-
- while (true) {
- while (!::y2014_bot3::autonomous::autonomous->run_auto) {
- ::y2014_bot3::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_bot3::autonomous::HandleAuto();
-
- auto elapsed_time = ::aos::monotonic_clock::now() - start_time;
- LOG(INFO, "Auto mode exited in %f, waiting for it to finish.\n",
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- elapsed_time)
- .count());
- while (::y2014_bot3::autonomous::autonomous->run_auto) {
- ::y2014_bot3::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/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index 6736329..70eff54 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -10,9 +10,12 @@
#include "aos/util/log_interval.h"
#include "aos/time/time.h"
-#include "frc971/queues/gyro.q.h"
+#include "aos/input/drivetrain_input.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014_bot3/autonomous/auto.q.h"
+#include "frc971/queues/gyro.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_base.h"
#include "y2014_bot3/control_loops/rollers/rollers.q.h"
using ::frc971::control_loops::drivetrain_queue;
@@ -23,6 +26,7 @@
using ::aos::input::driver_station::POVLocation;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::ControlBit;
+using ::aos::input::DrivetrainInputReader;
namespace y2014_bot3 {
namespace input {
@@ -44,7 +48,11 @@
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() : is_high_gear_(false) {}
+ Reader() {
+ drivetrain_input_reader_ = DrivetrainInputReader::Make(
+ DrivetrainInputReader::InputType::kSteeringWheel,
+ ::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig());
+ }
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
bool last_auto_running = auto_running_;
@@ -62,48 +70,20 @@
HandleDrivetrain(data);
HandleTeleop(data);
}
+
+ action_queue_.Tick();
}
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
- bool is_control_loop_driving = false;
- const double wheel = -data.GetAxis(kSteeringWheel);
- const double throttle = -data.GetAxis(kDriveThrottle);
-
- if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
- left_goal_ = drivetrain_queue.status->estimated_left_position;
- right_goal_ = drivetrain_queue.status->estimated_right_position;
- }
- }
- if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
- is_control_loop_driving = true;
- }
-
- if (!drivetrain_queue.goal.MakeWithBuilder()
- .wheel(wheel)
- .throttle(throttle)
- .highgear(is_high_gear_)
- .quickturn(data.IsPressed(kQuickTurn))
- .control_loop_driving(is_control_loop_driving)
- .left_goal(left_goal_ - wheel * 0.5 + throttle * 0.3)
- .right_goal(right_goal_ + wheel * 0.5 + throttle * 0.3)
- .left_velocity_goal(0)
- .right_velocity_goal(0)
- .Send()) {
- LOG(WARNING, "sending stick values failed\n");
- }
-
- if (data.PosEdge(kShiftLow)) {
- is_high_gear_ = false;
- }
-
- if (data.PosEdge(kShiftHigh) || data.PosEdge(kShiftHigh2)) {
- is_high_gear_ = true;
- }
+ drivetrain_input_reader_->HandleDrivetrain(data);
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+
// Rollers.
auto rollers_goal = control_loops::rollers_queue.goal.MakeMessage();
rollers_goal->Zero();
@@ -126,24 +106,26 @@
private:
void StartAuto() {
LOG(INFO, "Starting auto mode.\n");
- ::y2014_bot3::autonomous::autonomous.MakeWithBuilder().run_auto(true).Send();
+ ::frc971::autonomous::AutonomousActionParams params;
+ params.mode = 0;
+ action_queue_.EnqueueAction(
+ ::frc971::autonomous::MakeAutonomousAction(params));
}
void StopAuto() {
LOG(INFO, "Stopping auto mode\n");
- ::y2014_bot3::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
+ action_queue_.CancelAllActions();
}
bool auto_running_ = false;
- bool is_high_gear_;
- // Turning goals.
- double left_goal_;
- double right_goal_;
-
::aos::util::SimpleLogInterval no_drivetrain_status_ =
::aos::util::SimpleLogInterval(::std::chrono::milliseconds(200), WARNING,
"no drivetrain status");
+
+ ::aos::common::actions::ActionQueue action_queue_;
+
+ ::std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
};
} // namespace joysticks
diff --git a/y2014_bot3/wpilib_interface.cc b/y2014_bot3/wpilib_interface.cc
index 2b0edb5..613b757 100644
--- a/y2014_bot3/wpilib_interface.cc
+++ b/y2014_bot3/wpilib_interface.cc
@@ -31,7 +31,6 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014_bot3/control_loops/drivetrain/drivetrain_base.h"
#include "y2014_bot3/control_loops/rollers/rollers.q.h"
-#include "y2014_bot3/autonomous/auto.q.h"
#include "y2014_bot3/control_loops/rollers/rollers.h"
#include "frc971/wpilib/joystick_sender.h"