Merge changes I04c06100,I7f88d30c,I1258a5ef
* changes:
Add a test autonomous as mode 0
Move y2017 auton actor to use common class
Factor out drivetrain functionality from y2016 auton actor
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_
diff --git a/y2016/BUILD b/y2016/BUILD
index f91ba02..d3c4a9e 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -112,6 +112,7 @@
'//aos/common/util:wrapping_counter',
'//aos/linux_code:init',
'//third_party:wpilib',
+ '//frc971/autonomous:auto_queue',
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/control_loops:queues',
'//frc971/wpilib:joystick_sender',
@@ -131,6 +132,5 @@
'//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/superstructure:superstructure_queue',
'//y2016/queues:ball_detector',
- '//y2016/actors:autonomous_action_queue',
],
)
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index cfbeb2b..6e7db6e 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -49,16 +49,6 @@
],
)
-queue_library(
- name = 'autonomous_action_queue',
- srcs = [
- 'autonomous_action.q',
- ],
- deps = [
- '//aos/common/actions:action_queue',
- ],
-)
-
cc_library(
name = 'autonomous_action_lib',
srcs = [
@@ -68,11 +58,11 @@
'autonomous_actor.h',
],
deps = [
- ':autonomous_action_queue',
':vision_align_action_lib',
'//aos/common/util:phased_loop',
'//aos/common/logging',
'//aos/common/actions:action_lib',
+ '//frc971/autonomous:base_autonomous_actor',
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//y2016/queues:ball_detector',
'//y2016/control_loops/superstructure:superstructure_queue',
@@ -90,7 +80,6 @@
],
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
deleted file mode 100644
index 5f87a8a..0000000
--- a/y2016/actors/autonomous_action.q
+++ /dev/null
@@ -1,29 +0,0 @@
-package y2016.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/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 5194b2d..4646cde 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -12,7 +12,6 @@
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/actors/autonomous_action.q.h"
#include "y2016/queues/ball_detector.q.h"
#include "y2016/vision/vision.q.h"
@@ -50,256 +49,13 @@
}
} // 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)
- .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 AutonomousActor::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 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(::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;
- }
- }
-}
+AutonomousActor::AutonomousActor(
+ ::frc971::autonomous::AutonomousActionQueueGroup *s)
+ : frc971::autonomous::BaseAutonomousActor(
+ s, control_loops::drivetrain::GetDrivetrainConfig()) {}
constexpr double kDoNotTurnCare = 2.0;
-bool AutonomousActor::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 AutonomousActor::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;
- }
- }
- }
-}
-
-bool AutonomousActor::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 AutonomousActor::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 AutonomousActor::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 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(::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;
- }
- }
-}
-
void AutonomousActor::MoveSuperstructure(
double intake, double shoulder, double wrist,
const ProfileParameters intake_params,
@@ -461,7 +217,7 @@
::y2016::vision::vision_status.FetchLatest();
if (::y2016::vision::vision_status.get()) {
vision_valid = (::y2016::vision::vision_status->left_image_valid &&
- ::y2016::vision::vision_status->right_image_valid);
+ ::y2016::vision::vision_status->right_image_valid);
last_angle = ::y2016::vision::vision_status->horizontal_angle;
}
@@ -641,10 +397,8 @@
void AutonomousActor::BackLongShotTwoBallFinish() {
LOG(INFO, "Expanding for back long shot\n");
- MoveSuperstructure(
- 0.00, M_PI / 2.0 - 0.2, -0.625 + 0.03, {7.0, 40.0},
- {4.0, 6.0},
- {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.625 + 0.03, {7.0, 40.0},
+ {4.0, 6.0}, {10.0, 25.0}, false, 0.0);
}
void AutonomousActor::BackLongShot() {
@@ -1033,7 +787,8 @@
if (!WaitForDriveDone()) return;
}
-bool AutonomousActor::RunAction(const actors::AutonomousActionParams ¶ms) {
+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);
@@ -1189,11 +944,5 @@
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
index 7cf932a..24df091 100644
--- a/y2016/actors/autonomous_actor.h
+++ b/y2016/actors/autonomous_actor.h
@@ -6,58 +6,27 @@
#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 "y2016/actors/autonomous_action.q.h"
#include "y2016/actors/vision_align_actor.h"
namespace y2016 {
namespace actors {
using ::frc971::ProfileParameters;
-class AutonomousActor
- : public ::aos::common::actions::ActorBase<AutonomousActionQueueGroup> {
+class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
- explicit AutonomousActor(AutonomousActionQueueGroup *s);
+ explicit AutonomousActor(::frc971::autonomous::AutonomousActionQueueGroup *s);
- bool RunAction(const actors::AutonomousActionParams ¶ms) override;
+ bool RunAction(
+ const ::frc971::autonomous::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,
- ::frc971::ProfileParameters angular);
- // 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 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);
-
- 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 SuperstructureGoal {
@@ -77,7 +46,6 @@
void WaitForSuperstructureLow();
void WaitForIntake();
bool IntakeDone();
- bool WaitForDriveProfileDone();
void FrontLongShot();
void FrontMiddleShot();
@@ -118,13 +86,6 @@
::std::unique_ptr<actors::VisionAlignAction> vision_action_;
};
-typedef ::aos::common::actions::TypedAction<AutonomousActionQueueGroup>
- AutonomousAction;
-
-// Makes a new AutonomousActor action.
-::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
- const ::y2016::actors::AutonomousActionParams ¶ms);
-
} // namespace actors
} // namespace y2016
diff --git a/y2016/actors/autonomous_actor_main.cc b/y2016/actors/autonomous_actor_main.cc
index 6f01e65..f5520bc 100644
--- a/y2016/actors/autonomous_actor_main.cc
+++ b/y2016/actors/autonomous_actor_main.cc
@@ -1,14 +1,13 @@
#include <stdio.h>
#include "aos/linux_code/init.h"
-#include "y2016/actors/autonomous_action.q.h"
#include "y2016/actors/autonomous_actor.h"
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
::y2016::actors::AutonomousActor autonomous(
- &::y2016::actors::autonomous_action);
+ &::frc971::autonomous::autonomous_action);
autonomous.Run();
::aos::Cleanup();
diff --git a/y2016/dashboard/BUILD b/y2016/dashboard/BUILD
index fd2fdbb..ce3df07 100644
--- a/y2016/dashboard/BUILD
+++ b/y2016/dashboard/BUILD
@@ -28,8 +28,8 @@
'//aos/common/logging',
'//aos/common/util:phased_loop',
'//aos/common:time',
+ '//frc971/autonomous:auto_queue',
'//third_party/seasocks',
- '//y2016/actors:autonomous_action_queue',
'//y2016/vision:vision_queue',
'//y2016/control_loops/superstructure:superstructure_queue',
'//y2016/queues:ball_detector',
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index 798c08b..4b4c0e9 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -16,7 +16,8 @@
#include "aos/common/util/phased_loop.h"
#include "aos/common/mutex.h"
-#include "y2016/actors/autonomous_action.q.h"
+#include "frc971/autonomous/auto.q.h"
+
#include "y2016/vision/vision.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
@@ -80,7 +81,7 @@
// gone wrong with reading the auto queue.
int auto_mode_indicator = -1;
- ::y2016::actors::auto_mode.FetchLatest();
+ ::frc971::autonomous::auto_mode.FetchLatest();
::y2016::control_loops::superstructure_queue.status.FetchLatest();
::y2016::sensors::ball_detector.FetchLatest();
::y2016::vision::vision_status.FetchLatest();
@@ -121,8 +122,8 @@
}
}
- if (::y2016::actors::auto_mode.get()) {
- auto_mode_indicator = ::y2016::actors::auto_mode->mode;
+ if (::frc971::autonomous::auto_mode.get()) {
+ auto_mode_indicator = ::frc971::autonomous::auto_mode->mode;
}
AddPoint("big indicator", big_indicator);
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index af01a8b..addfa7f 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -430,15 +430,16 @@
void StartAuto() {
LOG(INFO, "Starting auto mode\n");
- actors::AutonomousActionParams params;
- actors::auto_mode.FetchLatest();
- if (actors::auto_mode.get() != nullptr) {
- params.mode = actors::auto_mode->mode;
+ ::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(actors::MakeAutonomousAction(params));
+ action_queue_.EnqueueAction(
+ ::frc971::autonomous::MakeAutonomousAction(params));
}
void StopAuto() {
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index c998a3a..5e98b52 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -32,6 +32,7 @@
#include "aos/common/messages/robot_state.q.h"
#include "aos/common/commonmath.h"
+#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/control_loops/shooter/shooter.q.h"
@@ -40,7 +41,6 @@
#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
-#include "y2016/actors/autonomous_action.q.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -358,7 +358,7 @@
}
{
- auto auto_mode_message = ::y2016::actors::auto_mode.MakeMessage();
+ auto auto_mode_message = ::frc971::autonomous::auto_mode.MakeMessage();
auto_mode_message->mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (autonomous_modes_[i]->Get()) {
diff --git a/y2017/BUILD b/y2017/BUILD
index 4e66d6f..a7e08ba 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -62,6 +62,7 @@
'//aos/common/util:wrapping_counter',
'//aos/linux_code:init',
'//third_party:wpilib',
+ '//frc971/autonomous:auto_queue',
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/control_loops:queues',
'//frc971/wpilib:joystick_sender',
@@ -77,7 +78,6 @@
'//frc971/wpilib:ADIS16448',
'//frc971/wpilib:dma',
'//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017/actors:autonomous_action_queue',
],
)
diff --git a/y2017/actors/BUILD b/y2017/actors/BUILD
index 6766422..b1cacc9 100644
--- a/y2017/actors/BUILD
+++ b/y2017/actors/BUILD
@@ -9,16 +9,6 @@
],
)
-queue_library(
- name = 'autonomous_action_queue',
- srcs = [
- 'autonomous_action.q',
- ],
- deps = [
- '//aos/common/actions:action_queue',
- ],
-)
-
cc_library(
name = 'autonomous_action_lib',
srcs = [
@@ -28,12 +18,13 @@
'autonomous_actor.h',
],
deps = [
- ':autonomous_action_queue',
'//aos/common/util:phased_loop',
'//aos/common/logging',
'//aos/common/actions:action_lib',
+ '//frc971/autonomous:base_autonomous_actor',
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/control_loops/drivetrain:drivetrain_config',
+ '//y2017/control_loops/drivetrain:drivetrain_base',
],
)
@@ -44,7 +35,7 @@
],
deps = [
':autonomous_action_lib',
- ':autonomous_action_queue',
'//aos/linux_code:init',
+ '//frc971/autonomous:auto_queue',
],
-)
\ No newline at end of file
+)
diff --git a/y2017/actors/autonomous_action.q b/y2017/actors/autonomous_action.q
deleted file mode 100644
index 52873ca..0000000
--- a/y2017/actors/autonomous_action.q
+++ /dev/null
@@ -1,29 +0,0 @@
-package y2017.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/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index ad4b87c..9dd93d6 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -9,7 +9,7 @@
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2017/actors/autonomous_action.q.h"
+#include "y2017/control_loops/drivetrain/drivetrain_base.h"
namespace y2017 {
namespace actors {
@@ -19,39 +19,35 @@
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 kSlowDrive = {0.8, 2.5};
+const ProfileParameters kSlowTurn = {0.8, 3.0};
+
} // namespace
-AutonomousActor::AutonomousActor(actors::AutonomousActionQueueGroup *s)
- : aos::common::actions::ActorBase<actors::AutonomousActionQueueGroup>(s) {}
+AutonomousActor::AutonomousActor(
+ ::frc971::autonomous::AutonomousActionQueueGroup *s)
+ : frc971::autonomous::BaseAutonomousActor(
+ s, control_loops::drivetrain::GetDrivetrainConfig()) {}
-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(::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 AutonomousActor::RunAction(const actors::AutonomousActionParams ¶ms) {
+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);
switch (params.mode) {
case 0:
+ // Test case autonomous mode.
+ // Drives forward 1.0 meters and then turns 180 degrees.
+ StartDrive(1.1, 0.0, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveNear(1.0, 0.0)) return true;
+ StartDrive(0.0, M_PI / 2, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveDone()) return true;
break;
default:
@@ -72,11 +68,5 @@
return true;
}
-::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
- const ::y2017::actors::AutonomousActionParams ¶ms) {
- return ::std::unique_ptr<AutonomousAction>(
- new AutonomousAction(&::y2017::actors::autonomous_action, params));
-}
-
} // namespace actors
} // namespace y2017
diff --git a/y2017/actors/autonomous_actor.h b/y2017/actors/autonomous_actor.h
index ef63b8e..3c179ed 100644
--- a/y2017/actors/autonomous_actor.h
+++ b/y2017/actors/autonomous_actor.h
@@ -6,32 +6,24 @@
#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 "y2017/actors/autonomous_action.q.h"
namespace y2017 {
namespace actors {
using ::frc971::ProfileParameters;
-class AutonomousActor
- : public ::aos::common::actions::ActorBase<AutonomousActionQueueGroup> {
+class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
- explicit AutonomousActor(AutonomousActionQueueGroup *s);
+ explicit AutonomousActor(::frc971::autonomous::AutonomousActionQueueGroup *s);
- bool RunAction(const actors::AutonomousActionParams ¶ms) override;
+ bool RunAction(
+ const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
private:
- void WaitUntilDoneOrCanceled(::std::unique_ptr<aos::common::actions::Action>
- action);
+ // TODO(phil): Implement this.
};
-typedef ::aos::common::actions::TypedAction<AutonomousActionQueueGroup>
- AutonomousAction;
-
-// Makes a new AutonomousActor action.
-::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
- const ::y2017::actors::AutonomousActionParams ¶ms);
-
} // namespace actors
} // namespace y2017
diff --git a/y2017/actors/autonomous_actor_main.cc b/y2017/actors/autonomous_actor_main.cc
index 21c7e7a..eb18c58 100644
--- a/y2017/actors/autonomous_actor_main.cc
+++ b/y2017/actors/autonomous_actor_main.cc
@@ -1,14 +1,14 @@
#include <stdio.h>
#include "aos/linux_code/init.h"
-#include "y2017/actors/autonomous_action.q.h"
+#include "frc971/autonomous/auto.q.h"
#include "y2017/actors/autonomous_actor.h"
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
::y2017::actors::AutonomousActor autonomous(
- &::y2017::actors::autonomous_action);
+ &::frc971::autonomous::autonomous_action);
autonomous.Run();
::aos::Cleanup();
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index abddd86..1343640 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -31,11 +31,11 @@
#include "aos/common/messages/robot_state.q.h"
#include "aos/common/commonmath.h"
+#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
-#include "y2017/actors/autonomous_action.q.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "frc971/wpilib/joystick_sender.h"
@@ -353,7 +353,7 @@
}
{
- auto auto_mode_message = ::y2017::actors::auto_mode.MakeMessage();
+ auto auto_mode_message = ::frc971::autonomous::auto_mode.MakeMessage();
auto_mode_message->mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (autonomous_modes_[i]->Get()) {