Ported over autonomous methods to the autonomous_actor.cc file.
Change-Id: Icb467b993332752b70e6429c972064f9fc9d4318
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index d89f0ce..ad361bb 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -4,22 +4,149 @@
#include "aos/common/util/phased_loop.h"
#include "aos/common/logging/logging.h"
-#include "y2016/actors/autonomous_action.q.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2016/control_loops/drivetrain/drivetrain_base.h"
namespace y2016 {
namespace actors {
+using ::frc971::control_loops::ProfileParameters;
+using ::frc971::control_loops::drivetrain_queue;
+
+namespace {
+const ProfileParameters kSlowDrive = {1.0, 1.0};
+const ProfileParameters kFastDrive = {3.0, 2.5};
+
+const ProfileParameters kSlowTurn = {1.7, 3.0};
+const ProfileParameters kFastTurn = {3.0, 10.0};
+} // namespace
AutonomousActor::AutonomousActor(actors::AutonomousActionQueueGroup *s)
- : aos::common::actions::ActorBase<actors::AutonomousActionQueueGroup>(s) {}
+ : aos::common::actions::ActorBase<actors::AutonomousActionQueueGroup>(s),
+ left_initial_position_(0.0),
+ right_initial_position_(0.0),
+ dt_config_(control_loops::drivetrain::GetDrivetrainConfig()) {}
+
+void AutonomousActor::ResetDrivetrain() {
+ LOG(INFO, "resetting the drivetrain\n");
+ 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 AutonomousActor::StartDrive(double distance, double angle,
+ ProfileParameters linear,
+ ProfileParameters angular) {
+ {
+ {
+ const double dangle = angle * dt_config_.robot_radius;
+ left_initial_position_ += distance - dangle;
+ right_initial_position_ += distance + dangle;
+ }
+
+ auto drivetrain_message =
+ drivetrain_queue.goal.MakeMessage();
+ drivetrain_message->control_loop_driving = true;
+ drivetrain_message->highgear = true;
+ drivetrain_message->steering = 0.0;
+ drivetrain_message->throttle = 0.0;
+ drivetrain_message->left_goal = left_initial_position_;
+ drivetrain_message->left_velocity_goal = 0;
+ drivetrain_message->right_goal = right_initial_position_;
+ drivetrain_message->right_velocity_goal = 0;
+ drivetrain_message->linear = linear;
+ drivetrain_message->angular = angular;
+
+ LOG_STRUCT(DEBUG, "drivetrain_goal", *drivetrain_message);
+
+ drivetrain_message.Send();
+ }
+}
+
+void AutonomousActor::InitializeEncoders() {
+ drivetrain_queue.status.FetchAnother();
+ left_initial_position_ = drivetrain_queue.status->estimated_left_position;
+ right_initial_position_ = 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::WaitForDriveDone() {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ constexpr double kPositionTolerance = 0.02;
+ constexpr double kVelocityTolerance = 0.10;
+ 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 -
+ left_initial_position_) < kProfileTolerance &&
+ ::std::abs(drivetrain_queue.status->profiled_right_position_goal -
+ right_initial_position_) < kProfileTolerance &&
+ ::std::abs(drivetrain_queue.status->estimated_left_position -
+ left_initial_position_) < kPositionTolerance &&
+ ::std::abs(drivetrain_queue.status->estimated_right_position -
+ right_initial_position_) < 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;
+ }
+ }
+ }
+}
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;
+ InitializeEncoders();
+ ResetDrivetrain();
+
+ StartDrive(1.0, 0.0, kSlowDrive, kSlowTurn);
+
+ if (!WaitForDriveDone()) return true;
+
+ StartDrive(0.0, M_PI, kSlowDrive, kSlowTurn);
+
+ if (!WaitForDriveDone()) return true;
+
+ ::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;
}
diff --git a/y2016/actors/autonomous_actor.h b/y2016/actors/autonomous_actor.h
index 2a9d244..a4ace8a 100644
--- a/y2016/actors/autonomous_actor.h
+++ b/y2016/actors/autonomous_actor.h
@@ -5,7 +5,10 @@
#include "aos/common/actions/actor.h"
#include "aos/common/actions/actions.h"
+
#include "y2016/actors/autonomous_action.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
namespace y2016 {
namespace actors {
@@ -16,10 +19,26 @@
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::control_loops::ProfileParameters linear,
+ ::frc971::control_loops::ProfileParameters angular);
+ // Waits for the drive motion to finish. Returns true if it succeeded, and
+ // false if it cancels.
+ bool WaitForDriveDone();
+
+ double left_initial_position_, right_initial_position_;
+
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig dt_config_;
};
-using AutonomousAction =
- ::aos::common::actions::TypedAction<AutonomousActionQueueGroup>;
+typedef ::aos::common::actions::TypedAction<AutonomousActionQueueGroup>
+ AutonomousAction;
// Makes a new AutonomousActor action.
::std::unique_ptr<AutonomousAction> MakeAutonomousAction(