Factor out drivetrain functionality from y2016 auton actor
I hope that we can re-use this in the y2017 auton actor.
Change-Id: I1258a5ef99a706ebefaba8e2238bb80df68d586f
diff --git a/frc971/autonomous/BUILD b/frc971/autonomous/BUILD
index 7f6b3e7..3c88c4b 100644
--- a/frc971/autonomous/BUILD
+++ b/frc971/autonomous/BUILD
@@ -7,4 +7,25 @@
srcs = [
'auto.q',
],
+ deps = [
+ '//aos/common/actions:action_queue',
+ ],
+)
+
+cc_library(
+ name = 'base_autonomous_actor',
+ hdrs = [
+ 'base_autonomous_actor.h',
+ ],
+ srcs = [
+ 'base_autonomous_actor.cc',
+ ],
+ deps = [
+ ':auto_queue',
+ '//aos/common/actions:action_lib',
+ '//aos/common/util:phased_loop',
+ '//aos/common/logging',
+ '//frc971/control_loops/drivetrain:drivetrain_config',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ ],
)
diff --git a/frc971/autonomous/auto.q b/frc971/autonomous/auto.q
index 5791034..1f639b1 100644
--- a/frc971/autonomous/auto.q
+++ b/frc971/autonomous/auto.q
@@ -1,8 +1,36 @@
package frc971.autonomous;
+import "aos/common/actions/actions.q";
+
message AutoControl {
// True if auto mode should be running, false otherwise.
bool run_auto;
};
queue AutoControl autonomous;
+
+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/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
new file mode 100644
index 0000000..73102ec
--- /dev/null
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -0,0 +1,278 @@
+#include "frc971/autonomous/base_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"
+
+namespace frc971 {
+namespace autonomous {
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+namespace this_thread = ::std::this_thread;
+
+namespace {} // namespace
+
+BaseAutonomousActor::BaseAutonomousActor(
+ AutonomousActionQueueGroup *s,
+ const control_loops::drivetrain::DrivetrainConfig dt_config)
+ : aos::common::actions::ActorBase<AutonomousActionQueueGroup>(s),
+ dt_config_(dt_config),
+ initial_drivetrain_({0.0, 0.0}) {}
+
+void BaseAutonomousActor::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(initial_drivetrain_.left)
+ .left_velocity_goal(0)
+ .right_goal(initial_drivetrain_.right)
+ .right_velocity_goal(0)
+ .Send();
+}
+
+void BaseAutonomousActor::InitializeEncoders() {
+ drivetrain_queue.status.FetchAnother();
+ initial_drivetrain_.left = drivetrain_queue.status->estimated_left_position;
+ initial_drivetrain_.right = drivetrain_queue.status->estimated_right_position;
+}
+
+void BaseAutonomousActor::StartDrive(double distance, double angle,
+ ProfileParameters linear,
+ ProfileParameters angular) {
+ 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->highgear = 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;
+ drivetrain_message->angular = angular;
+
+ LOG_STRUCT(DEBUG, "drivetrain_goal", *drivetrain_message);
+
+ drivetrain_message.Send();
+}
+
+void BaseAutonomousActor::WaitUntilDoneOrCanceled(
+ ::std::unique_ptr<aos::common::actions::Action> action) {
+ if (!action) {
+ LOG(ERROR, "No action, not waiting\n");
+ return;
+ }
+
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ // Poll the running bit and see if we should cancel.
+ phased_loop.SleepUntilNext();
+ if (!action->Running() || ShouldCancel()) {
+ return;
+ }
+ }
+}
+
+bool BaseAutonomousActor::WaitForDriveDone() {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (IsDriveDone()) {
+ return true;
+ }
+ }
+}
+
+bool BaseAutonomousActor::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 BaseAutonomousActor::WaitForAboveAngle(double angle) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (IsDriveDone()) {
+ return true;
+ }
+ if (drivetrain_queue.status.get()) {
+ if (drivetrain_queue.status->ground_angle > angle) {
+ return true;
+ }
+ }
+ }
+}
+
+bool BaseAutonomousActor::WaitForBelowAngle(double angle) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (IsDriveDone()) {
+ return true;
+ }
+ if (drivetrain_queue.status.get()) {
+ if (drivetrain_queue.status->ground_angle < angle) {
+ return true;
+ }
+ }
+ }
+}
+
+bool BaseAutonomousActor::WaitForMaxBy(double angle) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ double max_angle = -M_PI;
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (IsDriveDone()) {
+ return true;
+ }
+ if (drivetrain_queue.status.get()) {
+ if (drivetrain_queue.status->ground_angle > max_angle) {
+ max_angle = drivetrain_queue.status->ground_angle;
+ }
+ if (drivetrain_queue.status->ground_angle < max_angle - angle) {
+ return true;
+ }
+ }
+ }
+}
+
+bool BaseAutonomousActor::WaitForDriveNear(double distance, double angle) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(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 BaseAutonomousActor::WaitForDriveProfileDone() {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(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;
+ }
+ }
+ }
+}
+
+::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
+ const AutonomousActionParams ¶ms) {
+ return ::std::unique_ptr<AutonomousAction>(
+ new AutonomousAction(&autonomous_action, params));
+}
+
+} // namespace autonomous
+} // namespace frc971
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
new file mode 100644
index 0000000..69e2e5a
--- /dev/null
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -0,0 +1,69 @@
+#ifndef FRC971_AUTONOMOUS_BASE_AUTONOMOUS_ACTOR_H_
+#define FRC971_AUTONOMOUS_BASE_AUTONOMOUS_ACTOR_H_
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace frc971 {
+namespace autonomous {
+
+class BaseAutonomousActor
+ : public ::aos::common::actions::ActorBase<AutonomousActionQueueGroup> {
+ public:
+ explicit BaseAutonomousActor(
+ AutonomousActionQueueGroup *s,
+ const control_loops::drivetrain::DrivetrainConfig dt_config);
+
+ protected:
+ void ResetDrivetrain();
+ void InitializeEncoders();
+ void StartDrive(double distance, double angle, ProfileParameters linear,
+ ProfileParameters angular);
+
+ void WaitUntilDoneOrCanceled(
+ ::std::unique_ptr<aos::common::actions::Action> action);
+ // Waits for the drive motion to finish. Returns true if it succeeded, and
+ // false if it cancels.
+ bool WaitForDriveDone();
+
+ // Returns true if the drive has finished.
+ bool IsDriveDone();
+
+ // Waits until the robot is pitched up above the specified angle, or the move
+ // finishes. Returns true on success, and false if it cancels.
+ bool WaitForAboveAngle(double angle);
+ bool WaitForBelowAngle(double angle);
+ bool WaitForMaxBy(double angle);
+
+ // 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);
+
+ bool WaitForDriveProfileDone();
+
+ const control_loops::drivetrain::DrivetrainConfig dt_config_;
+
+ // Initial drivetrain positions.
+ struct InitialDrivetrain {
+ double left;
+ double right;
+ };
+ InitialDrivetrain initial_drivetrain_;
+};
+
+using AutonomousAction =
+ ::aos::common::actions::TypedAction<AutonomousActionQueueGroup>;
+
+// Makes a new AutonomousActor action.
+::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
+ const AutonomousActionParams ¶ms);
+
+} // namespace autonomous
+} // namespace frc971
+
+#endif // FRC971_AUTONOMOUS_BASE_AUTONOMOUS_ACTOR_H_