Basic drive in a square auto mode.
Change-Id: Idbe7b8c7b3738ff12451ff80209874cf4c47a147
diff --git a/y2018/BUILD b/y2018/BUILD
index 3640115..6606233 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -11,7 +11,8 @@
":wpilib_interface",
"//aos:prime_start_binaries",
"//y2018/control_loops/drivetrain:drivetrain",
- "//y2018/control_loops/superstructure:superstructure.stripped",
+ "//y2018/actors:autonomous_action",
+ "//y2018/control_loops/superstructure:superstructure",
],
)
@@ -25,6 +26,7 @@
":joystick_reader.stripped",
":wpilib_interface.stripped",
"//aos:prime_start_binaries_stripped",
+ "//y2018/actors:autonomous_action.stripped",
"//y2018/control_loops/drivetrain:drivetrain.stripped",
"//y2018/control_loops/superstructure:superstructure.stripped",
],
@@ -44,6 +46,7 @@
"//aos/input:joystick_input",
"//aos/linux_code:init",
"//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2018/control_loops/drivetrain:drivetrain_base",
"//y2018/control_loops/superstructure:superstructure_queue",
diff --git a/y2018/actors/BUILD b/y2018/actors/BUILD
new file mode 100644
index 0000000..fa772b8
--- /dev/null
+++ b/y2018/actors/BUILD
@@ -0,0 +1,35 @@
+load("//aos/build:queues.bzl", "queue_library")
+
+cc_library(
+ name = "autonomous_action_lib",
+ srcs = [
+ "autonomous_actor.cc",
+ ],
+ hdrs = [
+ "autonomous_actor.h",
+ ],
+ deps = [
+ "//aos/common/actions:action_lib",
+ "//aos/common/logging",
+ "//aos/common/util:phased_loop",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops/drivetrain:drivetrain_config",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//y2018/control_loops/drivetrain:drivetrain_base",
+ "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure/arm:generated_graph",
+ ],
+)
+
+cc_binary(
+ name = "autonomous_action",
+ srcs = [
+ "autonomous_actor_main.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":autonomous_action_lib",
+ "//aos/linux_code:init",
+ "//frc971/autonomous:auto_queue",
+ ],
+)
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
new file mode 100644
index 0000000..83a1295
--- /dev/null
+++ b/y2018/actors/autonomous_actor.cc
@@ -0,0 +1,69 @@
+#include "y2018/actors/autonomous_actor.h"
+
+#include <inttypes.h>
+
+#include <chrono>
+#include <cmath>
+
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2018/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2018 {
+namespace actors {
+using ::frc971::ProfileParameters;
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+namespace this_thread = ::std::this_thread;
+
+namespace {
+
+double DoubleSeconds(monotonic_clock::duration duration) {
+ return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
+ .count();
+}
+
+const ProfileParameters kDrive = {1.5, 2.0};
+const ProfileParameters kTurn = {3.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();
+
+ for (int i = 0; i < 4; ++i) {
+ StartDrive(0.0, M_PI / 2.0, kDrive, kTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ // Drive, but get within 0.3 meters
+ StartDrive(1.0, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) 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 y2018
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
new file mode 100644
index 0000000..b8d58e2
--- /dev/null
+++ b/y2018/actors/autonomous_actor.h
@@ -0,0 +1,85 @@
+#ifndef Y2018_ACTORS_AUTONOMOUS_ACTOR_H_
+#define Y2018_ACTORS_AUTONOMOUS_ACTOR_H_
+
+#include <chrono>
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/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"
+#include "y2018/control_loops/superstructure/arm/generated_graph.h"
+#include "y2018/control_loops/superstructure/superstructure.q.h"
+
+namespace y2018 {
+namespace actors {
+using ::y2018::control_loops::superstructure_queue;
+
+namespace arm = ::y2018::control_loops::superstructure::arm;
+
+class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+ public:
+ explicit AutonomousActor(::frc971::autonomous::AutonomousActionQueueGroup *s);
+
+ bool RunAction(
+ const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ private:
+ void Reset() {
+ roller_voltage_ = 0.0;
+ left_intake_angle_ = -3.3;
+ right_intake_angle_ = -3.3;
+ arm_goal_position_ = arm::NeutralIndex();
+ grab_box_ = false;
+ open_claw_ = false;
+ deploy_fork_ = false;
+ InitializeEncoders();
+ ResetDrivetrain();
+ SendSuperstructureGoal();
+ }
+
+ double roller_voltage_ = 0.0;
+ double left_intake_angle_ = -3.3;
+ double right_intake_angle_ = -3.3;
+ uint32_t arm_goal_position_ = arm::NeutralIndex();
+ bool grab_box_ = false;
+ bool open_claw_ = false;
+ bool deploy_fork_ = false;
+
+ void set_roller_voltage(double roller_voltage) {
+ roller_voltage_ = roller_voltage;
+ }
+ void set_left_intake_angle(double left_intake_angle) {
+ left_intake_angle_ = left_intake_angle;
+ }
+ void set_right_intake_angle(double right_intake_angle) {
+ right_intake_angle_ = right_intake_angle;
+ }
+ void set_arm_goal_position(uint32_t arm_goal_position) {
+ arm_goal_position_ = arm_goal_position;
+ }
+ void set_grab_box(bool grab_box) { grab_box_ = grab_box; }
+ void set_open_claw(bool open_claw) { open_claw_ = open_claw; }
+ void set_deploy_fork(bool deploy_fork) { deploy_fork_ = deploy_fork; }
+
+ void SendSuperstructureGoal() {
+ auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ new_superstructure_goal->intake.roller_voltage = roller_voltage_;
+ new_superstructure_goal->intake.left_intake_angle = left_intake_angle_;
+ new_superstructure_goal->intake.right_intake_angle = right_intake_angle_;
+
+ new_superstructure_goal->arm_goal_position = arm_goal_position_;
+ new_superstructure_goal->grab_box = grab_box_;
+ new_superstructure_goal->open_claw = open_claw_;
+ new_superstructure_goal->deploy_fork = deploy_fork_;
+
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+ }
+};
+
+} // namespace actors
+} // namespace y2018
+
+#endif // Y2018_ACTORS_AUTONOMOUS_ACTOR_H_
diff --git a/y2018/actors/autonomous_actor_main.cc b/y2018/actors/autonomous_actor_main.cc
new file mode 100644
index 0000000..733bec0
--- /dev/null
+++ b/y2018/actors/autonomous_actor_main.cc
@@ -0,0 +1,16 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/autonomous/auto.q.h"
+#include "y2018/actors/autonomous_actor.h"
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init(-1);
+
+ ::y2018::actors::AutonomousActor autonomous(
+ &::frc971::autonomous::autonomous_action);
+ autonomous.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 2baa14a..bb6c1b1 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -11,6 +11,8 @@
#include "aos/input/drivetrain_input.h"
#include "aos/input/joystick_input.h"
#include "aos/linux_code/init.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
@@ -199,7 +201,20 @@
}
private:
- void StartAuto() { LOG(INFO, "Starting auto mode\n"); }
+ void StartAuto() {
+ LOG(INFO, "Starting auto mode\n");
+
+ ::frc971::autonomous::AutonomousActionParams params;
+ ::frc971::autonomous::auto_mode.FetchLatest();
+ if (::frc971::autonomous::auto_mode.get() != nullptr) {
+ params.mode = ::frc971::autonomous::auto_mode->mode;
+ } else {
+ LOG(WARNING, "no auto mode values\n");
+ params.mode = 0;
+ }
+ action_queue_.EnqueueAction(
+ ::frc971::autonomous::MakeAutonomousAction(params));
+ }
void StopAuto() {
LOG(INFO, "Stopping auto mode\n");