Wrote the 5 autonomous modes.
Change-Id: I46d3607811c78be14a3ca5f445899b15b0627fb5
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 1000c3b..db881ff 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -9,19 +9,27 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#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/vision/vision.q.h"
namespace y2016 {
namespace actors {
using ::frc971::control_loops::drivetrain_queue;
namespace {
-const ProfileParameters kSlowDrive = {1.0, 1.0};
+const ProfileParameters kSlowDrive = {0.8, 2.5};
+const ProfileParameters kLowBarDrive = {1.3, 2.5};
+const ProfileParameters kMoatDrive = {1.2, 3.5};
+const ProfileParameters kRealignDrive = {2.0, 2.5};
+const ProfileParameters kRockWallDrive = {0.8, 2.5};
const ProfileParameters kFastDrive = {3.0, 2.5};
-const ProfileParameters kSlowTurn = {1.7, 3.0};
+const ProfileParameters kSlowTurn = {0.8, 3.0};
const ProfileParameters kFastTurn = {3.0, 10.0};
+
+const double kDistanceShort = 0.25;
} // namespace
AutonomousActor::AutonomousActor(actors::AutonomousActionQueueGroup *s)
@@ -95,6 +103,54 @@
}
}
+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::WaitForDriveDone() {
::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
::aos::time::Time::InMS(5) / 2);
@@ -132,8 +188,7 @@
double intake, double shoulder, double wrist,
const ProfileParameters intake_params,
const ProfileParameters shoulder_params,
- const ProfileParameters wrist_params, double top_rollers,
- double bottom_rollers) {
+ const ProfileParameters wrist_params, bool traverse_up) {
superstructure_goal_ = {intake, shoulder, wrist};
auto new_superstructure_goal =
@@ -157,14 +212,149 @@
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;
+ new_superstructure_goal->voltage_top_rollers = 0.0;
+ new_superstructure_goal->voltage_bottom_rollers = 0.0;
+
+ new_superstructure_goal->traverse_unlatched = true;
+ new_superstructure_goal->traverse_down = !traverse_up;
if (!new_superstructure_goal.Send()) {
LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
+void AutonomousActor::SetShooterSpeed(double speed) {
+ shooter_speed_ = speed;
+
+ // In auto, we want to have the lights on whenever possible since we have no
+ // hope of a human aligning the robot.
+ bool force_lights_on = shooter_speed_ > 1.0;
+
+ if (!control_loops::shooter::shooter_queue.goal.MakeWithBuilder()
+ .angular_velocity(shooter_speed_)
+ .clamp_open(false)
+ .push_to_shooter(false)
+ .force_lights_on(force_lights_on)
+ .Send()) {
+ LOG(ERROR, "Sending shooter goal failed.\n");
+ }
+}
+
+void AutonomousActor::Shoot() {
+ uint32_t initial_shots = 0;
+
+ control_loops::shooter::shooter_queue.status.FetchLatest();
+ if (control_loops::shooter::shooter_queue.status.get()) {
+ initial_shots = control_loops::shooter::shooter_queue.status->shots;
+ }
+
+ // In auto, we want to have the lights on whenever possible since we have no
+ // hope of a human aligning the robot.
+ bool force_lights_on = shooter_speed_ > 1.0;
+
+ if (!control_loops::shooter::shooter_queue.goal.MakeWithBuilder()
+ .angular_velocity(shooter_speed_)
+ .clamp_open(false)
+ .push_to_shooter(true)
+ .force_lights_on(force_lights_on)
+ .Send()) {
+ LOG(ERROR, "Sending shooter goal failed.\n");
+ }
+
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ while (true) {
+ if (ShouldCancel()) return;
+
+ // Wait for the shot count to change so we know when the shot is complete.
+ control_loops::shooter::shooter_queue.status.FetchLatest();
+ if (control_loops::shooter::shooter_queue.status.get()) {
+ if (initial_shots < control_loops::shooter::shooter_queue.status->shots) {
+ return;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+}
+
+void AutonomousActor::WaitForShooterSpeed() {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ while (true) {
+ if (ShouldCancel()) return;
+
+ control_loops::shooter::shooter_queue.status.FetchLatest();
+ if (control_loops::shooter::shooter_queue.status.get()) {
+ if (control_loops::shooter::shooter_queue.status->left.ready &&
+ control_loops::shooter::shooter_queue.status->right.ready) {
+ return;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+}
+
+void AutonomousActor::AlignWithVisionGoal() {
+ actors::VisionAlignActionParams params;
+ vision_action_ = ::std::move(actors::MakeVisionAlignAction(params));
+ vision_action_->Start();
+}
+
+void AutonomousActor::WaitForAlignedWithVision() {
+ bool vision_valid = false;
+ double last_angle = 0.0;
+ int ready_to_fire = 0;
+
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ ::aos::time::Time end_time =
+ ::aos::time::Time::Now() + aos::time::Time::InSeconds(3);
+ while (end_time > ::aos::time::Time::Now()) {
+ if (ShouldCancel()) break;
+
+ ::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);
+ last_angle = ::y2016::vision::vision_status->horizontal_angle;
+ }
+
+ drivetrain_queue.status.FetchLatest();
+ drivetrain_queue.goal.FetchLatest();
+
+ if (drivetrain_queue.status.get() && drivetrain_queue.goal.get()) {
+ const double left_goal = drivetrain_queue.goal->left_goal;
+ const double right_goal = drivetrain_queue.goal->right_goal;
+ const double left_current =
+ drivetrain_queue.status->estimated_left_position;
+ const double right_current =
+ drivetrain_queue.status->estimated_right_position;
+ const double left_velocity =
+ drivetrain_queue.status->estimated_left_velocity;
+ const double right_velocity =
+ drivetrain_queue.status->estimated_right_velocity;
+
+ if (vision_valid && ::std::abs(last_angle) < 0.02 &&
+ ::std::abs((left_goal - right_goal) -
+ (left_current - right_current)) /
+ dt_config_.robot_radius / 2.0 <
+ 0.02 &&
+ ::std::abs(left_velocity - right_velocity) < 0.01) {
+ ++ready_to_fire;
+ } else {
+ ready_to_fire = 0;
+ }
+ if (ready_to_fire > 9) {
+ break;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+
+ vision_action_->Cancel();
+ WaitUntilDoneOrCanceled(::std::move(vision_action_));
+}
+
void AutonomousActor::WaitForSuperstructure() {
while (true) {
if (ShouldCancel()) return;
@@ -214,6 +404,126 @@
}
}
}
+void AutonomousActor::BackLongShot() {
+ LOG(INFO, "Expanding for back long shot\n");
+ MoveSuperstructure(-0.05, M_PI / 2.0 - 0.2, -0.62, {7.0, 40.0}, {4.0, 10.0},
+ {10.0, 25.0}, false);
+}
+
+void AutonomousActor::BackMiddleShot() {
+ LOG(INFO, "Expanding for back middle shot\n");
+ MoveSuperstructure(-0.05, M_PI / 2.0 - 0.2, -0.665, {7.0, 40.0}, {4.0, 10.0},
+ {10.0, 25.0}, false);
+}
+
+void AutonomousActor::TuckArm(bool low_bar, bool traverse_down) {
+ MoveSuperstructure(low_bar ? -0.05 : 2.0, -0.010, 0.0, {7.0, 40.0},
+ {4.0, 10.0}, {10.0, 25.0}, !traverse_down);
+}
+
+void AutonomousActor::DoFullShot(bool center) {
+ // Get the superstructure to unfold and get ready for shooting.
+ LOG(INFO, "Unfolding superstructure\n");
+ if (center) {
+ BackMiddleShot();
+ } else {
+ BackLongShot();
+ }
+
+ // Spin up the shooter wheels.
+ LOG(INFO, "Spinning up the shooter wheels\n");
+ SetShooterSpeed(640.0);
+
+ if (ShouldCancel()) return;
+ // Make sure that the base is aligned with the base.
+ LOG(INFO, "Waiting for the superstructure\n");
+ WaitForSuperstructure();
+ if (ShouldCancel()) return;
+ LOG(INFO, "Triggering the vision actor\n");
+ AlignWithVisionGoal();
+
+ // Wait for the drive base to be aligned with the target and make sure that
+ // the shooter is up to speed.
+ LOG(INFO, "Waiting for vision to be aligned\n");
+ WaitForAlignedWithVision();
+ if (ShouldCancel()) return;
+ LOG(INFO, "Waiting for shooter to be up to speed\n");
+ WaitForShooterSpeed();
+ if (ShouldCancel()) return;
+
+ LOG(INFO, "Shoot!\n");
+ Shoot();
+
+ // Turn off the shooter and fold up the superstructure.
+ if (ShouldCancel()) return;
+ LOG(INFO, "Stopping shooter\n");
+ SetShooterSpeed(0.0);
+ LOG(INFO, "Folding superstructure back down\n");
+ TuckArm(false, false);
+
+ // Wait for everything to be folded up.
+ LOG(INFO, "Waiting for superstructure to be folded back down\n");
+ WaitForSuperstructure();
+}
+
+void AutonomousActor::LowBarDrive() {
+ TuckArm(false, true);
+ StartDrive(-5.5, 0.0, kLowBarDrive, kSlowTurn);
+
+ if (!WaitForDriveNear(5.3, 0.0)) return;
+ TuckArm(true, true);
+
+ if (!WaitForDriveNear(5.0, 0.0)) return;
+
+ StartDrive(0.0, 0.0, kLowBarDrive, kSlowTurn);
+
+ if (!WaitForDriveNear(3.0, 0.0)) return;
+
+ StartDrive(0.0, 0.0, kLowBarDrive, kSlowTurn);
+
+ if (!WaitForDriveNear(1.0, 0.0)) return;
+
+ StartDrive(0, -M_PI / 4.0 - 0.1, kLowBarDrive, kSlowTurn);
+}
+
+void AutonomousActor::MiddleDrive() {
+ TuckArm(false, false);
+ StartDrive(4.7, 0.0, kMoatDrive, kSlowTurn);
+ if (!WaitForDriveDone()) return;
+ StartDrive(0.0, M_PI, kMoatDrive, kFastTurn);
+}
+
+void AutonomousActor::OneFromMiddleDrive(bool left) {
+ const double kTurnAngle = left ? M_PI / 2.0 - 0.40 : (-M_PI / 2.0 + 0.40);
+ const double kFlipTurnAngle =
+ left ? (M_PI - kTurnAngle) : (-M_PI - kTurnAngle);
+ TuckArm(false, false);
+ StartDrive(5.0 - kDistanceShort, 0.0, kMoatDrive, kFastTurn);
+ if (!WaitForDriveDone()) return;
+ StartDrive(0.0, kTurnAngle, kRealignDrive, kFastTurn);
+ if (!WaitForDriveDone()) return;
+ StartDrive(-1.3, 0.0, kRealignDrive, kFastTurn);
+ if (!WaitForDriveDone()) return;
+ StartDrive(0.0, kFlipTurnAngle, kRealignDrive, kFastTurn);
+
+ if (!WaitForDriveDone()) return;
+ StartDrive(0.3, 0.0, kRealignDrive, kFastTurn);
+}
+
+void AutonomousActor::TwoFromMiddleDrive() {
+ const double kTurnAngle = M_PI / 2.0 - 0.13;
+ TuckArm(false, false);
+ StartDrive(5.0 - kDistanceShort, 0.0, kMoatDrive, kFastTurn);
+ if (!WaitForDriveDone()) return;
+ StartDrive(0.0, kTurnAngle, kMoatDrive, kFastTurn);
+ if (!WaitForDriveDone()) return;
+ StartDrive(-2.6, 0.0, kRealignDrive, kFastTurn);
+ if (!WaitForDriveDone()) return;
+ StartDrive(0.0, M_PI - kTurnAngle, kMoatDrive, kFastTurn);
+
+ if (!WaitForDriveDone()) return;
+ StartDrive(0.3, 0.0, kRealignDrive, kFastTurn);
+}
bool AutonomousActor::RunAction(const actors::AutonomousActionParams ¶ms) {
LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
@@ -221,16 +531,35 @@
InitializeEncoders();
ResetDrivetrain();
- StartDrive(1.0, 0.0, kSlowDrive, kSlowTurn);
+ int index = 0;
+ switch (index) {
+ case 0:
+ LowBarDrive();
+ break;
+ case 1:
+ TwoFromMiddleDrive();
+ break;
+ case 2:
+ OneFromMiddleDrive(true);
+ break;
+ case 3:
+ MiddleDrive();
+ break;
+ case 4:
+ OneFromMiddleDrive(false);
+ break;
+ default:
+ LOG(ERROR, "Invalid auto index %d\n", index);
+ return true;
+ }
if (!WaitForDriveDone()) return true;
- StartDrive(0.0, M_PI, kSlowDrive, kSlowTurn);
-
- if (!WaitForDriveDone()) return true;
+ DoFullShot(index != 0);
::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
::aos::time::Time::InMS(5) / 2);
+
while (!ShouldCancel()) {
phased_loop.SleepUntilNext();
}