Wrote the 5 autonomous modes.
Change-Id: I46d3607811c78be14a3ca5f445899b15b0627fb5
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 8a0a7ff..2383c83 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -298,7 +298,7 @@
R_right(0, 0) = right_motor_speed;
const double wiggle =
- (static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
+ (static_cast<double>((counter_ % 30) / 15) - 0.5) * 8.0;
loop_->mutable_U(0, 0) = ::aos::Clip(
(R_left / dt_config_.v)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index 0c0a564..cee67c0 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -117,13 +117,16 @@
],
deps = [
':autonomous_action_queue',
+ ':vision_align_action_lib',
'//aos/common/util:phased_loop',
'//aos/common/logging',
'//aos/common/actions:action_lib',
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//y2016/control_loops/superstructure:superstructure_queue',
+ '//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/drivetrain:drivetrain_base',
'//y2016/queues:profile_params',
+ '//y2016/vision:vision_queue',
],
)
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();
}
diff --git a/y2016/actors/autonomous_actor.h b/y2016/actors/autonomous_actor.h
index 4668c84..15ed95d 100644
--- a/y2016/actors/autonomous_actor.h
+++ b/y2016/actors/autonomous_actor.h
@@ -7,6 +7,7 @@
#include "aos/common/actions/actions.h"
#include "y2016/actors/autonomous_action.q.h"
+#include "y2016/actors/vision_align_actor.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -33,6 +34,10 @@
// false if it cancels.
bool WaitForDriveDone();
+ // 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.
@@ -54,9 +59,32 @@
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);
+ const ProfileParameters wrist_params,
+ bool traverse_up);
void WaitForSuperstructure();
+
+ void BackLongShot();
+ void BackMiddleShot();
+ void TuckArm(bool arm_down, bool traverse_down);
+
+ void DoFullShot(bool center);
+ void LowBarDrive();
+ // Drive to the middle spot over the middle position. Designed for the rock
+ // wall, rough terain, or ramparts.
+ void MiddleDrive();
+
+ void OneFromMiddleDrive(bool left);
+ void TwoFromMiddleDrive();
+
+ double shooter_speed_ = 0.0;
+ void SetShooterSpeed(double speed);
+ void WaitForShooterSpeed();
+ void Shoot();
+
+ void AlignWithVisionGoal();
+ void WaitForAlignedWithVision();
+
+ ::std::unique_ptr<actors::VisionAlignAction> vision_action_;
};
typedef ::aos::common::actions::TypedAction<AutonomousActionQueueGroup>
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 32ccc7e..0936eea 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -70,6 +70,7 @@
Shooter::Shooter(ShooterQueue *my_shooter)
: aos::controls::ControlLoop<ShooterQueue>(my_shooter),
+ shots_(0),
last_pre_shot_timeout_(0, 0) {}
void Shooter::RunIteration(const ShooterQueue::Goal *goal,
@@ -127,20 +128,24 @@
}
if (::std::abs(goal->angular_velocity) < 10 ||
last_pre_shot_timeout_ < Time::Now()) {
- state_ = ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE;
+ state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
break;
case ShooterLatchState::WAITING_FOR_SPINUP:
shoot = true;
if (left_.velocity() > goal->angular_velocity * 0.95 &&
right_.velocity() > goal->angular_velocity * 0.95) {
- state_ = ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE;
+ state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
if (::std::abs(goal->angular_velocity) < 10 ||
last_pre_shot_timeout_ < Time::Now()) {
- state_ = ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE;
+ state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
break;
+ case ShooterLatchState::INCREMENT_SHOT_COUNT:
+ ++shots_;
+ state_ = ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE;
+ break;
case ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE:
shoot = true;
if (!goal->push_to_shooter) {
@@ -153,6 +158,8 @@
output->push_to_shooter = shoot;
}
}
+
+ status->shots = shots_;
}
} // namespace shooter
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
index d3a2f86..0b515f1 100644
--- a/y2016/control_loops/shooter/shooter.h
+++ b/y2016/control_loops/shooter/shooter.h
@@ -65,8 +65,10 @@
WAITING_FOR_SPINDOWN = 1,
// We are latched shooting waiting for the wheel to spin back up.
WAITING_FOR_SPINUP = 2,
+ // Increment the shot count for the Status.
+ INCREMENT_SHOT_COUNT = 3,
// Wait until the button is released.
- WAITING_FOR_SHOT_NEGEDGE = 3
+ WAITING_FOR_SHOT_NEGEDGE = 4,
};
protected:
@@ -78,6 +80,9 @@
private:
ShooterSide left_, right_;
+ // The number of shots since starting the Shooter.
+ uint32_t shots_;
+
// Current state.
ShooterLatchState state_ = ShooterLatchState::PASS_THROUGH;
::aos::time::Time last_pre_shot_timeout_;
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
index 6598319..b799b5a 100644
--- a/y2016/control_loops/shooter/shooter.q
+++ b/y2016/control_loops/shooter/shooter.q
@@ -48,6 +48,10 @@
// True if the shooter is ready. It is better to compare the velocities
// directly so there isn't confusion on if the goal is up to date.
bool ready;
+
+ // The number of shots that have been fired since the start of the shooter
+ // control loop.
+ uint32_t shots;
};
message Output {
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 97fe0dd..e5f0fff 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -46,7 +46,7 @@
// incorporating a small safety margin makes writing test cases much easier
// since you can directly compare statuses against the constants in the
// CollisionAvoidance class.
- constexpr double kSafetyMargin = 0.01; // radians
+ constexpr double kSafetyMargin = 0.02; // radians
// Avoid colliding the shooter with the frame.
// If the shoulder is below a certain angle or we want to move it below