Added 3 can auto.
Change-Id: Id81eafb797708d44c95b754947badc46fb552421
diff --git a/bot3/actors/actors.gyp b/bot3/actors/actors.gyp
index 794939a..8e1c2c6 100644
--- a/bot3/actors/actors.gyp
+++ b/bot3/actors/actors.gyp
@@ -4,6 +4,58 @@
'target_name': 'binaries',
'type': 'none',
'dependencies': [
+ 'drivetrain_action_bot3',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action_queue',
+ 'type': 'static_library',
+ 'sources': ['drivetrain_action.q'],
+ 'variables': {
+ 'header_path': 'bot3/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'drivetrain_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain_actor.cc',
+ ],
+ 'dependencies': [
+ 'drivetrain_action_queue',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(EXTERNALS):eigen',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_lib',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'drivetrain_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action_bot3',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'drivetrain_action_queue',
+ 'drivetrain_action_lib',
],
},
],
diff --git a/bot3/actors/drivetrain_action.q b/bot3/actors/drivetrain_action.q
new file mode 100644
index 0000000..9ad55d3
--- /dev/null
+++ b/bot3/actors/drivetrain_action.q
@@ -0,0 +1,29 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+// Parameters to send with start.
+struct DrivetrainActionParams {
+ double left_initial_position;
+ double right_initial_position;
+ double y_offset;
+ double theta_offset;
+ double maximum_velocity;
+ double maximum_acceleration;
+ double maximum_turn_velocity;
+ double maximum_turn_acceleration;
+};
+
+queue_group DrivetrainActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ DrivetrainActionParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/bot3/actors/drivetrain_actor.cc b/bot3/actors/drivetrain_actor.cc
new file mode 100644
index 0000000..8421f43
--- /dev/null
+++ b/bot3/actors/drivetrain_actor.cc
@@ -0,0 +1,172 @@
+#include "bot3/actors/drivetrain_actor.h"
+
+#include <functional>
+#include <numeric>
+
+#include <Eigen/Dense>
+
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/time.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/drivetrain/drivetrain_dog_motor_plant.h"
+
+namespace frc971 {
+namespace actors {
+
+using ::bot3::control_loops::drivetrain_queue;
+using ::bot3::control_loops::kDrivetrainTurnWidth;
+
+DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
+ : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
+
+bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
+ static const auto K = ::bot3::control_loops::MakeDrivetrainLoop().K();
+
+ const double yoffset = params.y_offset;
+ const double turn_offset =
+ params.theta_offset * kDrivetrainTurnWidth / 2.0;
+ LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
+
+ // Measured conversion to get the distance right.
+ ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
+ const double goal_velocity = 0.0;
+ const double epsilon = 0.01;
+ ::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
+
+ profile.set_maximum_acceleration(params.maximum_acceleration);
+ profile.set_maximum_velocity(params.maximum_velocity);
+ turn_profile.set_maximum_acceleration(params.maximum_turn_acceleration *
+ kDrivetrainTurnWidth /
+ 2.0);
+ turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
+ kDrivetrainTurnWidth / 2.0);
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(5, 2500);
+
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get()) {
+ const auto& status = *drivetrain_queue.status;
+ if (::std::abs(status.uncapped_left_voltage -
+ status.uncapped_right_voltage) > 24) {
+ LOG(DEBUG, "spinning in place\n");
+ // They're more than 24V apart, so stop moving forwards and let it deal
+ // with spinning first.
+ profile.SetGoal(
+ (status.filtered_left_position + status.filtered_right_position -
+ params.left_initial_position - params.right_initial_position) /
+ 2.0);
+ } else {
+ static const double divisor = K(0, 0) + K(0, 2);
+ double dx_left, dx_right;
+ if (status.uncapped_left_voltage > 12.0) {
+ dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
+ } else if (status.uncapped_left_voltage < -12.0) {
+ dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
+ } else {
+ dx_left = 0;
+ }
+ if (status.uncapped_right_voltage > 12.0) {
+ dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
+ } else if (status.uncapped_right_voltage < -12.0) {
+ dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
+ } else {
+ dx_right = 0;
+ }
+ double dx;
+ if (dx_left == 0 && dx_right == 0) {
+ dx = 0;
+ } else if (dx_left != 0 && dx_right != 0 &&
+ ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
+ // Both saturating in opposite directions. Don't do anything.
+ LOG(DEBUG, "Saturating opposite ways, not adjusting\n");
+ dx = 0;
+ } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
+ dx = dx_left;
+ } else {
+ dx = dx_right;
+ }
+ if (dx != 0) {
+ LOG(DEBUG, "adjusting goal by %f\n", dx);
+ profile.MoveGoal(-dx);
+ }
+ }
+ } else {
+ // If we ever get here, that's bad and we should just give up
+ LOG(ERROR, "no drivetrain status!\n");
+ return false;
+ }
+
+ const auto drive_profile_goal_state =
+ profile.Update(yoffset, goal_velocity);
+ const auto turn_profile_goal_state = turn_profile.Update(turn_offset, 0.0);
+ left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
+ right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
+
+ if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
+ ::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
+ break;
+ }
+
+ if (ShouldCancel()) return true;
+
+ LOG(DEBUG, "Driving left to %f, right to %f\n",
+ left_goal_state(0, 0) + params.left_initial_position,
+ right_goal_state(0, 0) + params.right_initial_position);
+ drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ //.highgear(false)
+ .left_goal(left_goal_state(0, 0) + params.left_initial_position)
+ .right_goal(right_goal_state(0, 0) + params.right_initial_position)
+ .left_velocity_goal(left_goal_state(1, 0))
+ .right_velocity_goal(right_goal_state(1, 0))
+ .Send();
+ }
+ if (ShouldCancel()) return true;
+ drivetrain_queue.status.FetchLatest();
+ while (!drivetrain_queue.status.get()) {
+ LOG(WARNING,
+ "No previous drivetrain status packet, trying to fetch again\n");
+ drivetrain_queue.status.FetchNextBlocking();
+ if (ShouldCancel()) return true;
+ }
+ while (true) {
+ if (ShouldCancel()) return true;
+ const double kPositionThreshold = 0.05;
+
+ const double left_error = ::std::abs(
+ drivetrain_queue.status->filtered_left_position -
+ (left_goal_state(0, 0) + params.left_initial_position));
+ const double right_error = ::std::abs(
+ drivetrain_queue.status->filtered_right_position -
+ (right_goal_state(0, 0) + params.right_initial_position));
+ const double velocity_error =
+ ::std::abs(drivetrain_queue.status->robot_speed);
+ if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
+ velocity_error < 0.2) {
+ break;
+ } else {
+ LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
+ velocity_error);
+ }
+ drivetrain_queue.status.FetchNextBlocking();
+ }
+ LOG(INFO, "Done moving\n");
+ return true;
+}
+
+::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
+ const ::frc971::actors::DrivetrainActionParams& params) {
+ return ::std::unique_ptr<DrivetrainAction>(
+ new DrivetrainAction(&::frc971::actors::drivetrain_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/bot3/actors/drivetrain_actor.h b/bot3/actors/drivetrain_actor.h
new file mode 100644
index 0000000..0002d7d
--- /dev/null
+++ b/bot3/actors/drivetrain_actor.h
@@ -0,0 +1,31 @@
+#ifndef Y2015_ACTIONS_DRIVETRAIN_ACTION_H_
+#define Y2015_ACTIONS_DRIVETRAIN_ACTION_H_
+
+#include <memory>
+
+#include "bot3/actors/drivetrain_action.q.h"
+#include "aos/common/actions/actor.h"
+#include "aos/common/actions/actions.h"
+
+namespace frc971 {
+namespace actors {
+
+class DrivetrainActor
+ : public aos::common::actions::ActorBase<DrivetrainActionQueueGroup> {
+ public:
+ explicit DrivetrainActor(DrivetrainActionQueueGroup* s);
+
+ bool RunAction(const actors::DrivetrainActionParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<DrivetrainActionQueueGroup>
+ DrivetrainAction;
+
+// Makes a new DrivetrainActor action.
+::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
+ const ::frc971::actors::DrivetrainActionParams& params);
+
+} // namespace actors
+} // namespace frc971
+
+#endif
diff --git a/bot3/actors/drivetrain_actor_main.cc b/bot3/actors/drivetrain_actor_main.cc
new file mode 100644
index 0000000..73a3ee5
--- /dev/null
+++ b/bot3/actors/drivetrain_actor_main.cc
@@ -0,0 +1,18 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "bot3/actors/drivetrain_action.q.h"
+#include "bot3/actors/drivetrain_actor.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actors::DrivetrainActor drivetrain(
+ &::frc971::actors::drivetrain_action);
+ drivetrain.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
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',
diff --git a/bot3/prime/start_list.txt b/bot3/prime/start_list.txt
index 5224094..2dcfd90 100644
--- a/bot3/prime/start_list.txt
+++ b/bot3/prime/start_list.txt
@@ -2,6 +2,7 @@
wpilib_interface_bot3
binary_log_writer
drivetrain_bot3
+drivetrain_action_bot3
intake
elevator
auto_bot3