Created actors files for autonomous and got rid of the unique autonomous folder.
Change-Id: I4b4d12070def8ed1a661b9ffd2f82f8d342b6c4b
diff --git a/y2016/BUILD b/y2016/BUILD
index b5d3f7c..71bdace 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -37,6 +37,7 @@
'//frc971/autonomous:auto_queue',
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/queues:gyro',
+ '//y2016/actors:autonomous_action_lib',
'//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/superstructure:superstructure_lib',
'//y2016/control_loops/superstructure:superstructure_queue',
@@ -52,7 +53,7 @@
'//y2016/control_loops/drivetrain:drivetrain',
'//y2016/control_loops/superstructure:superstructure',
'//y2016/control_loops/shooter:shooter',
- '//y2016/autonomous:auto',
+ '//y2016/actors:autonomous_action',
'//y2016/wpilib:wpilib_interface',
],
srcs = [
@@ -68,7 +69,7 @@
'//y2016/control_loops/drivetrain:drivetrain.stripped',
'//y2016/control_loops/superstructure:superstructure.stripped',
'//y2016/control_loops/shooter:shooter.stripped',
- '//y2016/autonomous:auto.stripped',
+ '//y2016/actors:autonomous_action.stripped',
'//y2016/wpilib:wpilib_interface.stripped',
],
srcs = [
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index 9d69c6b..771e6ef 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -7,6 +7,7 @@
srcs = [
':drivetrain_action',
':superstructure_action',
+ ':autonomous_action',
],
)
@@ -95,3 +96,41 @@
'//aos/linux_code:init',
],
)
+
+queue_library(
+ name = 'autonomous_action_queue',
+ srcs = [
+ 'autonomous_action.q',
+ ],
+ deps = [
+ '//aos/common/actions:action_queue',
+ ],
+)
+
+cc_library(
+ name = 'autonomous_action_lib',
+ srcs = [
+ 'autonomous_actor.cc',
+ ],
+ hdrs = [
+ 'autonomous_actor.h',
+ ],
+ deps = [
+ ':autonomous_action_queue',
+ '//aos/common/util:phased_loop',
+ '//aos/common/logging',
+ '//aos/common/actions:action_lib',
+ ],
+)
+
+cc_binary(
+ name = 'autonomous_action',
+ srcs = [
+ 'autonomous_actor_main.cc',
+ ],
+ deps = [
+ ':autonomous_action_lib',
+ ':autonomous_action_queue',
+ '//aos/linux_code:init',
+ ],
+)
diff --git a/y2016/actors/autonomous_action.q b/y2016/actors/autonomous_action.q
new file mode 100644
index 0000000..e2bfd81
--- /dev/null
+++ b/y2016/actors/autonomous_action.q
@@ -0,0 +1,21 @@
+package y2016.actors;
+
+import "aos/common/actions/actions.q";
+
+struct AutonomousActionParams {
+ int32_t mode; // 0 = single ball auto
+};
+
+queue_group AutonomousActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ AutonomousActionParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group AutonomousActionQueueGroup autonomous_action;
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
new file mode 100644
index 0000000..d89f0ce
--- /dev/null
+++ b/y2016/actors/autonomous_actor.cc
@@ -0,0 +1,34 @@
+#include "y2016/actors/autonomous_actor.h"
+
+#include <inttypes.h>
+
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/logging.h"
+#include "y2016/actors/autonomous_action.q.h"
+
+namespace y2016 {
+namespace actors {
+
+AutonomousActor::AutonomousActor(actors::AutonomousActionQueueGroup *s)
+ : aos::common::actions::ActorBase<actors::AutonomousActionQueueGroup>(s) {}
+
+bool AutonomousActor::RunAction(const actors::AutonomousActionParams ¶ms) {
+ LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
+
+ while (true) {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ break;
+ }
+
+ return true;
+}
+
+::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
+ const ::y2016::actors::AutonomousActionParams ¶ms) {
+ return ::std::unique_ptr<AutonomousAction>(
+ new AutonomousAction(&::y2016::actors::autonomous_action, params));
+}
+
+} // namespace actors
+} // namespace y2016
diff --git a/y2016/actors/autonomous_actor.h b/y2016/actors/autonomous_actor.h
new file mode 100644
index 0000000..2a9d244
--- /dev/null
+++ b/y2016/actors/autonomous_actor.h
@@ -0,0 +1,31 @@
+#ifndef Y2016_ACTORS_AUTONOMOUS_ACTOR_H_
+#define Y2016_ACTORS_AUTONOMOUS_ACTOR_H_
+
+#include <memory>
+
+#include "aos/common/actions/actor.h"
+#include "aos/common/actions/actions.h"
+#include "y2016/actors/autonomous_action.q.h"
+
+namespace y2016 {
+namespace actors {
+
+class AutonomousActor
+ : public ::aos::common::actions::ActorBase<AutonomousActionQueueGroup> {
+ public:
+ explicit AutonomousActor(AutonomousActionQueueGroup *s);
+
+ bool RunAction(const actors::AutonomousActionParams ¶ms) override;
+};
+
+using AutonomousAction =
+ ::aos::common::actions::TypedAction<AutonomousActionQueueGroup>;
+
+// Makes a new AutonomousActor action.
+::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
+ const ::y2016::actors::AutonomousActionParams ¶ms);
+
+} // namespace actors
+} // namespace y2016
+
+#endif // Y2016_ACTORS_AUTONOMOUS_ACTOR_H_
diff --git a/y2016/actors/autonomous_actor_main.cc b/y2016/actors/autonomous_actor_main.cc
new file mode 100644
index 0000000..8f0b305
--- /dev/null
+++ b/y2016/actors/autonomous_actor_main.cc
@@ -0,0 +1,18 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2016/actors/autonomous_action.q.h"
+#include "y2016/actors/autonomous_actor.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init(-1);
+
+ ::y2016::actors::AutonomousActor autonomous(
+ &::y2016::actors::autonomous_action);
+ autonomous.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2016/autonomous/BUILD b/y2016/autonomous/BUILD
deleted file mode 100644
index 3cda768..0000000
--- a/y2016/autonomous/BUILD
+++ /dev/null
@@ -1,37 +0,0 @@
-package(default_visibility = ['//visibility:public'])
-
-cc_library(
- name = 'auto_lib',
- srcs = [
- 'auto.cc',
- ],
- hdrs = [
- 'auto.h',
- ],
- deps = [
- '//aos/common/actions:action_lib',
- '//aos/common/controls:control_loop',
- '//aos/common/logging',
- '//aos/common/logging:queue_logging',
- '//aos/common:time',
- '//aos/common/util:phased_loop',
- '//aos/common/util:trapezoid_profile',
- '//frc971/autonomous:auto_queue',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//y2016:constants',
- '//y2016/queues:profile_params',
- '//y2016/actors:drivetrain_action_lib',
- ],
-)
-
-cc_binary(
- name = 'auto',
- srcs = [
- 'auto_main.cc',
- ],
- deps = [
- ':auto_lib',
- '//aos/linux_code:init',
- '//frc971/autonomous:auto_queue',
- ],
-)
diff --git a/y2016/autonomous/auto.cc b/y2016/autonomous/auto.cc
deleted file mode 100644
index 2b1c8d2..0000000
--- a/y2016/autonomous/auto.cc
+++ /dev/null
@@ -1,111 +0,0 @@
-#include "y2016/autonomous/auto.h"
-
-#include <stdio.h>
-
-#include <memory>
-
-#include "aos/common/actions/actions.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/logging/queue_logging.h"
-#include "aos/common/time.h"
-#include "aos/common/util/phased_loop.h"
-#include "aos/common/util/trapezoid_profile.h"
-
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2016/actors/drivetrain_actor.h"
-#include "y2016/constants.h"
-#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2016/queues/profile_params.q.h"
-
-using ::aos::time::Time;
-
-namespace y2016 {
-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 ResetDrivetrain() {
- LOG(INFO, "resetting the drivetrain\n");
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(false)
- .highgear(true)
- .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(
- ::std::unique_ptr<aos::common::actions::Action> action) {
- if (!action) {
- LOG(ERROR, "No action, not waiting\n");
- return;
- }
- while (true) {
- // Poll the running bit and auto done bits.
- ::aos::time::PhasedLoopXMS(10, 5000);
- if (!action->Running() || ShouldExitAuto()) {
- return;
- }
- }
-}
-
-const ProfileParams kFastDrive = {3.0, 2.5};
-const ProfileParams kSlowDrive = {2.5, 2.5};
-const ProfileParams kFastTurn = {3.0, 10.0};
-
-::std::unique_ptr<::y2016::actors::DrivetrainAction> SetDriveGoal(
- double distance, const ProfileParams drive_params, double theta = 0,
- const ProfileParams &turn_params = kFastTurn) {
- LOG(INFO, "Driving to %f\n", distance);
-
- ::y2016::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 ::std::move(drivetrain_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 HandleAuto() {
- LOG(INFO, "Handling auto mode\n");
-
- ResetDrivetrain();
- InitializeEncoders();
-}
-
-} // namespace autonomous
-} // namespace y2016
diff --git a/y2016/autonomous/auto.h b/y2016/autonomous/auto.h
deleted file mode 100644
index e5ab422..0000000
--- a/y2016/autonomous/auto.h
+++ /dev/null
@@ -1,12 +0,0 @@
-#ifndef Y2016_AUTONOMOUS_AUTO_H_
-#define Y2016_AUTONOMOUS_AUTO_H_
-
-namespace y2016 {
-namespace autonomous {
-
-void HandleAuto();
-
-} // namespace autonomous
-} // namespace y2016
-
-#endif // Y2016_AUTONOMOUS_AUTO_H_
diff --git a/y2016/autonomous/auto_main.cc b/y2016/autonomous/auto_main.cc
deleted file mode 100644
index adf1bca..0000000
--- a/y2016/autonomous/auto_main.cc
+++ /dev/null
@@ -1,46 +0,0 @@
-#include <stdio.h>
-
-#include "aos/common/time.h"
-#include "aos/linux_code/init.h"
-#include "aos/common/logging/logging.h"
-#include "frc971/autonomous/auto.q.h"
-#include "y2016/autonomous/auto.h"
-
-using ::aos::time::Time;
-
-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::time::Time start_time = ::aos::time::Time::Now();
- ::y2016::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/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 96586d3..d987268 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -20,6 +20,7 @@
#include "y2016/constants.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
+#include "y2016/actors/autonomous_actor.h"
using ::frc971::control_loops::drivetrain_queue;
using ::y2016::control_loops::shooter::shooter_queue;
@@ -289,12 +290,15 @@
private:
void StartAuto() {
LOG(INFO, "Starting auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(true).Send();
+
+ actors::AutonomousActionParams params;
+ params.mode = 0;
+ action_queue_.EnqueueAction(actors::MakeAutonomousAction(params));
}
void StopAuto() {
LOG(INFO, "Stopping auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
+ action_queue_.CancelAllActions();
}
bool is_high_gear_;