Add auto superstructure helpers.
Change-Id: Ia1a5db649dbbe7f72f86e5d821d91e11814abb75
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index ccccdd7..2181333 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -2,15 +2,18 @@
#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/control_loops/drivetrain/drivetrain_base.h"
+#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/actors/autonomous_action.q.h"
namespace y2016 {
namespace actors {
-using ::frc971::ProfileParameters;
using ::frc971::control_loops::drivetrain_queue;
namespace {
@@ -23,9 +26,8 @@
AutonomousActor::AutonomousActor(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()) {}
+ dt_config_(control_loops::drivetrain::GetDrivetrainConfig()),
+ initial_drivetrain_({0.0, 0.0}) {}
void AutonomousActor::ResetDrivetrain() {
LOG(INFO, "resetting the drivetrain\n");
@@ -34,9 +36,9 @@
.highgear(true)
.steering(0.0)
.throttle(0.0)
- .left_goal(left_initial_position_)
+ .left_goal(initial_drivetrain_.left)
.left_velocity_goal(0)
- .right_goal(right_initial_position_)
+ .right_goal(initial_drivetrain_.right)
.right_velocity_goal(0)
.Send();
}
@@ -47,19 +49,18 @@
{
{
const double dangle = angle * dt_config_.robot_radius;
- left_initial_position_ += distance - dangle;
- right_initial_position_ += distance + dangle;
+ initial_drivetrain_.left += distance - dangle;
+ initial_drivetrain_.right += distance + dangle;
}
- auto drivetrain_message =
- drivetrain_queue.goal.MakeMessage();
+ 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_goal = initial_drivetrain_.left;
drivetrain_message->left_velocity_goal = 0;
- drivetrain_message->right_goal = right_initial_position_;
+ drivetrain_message->right_goal = initial_drivetrain_.right;
drivetrain_message->right_velocity_goal = 0;
drivetrain_message->linear = linear;
drivetrain_message->angular = angular;
@@ -72,8 +73,8 @@
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;
+ initial_drivetrain_.left = drivetrain_queue.status->estimated_left_position;
+ initial_drivetrain_.right = drivetrain_queue.status->estimated_right_position;
}
void AutonomousActor::WaitUntilDoneOrCanceled(
@@ -109,13 +110,13 @@
drivetrain_queue.status.FetchLatest();
if (drivetrain_queue.status.get()) {
if (::std::abs(drivetrain_queue.status->profiled_left_position_goal -
- left_initial_position_) < kProfileTolerance &&
+ initial_drivetrain_.left) < kProfileTolerance &&
::std::abs(drivetrain_queue.status->profiled_right_position_goal -
- right_initial_position_) < kProfileTolerance &&
+ initial_drivetrain_.right) < kProfileTolerance &&
::std::abs(drivetrain_queue.status->estimated_left_position -
- left_initial_position_) < kPositionTolerance &&
+ initial_drivetrain_.left) < kPositionTolerance &&
::std::abs(drivetrain_queue.status->estimated_right_position -
- right_initial_position_) < kPositionTolerance &&
+ initial_drivetrain_.right) < kPositionTolerance &&
::std::abs(drivetrain_queue.status->estimated_left_velocity) <
kVelocityTolerance &&
::std::abs(drivetrain_queue.status->estimated_right_velocity) <
@@ -127,6 +128,92 @@
}
}
+void AutonomousActor::MoveSuperstructure(
+ double intake, double shoulder, double wrist,
+ const ProfileParameters intake_params,
+ const ProfileParameters shoulder_params,
+ const ProfileParameters wrist_params, double top_rollers,
+ double bottom_rollers) {
+ superstructure_goal_ = {intake, shoulder, wrist};
+
+ auto new_superstructure_goal =
+ ::y2016::control_loops::superstructure_queue.goal.MakeMessage();
+
+ new_superstructure_goal->angle_intake = intake;
+ new_superstructure_goal->angle_shoulder = shoulder;
+ new_superstructure_goal->angle_wrist = wrist;
+
+ new_superstructure_goal->max_angular_velocity_intake =
+ intake_params.max_velocity;
+ new_superstructure_goal->max_angular_velocity_shoulder =
+ shoulder_params.max_velocity;
+ new_superstructure_goal->max_angular_velocity_wrist =
+ wrist_params.max_velocity;
+
+ new_superstructure_goal->max_angular_acceleration_intake =
+ intake_params.max_acceleration;
+ new_superstructure_goal->max_angular_acceleration_shoulder =
+ shoulder_params.max_acceleration;
+ new_superstructure_goal->max_angular_acceleration_wrist =
+ wrist_params.max_acceleration;
+
+ new_superstructure_goal->voltage_top_rollers = top_rollers;
+ new_superstructure_goal->voltage_bottom_rollers = bottom_rollers;
+
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+}
+
+void AutonomousActor::WaitForSuperstructure() {
+ while (true) {
+ if (ShouldCancel()) return;
+ control_loops::superstructure_queue.status.FetchAnother();
+
+ constexpr double kProfileError = 1e-5;
+ constexpr double kEpsilon = 0.03;
+
+ if (control_loops::superstructure_queue.status->state < 12 ||
+ control_loops::superstructure_queue.status->state == 16) {
+ LOG(ERROR, "Superstructure no longer running, aborting action\n");
+ return;
+ }
+
+ if (::std::abs(
+ control_loops::superstructure_queue.status->intake.goal_angle -
+ superstructure_goal_.intake) < kProfileError &&
+ ::std::abs(
+ control_loops::superstructure_queue.status->shoulder.goal_angle -
+ superstructure_goal_.shoulder) < kProfileError &&
+ ::std::abs(
+ control_loops::superstructure_queue.status->wrist.goal_angle -
+ superstructure_goal_.wrist) < kProfileError &&
+ ::std::abs(control_loops::superstructure_queue.status->intake
+ .goal_angular_velocity) < kProfileError &&
+ ::std::abs(control_loops::superstructure_queue.status->shoulder
+ .goal_angular_velocity) < kProfileError &&
+ ::std::abs(control_loops::superstructure_queue.status->wrist
+ .goal_angular_velocity) < kProfileError) {
+ LOG(INFO, "Profile done.\n");
+ if (::std::abs(control_loops::superstructure_queue.status->intake.angle -
+ superstructure_goal_.intake) < kEpsilon &&
+ ::std::abs(
+ control_loops::superstructure_queue.status->shoulder.angle -
+ superstructure_goal_.shoulder) < kEpsilon &&
+ ::std::abs(control_loops::superstructure_queue.status->wrist.angle -
+ superstructure_goal_.wrist) < kEpsilon &&
+ ::std::abs(control_loops::superstructure_queue.status->intake
+ .angular_velocity) < kEpsilon &&
+ ::std::abs(control_loops::superstructure_queue.status->shoulder
+ .angular_velocity) < kEpsilon &&
+ ::std::abs(control_loops::superstructure_queue.status->wrist
+ .angular_velocity) < kEpsilon) {
+ LOG(INFO, "Near goal, done.\n");
+ }
+ }
+ }
+}
+
bool AutonomousActor::RunAction(const actors::AutonomousActionParams ¶ms) {
LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
diff --git a/y2016/actors/autonomous_actor.h b/y2016/actors/autonomous_actor.h
index d7d7e65..4668c84 100644
--- a/y2016/actors/autonomous_actor.h
+++ b/y2016/actors/autonomous_actor.h
@@ -12,6 +12,7 @@
namespace y2016 {
namespace actors {
+using ::frc971::ProfileParameters;
class AutonomousActor
: public ::aos::common::actions::ActorBase<AutonomousActionQueueGroup> {
@@ -32,9 +33,30 @@
// false if it cancels.
bool WaitForDriveDone();
- double left_initial_position_, right_initial_position_;
-
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 {
+ double intake;
+ double shoulder;
+ double wrist;
+ };
+ SuperstructureGoal superstructure_goal_;
+
+ void MoveSuperstructure(double intake, double shoulder, double wrist,
+ const ProfileParameters intake_params,
+ const ProfileParameters shoulder_params,
+ const ProfileParameters wrist_params, double top_rollers,
+ double bottom_rollers);
+ void WaitForSuperstructure();
};
typedef ::aos::common::actions::TypedAction<AutonomousActionQueueGroup>