Added 3 can auto.
Change-Id: Id81eafb797708d44c95b754947badc46fb552421
diff --git a/bot3/autonomous/auto.cc b/bot3/autonomous/auto.cc
index 813b1ec..e76d8aa 100644
--- a/bot3/autonomous/auto.cc
+++ b/bot3/autonomous/auto.cc
@@ -9,6 +9,7 @@
#include "aos/common/logging/queue_logging.h"
#include "bot3/autonomous/auto.q.h"
+#include "bot3/actors/drivetrain_actor.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.h"
#include "bot3/control_loops/elevator/elevator.q.h"
@@ -18,6 +19,7 @@
using ::bot3::control_loops::drivetrain_queue;
using ::bot3::control_loops::intake_queue;
using ::bot3::control_loops::elevator_queue;
+using ::bot3::control_loops::kDrivetrainTurnWidth;
namespace bot3 {
namespace autonomous {
@@ -28,6 +30,15 @@
};
namespace time = ::aos::time;
+namespace actors = ::frc971::actors;
+
+const ProfileParams kFastDrive = {3.0, 3.5};
+const ProfileParams kLastDrive = {4.0, 4.0};
+const ProfileParams kStackingFirstTurn = {3.0, 7.0};
+const ProfileParams kFinalDriveTurn = {3.0, 5.0};
+const ProfileParams kSlowFirstDriveTurn = {0.75, 1.5};
+const ProfileParams kSlowSecondDriveTurn = {0.6, 1.5};
+const ProfileParams kCanPickupDrive = {1.3, 3.0};
static double left_initial_position, right_initial_position;
@@ -61,6 +72,60 @@
control_loops::drivetrain_queue.status->filtered_right_position;
}
+::std::unique_ptr< ::frc971::actors::DrivetrainAction> SetDriveGoal(
+ double distance, const ProfileParams drive_params, double theta,
+ const ProfileParams &turn_params) {
+ LOG(INFO, "Driving to %f\n", distance);
+
+ actors::DrivetrainActionParams params;
+ params.left_initial_position = left_initial_position;
+ params.right_initial_position = right_initial_position;
+ params.y_offset = distance;
+ params.theta_offset = theta;
+ params.maximum_turn_acceleration = turn_params.acceleration;
+ params.maximum_turn_velocity = turn_params.velocity;
+ params.maximum_velocity = drive_params.velocity;
+ params.maximum_acceleration = drive_params.acceleration;
+ auto drivetrain_action = actors::MakeDrivetrainAction(params);
+
+ drivetrain_action->Start();
+ left_initial_position += distance - theta * kDrivetrainTurnWidth / 2.0;
+ right_initial_position += distance + theta * kDrivetrainTurnWidth / 2.0;
+ return ::std::move(drivetrain_action);
+}
+void WaitUntilDoneOrCanceled(
+ ::std::unique_ptr<aos::common::actions::Action> action) {
+ if (!action) {
+ LOG(ERROR, "No action, not waiting\n");
+ return;
+ }
+ while (true) {
+ // Poll the running bit and auto done bits.
+ ::aos::time::PhasedLoopXMS(5, 2500);
+ if (!action->Running() || ShouldExitAuto()) {
+ return;
+ }
+ }
+}
+
+void WaitUntilNear(double distance) {
+ while (true) {
+ if (ShouldExitAuto()) return;
+ control_loops::drivetrain_queue.status.FetchAnother();
+ double left_error = ::std::abs(
+ left_initial_position -
+ control_loops::drivetrain_queue.status->filtered_left_position);
+ double right_error = ::std::abs(
+ right_initial_position -
+ control_loops::drivetrain_queue.status->filtered_right_position);
+ const double kPositionThreshold = 0.05 + distance;
+ if (right_error < kPositionThreshold && left_error < kPositionThreshold) {
+ LOG(INFO, "At the goal\n");
+ return;
+ }
+ }
+}
+
void GrabberForTime(double voltage, double wait_time) {
::aos::time::Time now = ::aos::time::Time::Now();
::aos::time::Time end_time = now + time::Time::InSeconds(wait_time);
@@ -141,12 +206,250 @@
GrabberForTime(-3.0, 12.0);
}
+const ProfileParams kElevatorProfile = {1.0, 5.0};
+
+static double elevator_goal_height = 0.0;
+
+void SetElevatorHeight(double height, const ProfileParams params,
+ bool can_open = false, bool support_open = true) {
+ // Send our elevator goals, with limits set in the profile params.
+ auto new_elevator_goal = elevator_queue.goal.MakeMessage();
+ elevator_goal_height = height;
+ new_elevator_goal->max_velocity = params.velocity;
+ new_elevator_goal->max_acceleration = params.acceleration;
+ new_elevator_goal->height = elevator_goal_height;
+ new_elevator_goal->velocity = 0.0;
+ new_elevator_goal->passive_support = support_open;
+ new_elevator_goal->can_support = can_open;
+
+ if (new_elevator_goal.Send()) {
+ LOG(DEBUG, "sending goals: elevator: %f\n", elevator_goal_height);
+ } else {
+ LOG(ERROR, "Sending elevator goal failed.\n");
+ }
+}
+
+void WaitForElevator() {
+ while (true) {
+ if (ShouldExitAuto()) return;
+ control_loops::elevator_queue.status.FetchAnother();
+
+ constexpr double kProfileError = 1e-5;
+ constexpr double kEpsilon = 0.03;
+
+ if (::std::abs(control_loops::elevator_queue.status->goal_height -
+ elevator_goal_height) <
+ kProfileError &&
+ ::std::abs(control_loops::elevator_queue.status->goal_velocity) <
+ kProfileError) {
+ LOG(INFO, "Profile done.\n");
+ if (::std::abs(control_loops::elevator_queue.status->height -
+ elevator_goal_height) <
+ kEpsilon) {
+ LOG(INFO, "Near goal, done.\n");
+ return;
+ }
+ }
+ }
+}
+
+void WaitUntilElevatorBelow(double height) {
+ while (true) {
+ if (ShouldExitAuto()) return;
+ control_loops::elevator_queue.status.FetchAnother();
+
+ if (control_loops::elevator_queue.status->goal_height < height) {
+ LOG(INFO, "Profile below.\n");
+ if (control_loops::elevator_queue.status->height < height) {
+ LOG(INFO, "Elevator below goal, done.\n");
+ return;
+ }
+ }
+ }
+}
+
+void WaitUntilElevatorAbove(double height) {
+ while (true) {
+ if (ShouldExitAuto()) return;
+ control_loops::elevator_queue.status.FetchAnother();
+
+ if (control_loops::elevator_queue.status->goal_height > height) {
+ LOG(INFO, "Profile above.\n");
+ if (control_loops::elevator_queue.status->height > height) {
+ LOG(INFO, "Elevator above goal, done.\n");
+ return;
+ }
+ }
+ }
+}
+
+void LiftTote(double final_height = 0.48) {
+ // Send our intake goals.
+ if (!intake_queue.goal.MakeWithBuilder().movement(10.0).claw_closed(true)
+ .Send()) {
+ LOG(ERROR, "Sending intake goal failed.\n");
+ }
+
+ while (true) {
+ elevator_queue.status.FetchAnother();
+ if (!elevator_queue.status.get()) {
+ LOG(ERROR, "Got no elevator status packet.\n");
+ }
+ if (ShouldExitAuto()) return;
+ if (elevator_queue.status->has_tote) {
+ break;
+ }
+ }
+ if (!intake_queue.goal.MakeWithBuilder().movement(0.0).claw_closed(true)
+ .Send()) {
+ LOG(ERROR, "Sending intake goal failed.\n");
+ }
+ SetElevatorHeight(0.02, kElevatorProfile, false, false);
+ WaitUntilElevatorBelow(0.05);
+ if (ShouldExitAuto()) return;
+
+ SetElevatorHeight(final_height, kElevatorProfile, false, false);
+}
+
+void TripleCanAuto() {
+ ::aos::time::Time start_time = ::aos::time::Time::Now();
+
+ ::std::unique_ptr<::frc971::actors::DrivetrainAction> drive;
+ InitializeEncoders();
+ ResetDrivetrain();
+
+ SetElevatorHeight(0.03, kElevatorProfile, false, true);
+
+
+ LiftTote();
+ if (ShouldExitAuto()) return;
+
+ // The amount to turn out for going around the first can.
+ const double kFirstTurn = 0.5;
+
+ drive = SetDriveGoal(0.0, kFastDrive, kFirstTurn, kStackingFirstTurn);
+
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ drive = SetDriveGoal(2.0, kFastDrive, -kFirstTurn * 2.0, kSlowFirstDriveTurn);
+
+ WaitUntilNear(1.5);
+ if (ShouldExitAuto()) return;
+
+ if (!intake_queue.goal.MakeWithBuilder().movement(0.0).claw_closed(false)
+ .Send()) {
+ LOG(ERROR, "Sending intake goal failed.\n");
+ }
+
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ drive = SetDriveGoal(0.0, kFastDrive, kFirstTurn, kStackingFirstTurn);
+ LiftTote();
+ if (ShouldExitAuto()) return;
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ // The amount to turn out for going around the second can.
+ const double kSecondTurn = 0.35;
+ const double kSecondTurnExtraOnSecond = 0.10;
+
+ drive = SetDriveGoal(0.0, kFastDrive, kSecondTurn, kStackingFirstTurn);
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ drive =
+ SetDriveGoal(2.05, kFastDrive, -kSecondTurn * 2.0 - kSecondTurnExtraOnSecond, kSlowSecondDriveTurn);
+ WaitUntilNear(1.5);
+
+ if (!intake_queue.goal.MakeWithBuilder().movement(0.0).claw_closed(false)
+ .Send()) {
+ LOG(ERROR, "Sending intake goal failed.\n");
+ }
+
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ drive = SetDriveGoal(0.0, kFastDrive, kSecondTurn + kSecondTurnExtraOnSecond, kStackingFirstTurn);
+
+ LiftTote(0.18);
+
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ drive = SetDriveGoal(0.3, kFastDrive, -1.7, kFinalDriveTurn);
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ if (!intake_queue.goal.MakeWithBuilder().movement(0.0).claw_closed(false)
+ .Send()) {
+ LOG(ERROR, "Sending intake goal failed.\n");
+ }
+
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+ drive = SetDriveGoal(2.95, kLastDrive, 0.0, kFinalDriveTurn);
+
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ SetElevatorHeight(0.03, kElevatorProfile, true, true);
+ WaitForElevator();
+ if (ShouldExitAuto()) return;
+
+ drive = SetDriveGoal(-2.25, kFastDrive, 0.0, kFinalDriveTurn);
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ drive = SetDriveGoal(0.0, kFastDrive, 1.80, kFinalDriveTurn);
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ if (!intake_queue.goal.MakeWithBuilder().movement(10.0).claw_closed(true)
+ .Send()) {
+ LOG(ERROR, "Sending intake goal failed.\n");
+ }
+ SetElevatorHeight(0.03, kElevatorProfile, false, true);
+
+ drive = SetDriveGoal(1.0, kCanPickupDrive, 0.0, kFinalDriveTurn);
+ WaitUntilDoneOrCanceled(::std::move(drive));
+ if (ShouldExitAuto()) return;
+
+ SetElevatorHeight(0.03, kElevatorProfile, true, true);
+
+ while (true) {
+ elevator_queue.status.FetchAnother();
+ if (!elevator_queue.status.get()) {
+ LOG(ERROR, "Got no elevator status packet.\n");
+ }
+ if (ShouldExitAuto()) return;
+ if (elevator_queue.status->has_tote) {
+ LOG(INFO, "Got the tote!\n");
+ break;
+ }
+ if ((::aos::time::Time::Now() - start_time) > time::Time::InSeconds(15.0)) {
+ LOG(INFO, "Out of time\n");
+ break;
+ }
+ }
+
+ ::aos::time::Time end_time = ::aos::time::Time::Now();
+ LOG(INFO, "Ended auto with %f to spare\n",
+ (time::Time::InSeconds(15.0) - (end_time - start_time)).ToSeconds());
+ if (!intake_queue.goal.MakeWithBuilder().movement(0.0).claw_closed(true)
+ .Send()) {
+ LOG(ERROR, "Sending intake goal failed.\n");
+ }
+}
+
void HandleAuto() {
::aos::time::Time start_time = ::aos::time::Time::Now();
LOG(INFO, "Starting auto mode at %f\n", start_time.ToSeconds());
// TODO(comran): Add various options for different autos down below.
- CanGrabberAuto();
+ //CanGrabberAuto();
+ TripleCanAuto();
}
} // namespace autonomous
diff --git a/bot3/autonomous/autonomous.gyp b/bot3/autonomous/autonomous.gyp
index ed93ad6..11dd524 100644
--- a/bot3/autonomous/autonomous.gyp
+++ b/bot3/autonomous/autonomous.gyp
@@ -20,6 +20,7 @@
'<(AOS)/common/controls/controls.gyp:control_loop',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_lib',
+ '<(DEPTH)/bot3/actors/actors.gyp:drivetrain_action_lib',
'<(DEPTH)/bot3/control_loops/elevator/elevator.gyp:elevator_queue',
'<(DEPTH)/bot3/control_loops/intake/intake.gyp:intake_queue',
'<(AOS)/common/common.gyp:time',