Convert y2014 to y2016, and get the drivetrain running.
I made a y2016 folder based on what was in y2014. Right now, it only has
a drivetrain control loop, and all the constants are not up to date with
the current CAD design. Changes from y2014 can be viewed by comparing to
the BASE commit, since I put this commit on top of a dummy y2014 folder
renamed y2016. Hope this helps.
Change-Id: I3f1d1d1a14a99ac441c51b2a67cbade7cbd708ab
diff --git a/y2016/autonomous/BUILD b/y2016/autonomous/BUILD
index 984b110..3cda768 100644
--- a/y2016/autonomous/BUILD
+++ b/y2016/autonomous/BUILD
@@ -9,23 +9,18 @@
'auto.h',
],
deps = [
- '//frc971/autonomous:auto_queue',
+ '//aos/common/actions:action_lib',
'//aos/common/controls:control_loop',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//y2014/control_loops/shooter:shooter_queue',
- '//y2014/control_loops/claw:claw_queue',
- '//y2014:constants',
+ '//aos/common/logging',
+ '//aos/common/logging:queue_logging',
'//aos/common:time',
'//aos/common/util:phased_loop',
'//aos/common/util:trapezoid_profile',
- '//aos/common/logging',
- '//aos/common/actions:action_lib',
- '//y2014/actors:shoot_action_lib',
- '//y2014/actors:drivetrain_action_lib',
- '//y2014/queues:hot_goal',
- '//aos/common/logging:queue_logging',
- '//y2014/queues:profile_params',
- '//y2014/queues:auto_mode',
+ '//frc971/autonomous:auto_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//y2016:constants',
+ '//y2016/queues:profile_params',
+ '//y2016/actors:drivetrain_action_lib',
],
)
@@ -35,8 +30,8 @@
'auto_main.cc',
],
deps = [
+ ':auto_lib',
'//aos/linux_code:init',
'//frc971/autonomous:auto_queue',
- ':auto_lib',
],
)
diff --git a/y2016/autonomous/auto.cc b/y2016/autonomous/auto.cc
index e954637..ad211cf 100644
--- a/y2016/autonomous/auto.cc
+++ b/y2016/autonomous/auto.cc
@@ -1,29 +1,25 @@
+#include "y2016/autonomous/auto.h"
+
#include <stdio.h>
#include <memory>
-#include "aos/common/util/phased_loop.h"
-#include "aos/common/time.h"
-#include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/actions/actions.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
-#include "aos/common/actions/actions.h"
+#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
#include "frc971/autonomous/auto.q.h"
-#include "y2014/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/actors/shoot_actor.h"
-#include "y2014/actors/drivetrain_actor.h"
-#include "y2014/queues/auto_mode.q.h"
-
-#include "y2014/queues/hot_goal.q.h"
-#include "y2014/queues/profile_params.q.h"
+#include "y2016/actors/drivetrain_actor.h"
+#include "y2016/constants.h"
+#include "y2016/queues/profile_params.q.h"
using ::aos::time::Time;
-namespace y2014 {
+namespace y2016 {
namespace autonomous {
namespace time = ::aos::time;
@@ -39,19 +35,6 @@
return ans;
}
-void StopDrivetrain() {
- LOG(INFO, "Stopping the drivetrain\n");
- frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(true)
- .left_goal(left_initial_position)
- .left_velocity_goal(0)
- .right_goal(right_initial_position)
- .right_velocity_goal(0)
- .quickturn(false)
- .Send();
-}
-
void ResetDrivetrain() {
LOG(INFO, "resetting the drivetrain\n");
::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
@@ -81,111 +64,16 @@
}
}
-void StepDrive(double distance, double theta) {
- double left_goal = (left_initial_position + distance -
- theta * constants::GetValues().turn_width / 2.0);
- double right_goal = (right_initial_position + distance +
- theta * constants::GetValues().turn_width / 2.0);
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(true)
- .left_goal(left_goal)
- .right_goal(right_goal)
- .left_velocity_goal(0.0)
- .right_velocity_goal(0.0)
- .Send();
- left_initial_position = left_goal;
- right_initial_position = right_goal;
-}
-
-void PositionClawVertically(double intake_power = 0.0, double centering_power = 0.0) {
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.0)
- .separation_angle(0.0)
- .intake(intake_power)
- .centering(centering_power)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
-}
-
-void PositionClawBackIntake() {
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(-2.273474)
- .separation_angle(0.0)
- .intake(12.0)
- .centering(12.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
-}
-
-void PositionClawUpClosed() {
- // Move the claw to where we're going to shoot from but keep it closed until
- // it gets there.
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.86)
- .separation_angle(0.0)
- .intake(4.0)
- .centering(1.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
-}
-
-void PositionClawForShot() {
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.86)
- .separation_angle(0.10)
- .intake(4.0)
- .centering(1.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
-}
-
-void SetShotPower(double power) {
- LOG(INFO, "Setting shot power to %f\n", power);
- if (!control_loops::shooter_queue.goal.MakeWithBuilder()
- .shot_power(power)
- .shot_requested(false)
- .unload_requested(false)
- .load_requested(false)
- .Send()) {
- LOG(WARNING, "sending shooter goal failed\n");
- }
-}
-
-void WaitUntilNear(double distance) {
- while (true) {
- if (ShouldExitAuto()) return;
- ::frc971::control_loops::drivetrain_queue.status.FetchAnother();
- double left_error = ::std::abs(
- left_initial_position -
- ::frc971::control_loops::drivetrain_queue.status->filtered_left_position);
- double right_error = ::std::abs(
- right_initial_position -
- ::frc971::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;
- }
- }
-}
-
const ProfileParams kFastDrive = {3.0, 2.5};
const ProfileParams kSlowDrive = {2.5, 2.5};
-const ProfileParams kFastWithBallDrive = {3.0, 2.0};
-const ProfileParams kSlowWithBallDrive = {2.5, 2.0};
const ProfileParams kFastTurn = {3.0, 10.0};
-::std::unique_ptr<::y2014::actors::DrivetrainAction> SetDriveGoal(
+::std::unique_ptr<::y2016::actors::DrivetrainAction> SetDriveGoal(
double distance, const ProfileParams drive_params, double theta = 0,
const ProfileParams &turn_params = kFastTurn) {
LOG(INFO, "Driving to %f\n", distance);
- ::y2014::actors::DrivetrainActionParams params;
+ ::y2016::actors::DrivetrainActionParams params;
params.left_initial_position = left_initial_position;
params.right_initial_position = right_initial_position;
params.y_offset = distance;
@@ -203,13 +91,6 @@
return ::std::move(drivetrain_action);
}
-void Shoot() {
- // Shoot.
- auto shoot_action = actors::MakeShootAction();
- shoot_action->Start();
- WaitUntilDoneOrCanceled(::std::move(shoot_action));
-}
-
void InitializeEncoders() {
::frc971::control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
@@ -218,315 +99,12 @@
::frc971::control_loops::drivetrain_queue.status->filtered_right_position;
}
-void WaitUntilClawDone() {
- while (true) {
- // Poll the running bit and auto done bits.
- ::aos::time::PhasedLoopXMS(10, 5000);
- control_loops::claw_queue.status.FetchLatest();
- control_loops::claw_queue.goal.FetchLatest();
- if (ShouldExitAuto()) {
- return;
- }
- if (control_loops::claw_queue.status.get() == nullptr ||
- control_loops::claw_queue.goal.get() == nullptr) {
- continue;
- }
- bool ans =
- control_loops::claw_queue.status->zeroed &&
- (::std::abs(control_loops::claw_queue.status->bottom_velocity) <
- 1.0) &&
- (::std::abs(control_loops::claw_queue.status->bottom -
- control_loops::claw_queue.goal->bottom_angle) <
- 0.10) &&
- (::std::abs(control_loops::claw_queue.status->separation -
- control_loops::claw_queue.goal->separation_angle) <
- 0.4);
- if (ans) {
- return;
- }
- if (ShouldExitAuto()) return;
- }
-}
-
-class HotGoalDecoder {
- public:
- HotGoalDecoder() {
- ResetCounts();
- }
-
- void ResetCounts() {
- hot_goal.FetchLatest();
- if (hot_goal.get()) {
- start_counts_ = *hot_goal;
- LOG_STRUCT(INFO, "counts reset to", start_counts_);
- start_counts_valid_ = true;
- } else {
- LOG(WARNING, "no hot goal message. ignoring\n");
- start_counts_valid_ = false;
- }
- }
-
- void Update(bool block = false) {
- if (block) {
- hot_goal.FetchAnother();
- } else {
- hot_goal.FetchLatest();
- }
- if (hot_goal.get()) LOG_STRUCT(INFO, "new counts", *hot_goal);
- }
-
- bool left_triggered() const {
- if (!start_counts_valid_ || !hot_goal.get()) return false;
- return (hot_goal->left_count - start_counts_.left_count) > kThreshold;
- }
-
- bool right_triggered() const {
- if (!start_counts_valid_ || !hot_goal.get()) return false;
- return (hot_goal->right_count - start_counts_.right_count) > kThreshold;
- }
-
- bool is_left() const {
- if (!start_counts_valid_ || !hot_goal.get()) return false;
- const uint64_t left_difference =
- hot_goal->left_count - start_counts_.left_count;
- const uint64_t right_difference =
- hot_goal->right_count - start_counts_.right_count;
- if (left_difference > kThreshold) {
- if (right_difference > kThreshold) {
- // We've seen a lot of both, so pick the one we've seen the most of.
- return left_difference > right_difference;
- } else {
- // We've seen enough left but not enough right, so go with it.
- return true;
- }
- } else {
- // We haven't seen enough left, so it's not left.
- return false;
- }
- }
-
- bool is_right() const {
- if (!start_counts_valid_ || !hot_goal.get()) return false;
- const uint64_t left_difference =
- hot_goal->left_count - start_counts_.left_count;
- const uint64_t right_difference =
- hot_goal->right_count - start_counts_.right_count;
- if (right_difference > kThreshold) {
- if (left_difference > kThreshold) {
- // We've seen a lot of both, so pick the one we've seen the most of.
- return right_difference > left_difference;
- } else {
- // We've seen enough right but not enough left, so go with it.
- return true;
- }
- } else {
- // We haven't seen enough right, so it's not right.
- return false;
- }
- }
-
- private:
- static const uint64_t kThreshold = 5;
-
- ::y2014::HotGoal start_counts_;
- bool start_counts_valid_;
-};
-
void HandleAuto() {
- enum class AutoVersion : uint8_t {
- kStraight,
- kDoubleHot,
- kSingleHot,
- };
-
- // The front of the robot is 1.854 meters from the wall
- static const double kShootDistance = 3.15;
- static const double kPickupDistance = 0.5;
- static const double kTurnAngle = 0.3;
-
- ::aos::time::Time start_time = ::aos::time::Time::Now();
LOG(INFO, "Handling auto mode\n");
- AutoVersion auto_version;
- ::y2014::sensors::auto_mode.FetchLatest();
- if (!::y2014::sensors::auto_mode.get()) {
- LOG(WARNING, "not sure which auto mode to use\n");
- auto_version = AutoVersion::kStraight;
- } else {
- static const double kSelectorMin = 0.2, kSelectorMax = 4.4;
-
- const double kSelectorStep = (kSelectorMax - kSelectorMin) / 3.0;
- if (::y2014::sensors::auto_mode->voltage < kSelectorStep + kSelectorMin) {
- auto_version = AutoVersion::kSingleHot;
- } else if (::y2014::sensors::auto_mode->voltage <
- 2 * kSelectorStep + kSelectorMin) {
- auto_version = AutoVersion::kStraight;
- } else {
- auto_version = AutoVersion::kDoubleHot;
- }
- }
- LOG(INFO, "running auto %" PRIu8 "\n", static_cast<uint8_t>(auto_version));
-
- const ProfileParams &drive_params =
- (auto_version == AutoVersion::kStraight) ? kFastDrive : kSlowDrive;
- const ProfileParams &drive_with_ball_params =
- (auto_version == AutoVersion::kStraight) ? kFastWithBallDrive
- : kSlowWithBallDrive;
-
- HotGoalDecoder hot_goal_decoder;
- // True for left, false for right.
- bool first_shot_left, second_shot_left_default, second_shot_left;
-
ResetDrivetrain();
-
- time::SleepFor(time::Time::InSeconds(0.1));
- if (ShouldExitAuto()) return;
InitializeEncoders();
-
- // Turn the claw on, keep it straight up until the ball has been grabbed.
- LOG(INFO, "Claw going up at %f\n",
- (::aos::time::Time::Now() - start_time).ToSeconds());
- PositionClawVertically(12.0, 4.0);
- SetShotPower(115.0);
-
- // Wait for the ball to enter the claw.
- time::SleepFor(time::Time::InSeconds(0.25));
- if (ShouldExitAuto()) return;
- LOG(INFO, "Readying claw for shot at %f\n",
- (::aos::time::Time::Now() - start_time).ToSeconds());
-
- {
- if (ShouldExitAuto()) return;
- // Drive to the goal.
- auto drivetrain_action = SetDriveGoal(-kShootDistance, drive_params);
- time::SleepFor(time::Time::InSeconds(0.75));
- PositionClawForShot();
- LOG(INFO, "Waiting until drivetrain is finished\n");
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- }
-
- hot_goal_decoder.Update();
- if (hot_goal_decoder.is_left()) {
- LOG(INFO, "first shot left\n");
- first_shot_left = true;
- second_shot_left_default = false;
- } else if (hot_goal_decoder.is_right()) {
- LOG(INFO, "first shot right\n");
- first_shot_left = false;
- second_shot_left_default = true;
- } else {
- LOG(INFO, "first shot defaulting left\n");
- first_shot_left = true;
- second_shot_left_default = true;
- }
- if (auto_version == AutoVersion::kDoubleHot) {
- if (ShouldExitAuto()) return;
- auto drivetrain_action = SetDriveGoal(
- 0, drive_with_ball_params, first_shot_left ? kTurnAngle : -kTurnAngle);
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- } else if (auto_version == AutoVersion::kSingleHot) {
- do {
- // TODO(brians): Wait for next message with timeout or something.
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.003));
- hot_goal_decoder.Update(false);
- if (ShouldExitAuto()) return;
- } while (!hot_goal_decoder.left_triggered() &&
- (::aos::time::Time::Now() - start_time) <
- ::aos::time::Time::InSeconds(9));
- } else if (auto_version == AutoVersion::kStraight) {
- time::SleepFor(time::Time::InSeconds(0.4));
- }
-
- // Shoot.
- LOG(INFO, "Shooting at %f\n",
- (::aos::time::Time::Now() - start_time).ToSeconds());
- Shoot();
- time::SleepFor(time::Time::InSeconds(0.05));
-
- if (auto_version == AutoVersion::kDoubleHot) {
- if (ShouldExitAuto()) return;
- auto drivetrain_action = SetDriveGoal(
- 0, drive_with_ball_params, first_shot_left ? -kTurnAngle : kTurnAngle);
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- } else if (auto_version == AutoVersion::kSingleHot) {
- LOG(INFO, "auto done at %f\n",
- (::aos::time::Time::Now() - start_time).ToSeconds());
- PositionClawVertically(0.0, 0.0);
- return;
- }
-
- {
- if (ShouldExitAuto()) return;
- // Intake the new ball.
- LOG(INFO, "Claw ready for intake at %f\n",
- (::aos::time::Time::Now() - start_time).ToSeconds());
- PositionClawBackIntake();
- auto drivetrain_action =
- SetDriveGoal(kShootDistance + kPickupDistance, drive_params);
- LOG(INFO, "Waiting until drivetrain is finished\n");
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- LOG(INFO, "Wait for the claw at %f\n",
- (::aos::time::Time::Now() - start_time).ToSeconds());
- WaitUntilClawDone();
- if (ShouldExitAuto()) return;
- }
-
- // Drive back.
- {
- LOG(INFO, "Driving back at %f\n",
- (::aos::time::Time::Now() - start_time).ToSeconds());
- auto drivetrain_action =
- SetDriveGoal(-(kShootDistance + kPickupDistance), drive_params);
- time::SleepFor(time::Time::InSeconds(0.3));
- hot_goal_decoder.ResetCounts();
- if (ShouldExitAuto()) return;
- PositionClawUpClosed();
- WaitUntilClawDone();
- if (ShouldExitAuto()) return;
- PositionClawForShot();
- LOG(INFO, "Waiting until drivetrain is finished\n");
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- WaitUntilClawDone();
- if (ShouldExitAuto()) return;
- }
-
- hot_goal_decoder.Update();
- if (hot_goal_decoder.is_left()) {
- LOG(INFO, "second shot left\n");
- second_shot_left = true;
- } else if (hot_goal_decoder.is_right()) {
- LOG(INFO, "second shot right\n");
- second_shot_left = false;
- } else {
- LOG(INFO, "second shot defaulting %s\n",
- second_shot_left_default ? "left" : "right");
- second_shot_left = second_shot_left_default;
- }
- if (auto_version == AutoVersion::kDoubleHot) {
- if (ShouldExitAuto()) return;
- auto drivetrain_action = SetDriveGoal(
- 0, drive_params, second_shot_left ? kTurnAngle : -kTurnAngle);
- WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
- if (ShouldExitAuto()) return;
- } else if (auto_version == AutoVersion::kStraight) {
- time::SleepFor(time::Time::InSeconds(0.4));
- }
-
- LOG(INFO, "Shooting at %f\n",
- (::aos::time::Time::Now() - start_time).ToSeconds());
- // Shoot
- Shoot();
- if (ShouldExitAuto()) return;
-
- // Get ready to zero when we come back up.
- time::SleepFor(time::Time::InSeconds(0.05));
- PositionClawVertically(0.0, 0.0);
}
} // namespace autonomous
-} // namespace y2014
+} // namespace y2016
diff --git a/y2016/autonomous/auto.h b/y2016/autonomous/auto.h
index dd00bba..e5ab422 100644
--- a/y2016/autonomous/auto.h
+++ b/y2016/autonomous/auto.h
@@ -1,12 +1,12 @@
-#ifndef Y2014_AUTONOMOUS_AUTO_H_
-#define Y2014_AUTONOMOUS_AUTO_H_
+#ifndef Y2016_AUTONOMOUS_AUTO_H_
+#define Y2016_AUTONOMOUS_AUTO_H_
-namespace y2014 {
+namespace y2016 {
namespace autonomous {
void HandleAuto();
} // namespace autonomous
-} // namespace y2014
+} // namespace y2016
-#endif // Y2014_AUTONOMOUS_AUTO_H_
+#endif // Y2016_AUTONOMOUS_AUTO_H_
diff --git a/y2016/autonomous/auto_main.cc b/y2016/autonomous/auto_main.cc
index bf3acf8..adf1bca 100644
--- a/y2016/autonomous/auto_main.cc
+++ b/y2016/autonomous/auto_main.cc
@@ -4,15 +4,16 @@
#include "aos/linux_code/init.h"
#include "aos/common/logging/logging.h"
#include "frc971/autonomous/auto.q.h"
-#include "y2014/autonomous/auto.h"
+#include "y2016/autonomous/auto.h"
using ::aos::time::Time;
-int main(int /*argc*/, char * /*argv*/[]) {
+int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
LOG(INFO, "Auto main started\n");
::frc971::autonomous::autonomous.FetchLatest();
+
while (!::frc971::autonomous::autonomous.get()) {
::frc971::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
@@ -23,20 +24,23 @@
::frc971::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
}
+
LOG(INFO, "Starting auto mode\n");
::aos::time::Time start_time = ::aos::time::Time::Now();
- ::y2014::autonomous::HandleAuto();
+ ::y2016::autonomous::HandleAuto();
::aos::time::Time elapsed_time = ::aos::time::Time::Now() - start_time;
LOG(INFO, "Auto mode exited in %f, waiting for it to finish.\n",
elapsed_time.ToSeconds());
+
while (::frc971::autonomous::autonomous->run_auto) {
::frc971::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
}
+
LOG(INFO, "Waiting for auto to start back up.\n");
}
+
::aos::Cleanup();
return 0;
}
-