Third robot commit.
All tests pass!
Change-Id: I086248537f075fd06afdfb3e94670eb7646aaf6c
diff --git a/y2016_bot3/actors/BUILD b/y2016_bot3/actors/BUILD
new file mode 100644
index 0000000..6453f13
--- /dev/null
+++ b/y2016_bot3/actors/BUILD
@@ -0,0 +1,102 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+filegroup(
+ name = 'binaries',
+ srcs = [
+ ':drivetrain_action',
+ ':autonomous_action',
+ ],
+)
+
+queue_library(
+ name = 'drivetrain_action_queue',
+ srcs = [
+ 'drivetrain_action.q',
+ ],
+ deps = [
+ '//aos/common/actions:action_queue',
+ ],
+)
+
+cc_library(
+ name = 'drivetrain_action_lib',
+ srcs = [
+ 'drivetrain_actor.cc',
+ ],
+ hdrs = [
+ 'drivetrain_actor.h',
+ ],
+ deps = [
+ ':drivetrain_action_queue',
+ '//aos/common:time',
+ '//aos/common:math',
+ '//aos/common/util:phased_loop',
+ '//aos/common/logging',
+ '//aos/common/actions:action_lib',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/util:trapezoid_profile',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops:state_feedback_loop',
+ '//third_party/eigen',
+ '//y2016_bot3/control_loops/intake:intake_lib',
+ '//y2016_bot3/control_loops/drivetrain:drivetrain_base',
+ '//y2016_bot3/control_loops/drivetrain:polydrivetrain_plants',
+ ],
+)
+
+cc_binary(
+ name = 'drivetrain_action',
+ srcs = [
+ 'drivetrain_actor_main.cc',
+ ],
+ deps = [
+ ':drivetrain_action_lib',
+ ':drivetrain_action_queue',
+ '//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',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//y2016_bot3/queues:ball_detector',
+ '//y2016_bot3/control_loops/intake:intake_queue',
+ '//y2016_bot3/control_loops/drivetrain:drivetrain_base',
+ '//y2016_bot3/queues:profile_params',
+ ],
+)
+
+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_bot3/actors/autonomous_action.q b/y2016_bot3/actors/autonomous_action.q
new file mode 100644
index 0000000..37e4866
--- /dev/null
+++ b/y2016_bot3/actors/autonomous_action.q
@@ -0,0 +1,29 @@
+package y2016_bot3.actors;
+
+import "aos/common/actions/actions.q";
+
+message AutonomousMode {
+ // Mode read from the mode setting sensors.
+ int32_t mode;
+};
+
+queue AutonomousMode auto_mode;
+
+struct AutonomousActionParams {
+ // The mode from the sensors when auto starts.
+ int32_t mode;
+};
+
+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_bot3/actors/autonomous_actor.cc b/y2016_bot3/actors/autonomous_actor.cc
new file mode 100644
index 0000000..d6263a7
--- /dev/null
+++ b/y2016_bot3/actors/autonomous_actor.cc
@@ -0,0 +1,358 @@
+#include "y2016_bot3/actors/autonomous_actor.h"
+
+#include <inttypes.h>
+
+#include <cmath>
+
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2016_bot3/control_loops/drivetrain/drivetrain_base.h"
+#include "y2016_bot3/control_loops/intake/intake.q.h"
+#include "y2016_bot3/actors/autonomous_action.q.h"
+#include "y2016_bot3/queues/ball_detector.q.h"
+
+namespace y2016_bot3 {
+namespace actors {
+using ::frc971::control_loops::drivetrain_queue;
+
+namespace {
+const ProfileParameters kLowBarDrive = {1.3, 2.5};
+} // namespace
+
+AutonomousActor::AutonomousActor(actors::AutonomousActionQueueGroup *s)
+ : aos::common::actions::ActorBase<actors::AutonomousActionQueueGroup>(s),
+ dt_config_(control_loops::drivetrain::GetDrivetrainConfig()),
+ initial_drivetrain_({0.0, 0.0}) {}
+
+void AutonomousActor::ResetDrivetrain() {
+ LOG(INFO, "resetting the drivetrain\n");
+ drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .steering(0.0)
+ .throttle(0.0)
+ .left_goal(initial_drivetrain_.left)
+ .left_velocity_goal(0)
+ .right_goal(initial_drivetrain_.right)
+ .right_velocity_goal(0)
+ .Send();
+}
+
+void AutonomousActor::StartDrive(double distance, double angle,
+ ProfileParameters linear) {
+ {
+ LOG(INFO, "Driving distance %f, angle %f\n", distance, angle);
+ {
+ const double dangle = angle * dt_config_.robot_radius;
+ initial_drivetrain_.left += distance - dangle;
+ initial_drivetrain_.right += distance + dangle;
+ }
+
+ auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
+ drivetrain_message->control_loop_driving = true;
+ drivetrain_message->steering = 0.0;
+ drivetrain_message->throttle = 0.0;
+ drivetrain_message->left_goal = initial_drivetrain_.left;
+ drivetrain_message->left_velocity_goal = 0;
+ drivetrain_message->right_goal = initial_drivetrain_.right;
+ drivetrain_message->right_velocity_goal = 0;
+ drivetrain_message->linear = linear;
+
+ LOG_STRUCT(DEBUG, "drivetrain_goal", *drivetrain_message);
+
+ drivetrain_message.Send();
+ }
+}
+
+void AutonomousActor::InitializeEncoders() {
+ drivetrain_queue.status.FetchAnother();
+ initial_drivetrain_.left = drivetrain_queue.status->estimated_left_position;
+ initial_drivetrain_.right = drivetrain_queue.status->estimated_right_position;
+}
+
+void AutonomousActor::WaitUntilDoneOrCanceled(
+ ::std::unique_ptr<aos::common::actions::Action> action) {
+ if (!action) {
+ LOG(ERROR, "No action, not waiting\n");
+ return;
+ }
+
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ while (true) {
+ // Poll the running bit and see if we should cancel.
+ phased_loop.SleepUntilNext();
+ if (!action->Running() || ShouldCancel()) {
+ return;
+ }
+ }
+}
+
+bool AutonomousActor::WaitForDriveNear(double distance, double angle) {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ constexpr double kPositionTolerance = 0.02;
+ constexpr double kProfileTolerance = 0.001;
+
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get()) {
+ const double left_profile_error =
+ (initial_drivetrain_.left -
+ drivetrain_queue.status->profiled_left_position_goal);
+ const double right_profile_error =
+ (initial_drivetrain_.right -
+ drivetrain_queue.status->profiled_right_position_goal);
+
+ const double left_error =
+ (initial_drivetrain_.left -
+ drivetrain_queue.status->estimated_left_position);
+ const double right_error =
+ (initial_drivetrain_.right -
+ drivetrain_queue.status->estimated_right_position);
+
+ const double profile_distance_to_go =
+ (left_profile_error + right_profile_error) / 2.0;
+ const double profile_angle_to_go =
+ (right_profile_error - left_profile_error) /
+ (dt_config_.robot_radius * 2.0);
+
+ const double distance_to_go = (left_error + right_error) / 2.0;
+ const double angle_to_go =
+ (right_error - left_error) / (dt_config_.robot_radius * 2.0);
+
+ if (::std::abs(profile_distance_to_go) < distance + kProfileTolerance &&
+ ::std::abs(profile_angle_to_go) < angle + kProfileTolerance &&
+ ::std::abs(distance_to_go) < distance + kPositionTolerance &&
+ ::std::abs(angle_to_go) < angle + kPositionTolerance) {
+ LOG(INFO, "Closer than %f distance, %f angle\n", distance, angle);
+ return true;
+ }
+ }
+ }
+}
+
+bool AutonomousActor::WaitForDriveProfileDone() {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ constexpr double kProfileTolerance = 0.001;
+
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get()) {
+ if (::std::abs(drivetrain_queue.status->profiled_left_position_goal -
+ initial_drivetrain_.left) < kProfileTolerance &&
+ ::std::abs(drivetrain_queue.status->profiled_right_position_goal -
+ initial_drivetrain_.right) < kProfileTolerance) {
+ LOG(INFO, "Finished drive\n");
+ return true;
+ }
+ }
+ }
+}
+
+bool AutonomousActor::IsDriveDone() {
+ constexpr double kPositionTolerance = 0.02;
+ constexpr double kVelocityTolerance = 0.10;
+ constexpr double kProfileTolerance = 0.001;
+
+ if (drivetrain_queue.status.get()) {
+ if (::std::abs(drivetrain_queue.status->profiled_left_position_goal -
+ initial_drivetrain_.left) < kProfileTolerance &&
+ ::std::abs(drivetrain_queue.status->profiled_right_position_goal -
+ initial_drivetrain_.right) < kProfileTolerance &&
+ ::std::abs(drivetrain_queue.status->estimated_left_position -
+ initial_drivetrain_.left) < kPositionTolerance &&
+ ::std::abs(drivetrain_queue.status->estimated_right_position -
+ initial_drivetrain_.right) < kPositionTolerance &&
+ ::std::abs(drivetrain_queue.status->estimated_left_velocity) <
+ kVelocityTolerance &&
+ ::std::abs(drivetrain_queue.status->estimated_right_velocity) <
+ kVelocityTolerance) {
+ LOG(INFO, "Finished drive\n");
+ return true;
+ }
+ }
+ return false;
+}
+
+bool AutonomousActor::WaitForDriveDone() {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (IsDriveDone()) {
+ return true;
+ }
+ }
+}
+
+void AutonomousActor::MoveIntake(double intake_goal,
+ const ProfileParameters intake_params,
+ bool traverse_up, double roller_power) {
+
+ auto new_intake_goal =
+ ::y2016_bot3::control_loops::intake_queue.goal.MakeMessage();
+
+ new_intake_goal->angle_intake = intake_goal;
+
+ new_intake_goal->max_angular_velocity_intake =
+ intake_params.max_velocity;
+
+ new_intake_goal->max_angular_acceleration_intake =
+ intake_params.max_acceleration;
+
+ new_intake_goal->voltage_top_rollers = roller_power;
+ new_intake_goal->voltage_bottom_rollers = roller_power;
+
+ new_intake_goal->traverse_unlatched = true;
+ new_intake_goal->traverse_down = !traverse_up;
+
+ if (!new_intake_goal.Send()) {
+ LOG(ERROR, "Sending intake goal failed.\n");
+ }
+}
+
+bool AutonomousActor::IntakeDone() {
+ control_loops::intake_queue.status.FetchAnother();
+
+ constexpr double kProfileError = 1e-5;
+ constexpr double kEpsilon = 0.15;
+
+ if (control_loops::intake_queue.status->state < 12 ||
+ control_loops::intake_queue.status->state == 16) {
+ LOG(ERROR, "Intake no longer running, aborting action\n");
+ return true;
+ }
+
+ if (::std::abs(control_loops::intake_queue.status->intake.goal_angle -
+ intake_goal_.intake) < kProfileError &&
+ ::std::abs(control_loops::intake_queue.status->intake
+ .goal_angular_velocity) < kProfileError) {
+ LOG(DEBUG, "Profile done.\n");
+ if (::std::abs(control_loops::intake_queue.status->intake.angle -
+ intake_goal_.intake) < kEpsilon &&
+ ::std::abs(control_loops::intake_queue.status->intake
+ .angular_velocity) < kEpsilon) {
+ LOG(INFO, "Near goal, done.\n");
+ return true;
+ }
+ }
+ return false;
+}
+
+void AutonomousActor::WaitForIntake() {
+ while (true) {
+ if (ShouldCancel()) return;
+ if (IntakeDone()) return;
+ }
+}
+
+void AutonomousActor::LowBarDrive() {
+ StartDrive(-5.5, 0.0, kLowBarDrive);
+
+ if (!WaitForDriveNear(5.3, 0.0)) return;
+
+ if (!WaitForDriveNear(5.0, 0.0)) return;
+
+ StartDrive(0.0, 0.0, kLowBarDrive);
+
+ if (!WaitForDriveNear(3.0, 0.0)) return;
+
+ StartDrive(0.0, 0.0, kLowBarDrive);
+
+ if (!WaitForDriveNear(1.0, 0.0)) return;
+
+ StartDrive(0, -M_PI / 4.0 - 0.2, kLowBarDrive);
+}
+
+void AutonomousActor::WaitForBallOrDriveDone() {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ while (true) {
+ if (ShouldCancel()) {
+ return;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (IsDriveDone()) {
+ return;
+ }
+
+ ::y2016_bot3::sensors::ball_detector.FetchLatest();
+ if (::y2016_bot3::sensors::ball_detector.get()) {
+ const bool ball_detected =
+ ::y2016_bot3::sensors::ball_detector->voltage > 2.5;
+ if (ball_detected) {
+ return;
+ }
+ }
+ }
+}
+
+void AutonomousActor::WaitForBall() {
+ while (true) {
+ ::y2016_bot3::sensors::ball_detector.FetchAnother();
+ if (::y2016_bot3::sensors::ball_detector.get()) {
+ const bool ball_detected =
+ ::y2016_bot3::sensors::ball_detector->voltage > 2.5;
+ if (ball_detected) {
+ return;
+ }
+ if (ShouldCancel()) return;
+ }
+ }
+}
+
+bool AutonomousActor::RunAction(const actors::AutonomousActionParams ¶ms) {
+ aos::time::Time start_time = aos::time::Time::Now();
+ LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
+
+ InitializeEncoders();
+ ResetDrivetrain();
+
+ switch (params.mode) {
+ case 0:
+ default:
+ // TODO: Write auto code
+ LOG(ERROR, "Uhh... someone implement this please :)");
+ break;
+ }
+
+ if (!WaitForDriveDone()) return true;
+
+ LOG(INFO, "Done %f\n", (aos::time::Time::Now() - start_time).ToSeconds());
+
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+
+ while (!ShouldCancel()) {
+ phased_loop.SleepUntilNext();
+ }
+ LOG(DEBUG, "Done running\n");
+
+ return true;
+}
+
+::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
+ const ::y2016_bot3::actors::AutonomousActionParams ¶ms) {
+ return ::std::unique_ptr<AutonomousAction>(
+ new AutonomousAction(&::y2016_bot3::actors::autonomous_action, params));
+}
+
+} // namespace actors
+} // namespace y2016_bot3
diff --git a/y2016_bot3/actors/autonomous_actor.h b/y2016_bot3/actors/autonomous_actor.h
new file mode 100644
index 0000000..607e833
--- /dev/null
+++ b/y2016_bot3/actors/autonomous_actor.h
@@ -0,0 +1,90 @@
+#ifndef Y2016_BOT3_ACTORS_AUTONOMOUS_ACTOR_H_
+#define Y2016_BOT3_ACTORS_AUTONOMOUS_ACTOR_H_
+
+#include <memory>
+
+#include "aos/common/actions/actor.h"
+#include "aos/common/actions/actions.h"
+
+#include "y2016_bot3/actors/autonomous_action.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2016_bot3 {
+namespace actors {
+using ::frc971::ProfileParameters;
+
+class AutonomousActor
+ : public ::aos::common::actions::ActorBase<AutonomousActionQueueGroup> {
+ public:
+ explicit AutonomousActor(AutonomousActionQueueGroup *s);
+
+ bool RunAction(const actors::AutonomousActionParams ¶ms) override;
+
+ private:
+ void ResetDrivetrain();
+ void InitializeEncoders();
+ void WaitUntilDoneOrCanceled(::std::unique_ptr<aos::common::actions::Action>
+ action);
+ void StartDrive(double distance, double angle,
+ ::frc971::ProfileParameters linear);
+ // Waits for the drive motion to finish. Returns true if it succeeded, and
+ // false if it cancels.
+ bool WaitForDriveDone();
+ void WaitForBallOrDriveDone();
+
+ void StealAndMoveOverBy(double distance);
+
+ // Returns true if the drive has finished.
+ bool IsDriveDone();
+
+ // Waits until the profile and distance is within distance and angle of the
+ // goal. Returns true on success, and false when canceled.
+ bool WaitForDriveNear(double distance, double angle);
+
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig dt_config_;
+
+ // Initial drivetrain positions.
+ struct InitialDrivetrain {
+ double left;
+ double right;
+ };
+ InitialDrivetrain initial_drivetrain_;
+
+ // Internal struct holding superstructure goals sent by autonomous to the
+ // loop.
+ struct IntakeGoal {
+ double intake;
+ };
+ IntakeGoal intake_goal_;
+
+ void MoveIntake(double intake, const ProfileParameters intake_params,
+ bool traverse_up, double roller_power);
+ void WaitForIntakeProfile();
+ void WaitForIntakeLow();
+ void WaitForIntake();
+ bool IntakeDone();
+ bool WaitForDriveProfileDone();
+
+ void WaitForBall();
+
+ void LowBarDrive();
+ // Drive to the middle spot over the middle position. Designed for the rock
+ // wall, rough terain, or ramparts.
+ void MiddleDrive();
+
+ void OneFromMiddleDrive(bool left);
+ void TwoFromMiddleDrive();
+};
+
+typedef ::aos::common::actions::TypedAction<AutonomousActionQueueGroup>
+ AutonomousAction;
+
+// Makes a new AutonomousActor action.
+::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
+ const ::y2016_bot3::actors::AutonomousActionParams ¶ms);
+
+} // namespace actors
+} // namespace y2016_bot3
+
+#endif // Y2016_BOT3_ACTORS_AUTONOMOUS_ACTOR_H_
diff --git a/y2016_bot3/actors/autonomous_actor_main.cc b/y2016_bot3/actors/autonomous_actor_main.cc
new file mode 100644
index 0000000..43cbbe8
--- /dev/null
+++ b/y2016_bot3/actors/autonomous_actor_main.cc
@@ -0,0 +1,18 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2016_bot3/actors/autonomous_action.q.h"
+#include "y2016_bot3/actors/autonomous_actor.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init(-1);
+
+ ::y2016_bot3::actors::AutonomousActor autonomous(
+ &::y2016_bot3::actors::autonomous_action);
+ autonomous.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2016_bot3/actors/drivetrain_action.q b/y2016_bot3/actors/drivetrain_action.q
new file mode 100644
index 0000000..7775cad
--- /dev/null
+++ b/y2016_bot3/actors/drivetrain_action.q
@@ -0,0 +1,29 @@
+package y2016_bot3.actors;
+
+import "aos/common/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/y2016_bot3/actors/drivetrain_actor.cc b/y2016_bot3/actors/drivetrain_actor.cc
new file mode 100644
index 0000000..686c50f
--- /dev/null
+++ b/y2016_bot3/actors/drivetrain_actor.cc
@@ -0,0 +1,181 @@
+#include "y2016_bot3/actors/drivetrain_actor.h"
+
+#include <functional>
+#include <numeric>
+
+#include <Eigen/Dense>
+
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/time.h"
+
+#include "y2016_bot3/actors/drivetrain_actor.h"
+#include "y2016_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+namespace y2016_bot3 {
+namespace actors {
+
+DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup *s)
+ : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
+ loop_(::y2016_bot3::control_loops::drivetrain::MakeDrivetrainLoop()) {
+ loop_.set_controller_index(3);
+}
+
+bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
+ static const auto K = loop_.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(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(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);
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(5, 2500);
+
+ ::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()
+ .control_loop_driving(true)
+ .highgear(true)
+ .left_goal(left_goal_state(0, 0) + params.left_initial_position)
+ .right_goal(right_goal_state(0, 0) + params.right_initial_position)
+ .left_velocity_goal(left_goal_state(1, 0))
+ .right_velocity_goal(right_goal_state(1, 0))
+ .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 ::y2016_bot3::actors::DrivetrainActionParams ¶ms) {
+ return ::std::unique_ptr<DrivetrainAction>(
+ new DrivetrainAction(&::y2016_bot3::actors::drivetrain_action, params));
+}
+
+} // namespace actors
+} // namespace y2016_bot3
diff --git a/y2016_bot3/actors/drivetrain_actor.h b/y2016_bot3/actors/drivetrain_actor.h
new file mode 100644
index 0000000..ebf3ed5
--- /dev/null
+++ b/y2016_bot3/actors/drivetrain_actor.h
@@ -0,0 +1,36 @@
+#ifndef Y2016_BOT3_ACTORS_DRIVETRAIN_ACTOR_H_
+#define Y2016_BOT3_ACTORS_DRIVETRAIN_ACTOR_H_
+
+#include <memory>
+
+#include "aos/common/actions/actor.h"
+#include "aos/common/actions/actions.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "y2016_bot3/actors/drivetrain_action.q.h"
+
+namespace y2016_bot3 {
+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 ::y2016_bot3::actors::DrivetrainActionParams ¶ms);
+
+} // namespace actors
+} // namespace y2016_bot3
+
+#endif
diff --git a/y2016_bot3/actors/drivetrain_actor_main.cc b/y2016_bot3/actors/drivetrain_actor_main.cc
new file mode 100644
index 0000000..41a4d28
--- /dev/null
+++ b/y2016_bot3/actors/drivetrain_actor_main.cc
@@ -0,0 +1,18 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2016_bot3/actors/drivetrain_action.q.h"
+#include "y2016_bot3/actors/drivetrain_actor.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init(-1);
+
+ ::y2016_bot3::actors::DrivetrainActor drivetrain(
+ &::y2016_bot3::actors::drivetrain_action);
+ drivetrain.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}