Port y2014 bot3 to new control system.
Change-Id: I894277089335e36ea95b52e033056b1a8fb4ca30
diff --git a/y2014_bot3/autonomous/BUILD b/y2014_bot3/autonomous/BUILD
new file mode 100644
index 0000000..698869d
--- /dev/null
+++ b/y2014_bot3/autonomous/BUILD
@@ -0,0 +1,44 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'auto_queue',
+ srcs = [
+ 'auto.q',
+ ],
+)
+
+cc_library(
+ name = 'auto_lib',
+ srcs = [
+ 'auto.cc',
+ ],
+ hdrs = [
+ 'auto.h',
+ ],
+ deps = [
+ ':auto_queue',
+ '//aos/common/controls:control_loop',
+ '//y2014_bot3/control_loops/drivetrain:drivetrain_queue',
+ '//y2014_bot3/control_loops/drivetrain:drivetrain_lib',
+ '//y2014_bot3/control_loops/rollers:rollers_queue',
+ '//aos/common:time',
+ '//aos/common/util:phased_loop',
+ '//aos/common/util:trapezoid_profile',
+ '//aos/common/logging',
+ '//aos/common/logging:queue_logging',
+ ],
+)
+
+cc_binary(
+ name = 'auto_y2014_bot3',
+ srcs = [
+ 'auto_main.cc',
+ ],
+ deps = [
+ '//aos/linux_code:init',
+ ':auto_queue',
+ ':auto_lib',
+ ],
+)
diff --git a/y2014_bot3/autonomous/auto.cc b/y2014_bot3/autonomous/auto.cc
index ab9ffa5..bf5a3cb 100644
--- a/y2014_bot3/autonomous/auto.cc
+++ b/y2014_bot3/autonomous/auto.cc
@@ -6,19 +6,18 @@
#include "aos/common/time.h"
#include "aos/common/util/trapezoid_profile.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
-#include "bot3/actions/drivetrain_action.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
-#include "bot3/control_loops/rollers/rollers.q.h"
-#include "frc971/actions/action_client.h"
-#include "frc971/actions/drivetrain_action.q.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "y2014_bot3/autonomous/auto.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2014_bot3/control_loops/rollers/rollers.q.h"
using ::aos::time::Time;
+using ::y2014_bot3::control_loops::drivetrain_queue;
+using ::y2014_bot3::control_loops::rollers_queue;
-namespace bot3 {
+namespace y2014_bot3 {
namespace autonomous {
namespace time = ::aos::time;
@@ -26,31 +25,18 @@
static double left_initial_position, right_initial_position;
bool ShouldExitAuto() {
- ::frc971::autonomous::autonomous.FetchLatest();
- bool ans = !::frc971::autonomous::autonomous->run_auto;
+ ::y2014_bot3::autonomous::autonomous.FetchLatest();
+ bool ans = !::y2014_bot3::autonomous::autonomous->run_auto;
if (ans) {
LOG(INFO, "Time to exit auto mode\n");
}
return ans;
}
-void StopDrivetrain() {
- LOG(INFO, "Stopping the drivetrain\n");
- control_loops::drivetrain.goal.MakeWithBuilder()
- .control_loop_driving(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");
- control_loops::drivetrain.goal.MakeWithBuilder()
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(false)
- .highgear(false)
.steering(0.0)
.throttle(0.0)
.left_goal(left_initial_position)
@@ -60,118 +46,48 @@
.Send();
}
-void WaitUntilDoneOrCanceled(::frc971::Action *action) {
- while (true) {
- // Poll the running bit and auto done bits.
- ::aos::time::PhasedLoop10MS(5000);
- if (!action->Running() || ShouldExitAuto()) {
- return;
- }
- }
-}
-
-::std::unique_ptr<::frc971::TypedAction
- < ::frc971::actions::DrivetrainActionQueueGroup>>
-SetDriveGoal(double distance, bool slow_acceleration,
- double maximum_velocity = 1.7, double theta = 0) {
- LOG(INFO, "Driving to %f\n", distance);
- auto drivetrain_action = actions::MakeDrivetrainAction();
- drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
- drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
- drivetrain_action->GetGoal()->y_offset = distance;
- drivetrain_action->GetGoal()->theta_offset = theta;
- drivetrain_action->GetGoal()->maximum_velocity = maximum_velocity;
- drivetrain_action->GetGoal()->maximum_acceleration =
- slow_acceleration ? 2.5 : 3.0;
- drivetrain_action->Start();
- left_initial_position +=
- distance - theta * control_loops::kBot3TurnWidth / 2.0;
- right_initial_position +=
- distance + theta * control_loops::kBot3TurnWidth / 2.0;
- return ::std::move(drivetrain_action);
-}
-
void InitializeEncoders() {
- control_loops::drivetrain.status.FetchLatest();
- while (!control_loops::drivetrain.status.get()) {
- LOG(WARNING,
- "No previous drivetrain position packet, trying to fetch again\n");
- control_loops::drivetrain.status.FetchNextBlocking();
- }
+ control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
- control_loops::drivetrain.status->filtered_left_position;
+ control_loops::drivetrain_queue.status->filtered_left_position;
right_initial_position =
- control_loops::drivetrain.status->filtered_right_position;
-
-}
-
-void StopRollers() {
- control_loops::rollers.goal.MakeWithBuilder()
- .intake(0)
- .low_spit(0)
- .human_player(false)
- .Send();
-}
-
-void SpitBallFront() {
- control_loops::rollers.goal.MakeWithBuilder()
- .intake(0)
- .low_spit(1)
- .human_player(false)
- .Send();
- time::SleepFor(time::Time::InSeconds(1));
- StopRollers();
-}
-
-void IntakeBallBack() {
- control_loops::rollers.goal.MakeWithBuilder()
- .intake(-1)
- .low_spit(0)
- .human_player(false)
- .Send();
- time::SleepFor(time::Time::InSeconds(1.5));
- StopRollers();
-}
-
-void ScoreBall(const double distance, const double velocity) {
- // Drive to the goal, score, and drive back.
- {
- // Drive forward.
- auto drivetrain_action = SetDriveGoal(distance,
- false, velocity);
- LOG(INFO, "Waiting until drivetrain is finished.\n");
- WaitUntilDoneOrCanceled(drivetrain_action.get());
- time::SleepFor(time::Time::InSeconds(0.5));
- if (ShouldExitAuto()) return;
- }
- {
- LOG(INFO, "Spitting ball.\n");
- SpitBallFront();
- time::SleepFor(time::Time::InSeconds(0.5));
- if (ShouldExitAuto()) return;
- }
- {
- // Drive back.
- LOG(INFO, "Driving back.\n");
- auto drivetrain_action = SetDriveGoal(-distance,
- false, velocity);
- LOG(INFO, "Waiting until drivetrain is finished.\n");
- WaitUntilDoneOrCanceled(drivetrain_action.get());
- if (ShouldExitAuto()) return;
- }
+ control_loops::drivetrain_queue.status->filtered_right_position;
}
void HandleAuto() {
- constexpr double kDriveDistance = 4.86;
- constexpr double kAutoVelocity = 2.5;
+ ::aos::time::Time start_time = ::aos::time::Time::Now();
+ LOG(INFO, "Starting auto mode at %f\n", start_time.ToSeconds());
- if (ShouldExitAuto()) return;
+ // TODO(comran): Add various options for different autos down below.
ResetDrivetrain();
InitializeEncoders();
- if (ShouldExitAuto()) return;
- ScoreBall(kDriveDistance, kAutoVelocity);
+ LOG(INFO, "Driving\n");
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .highgear(false)
+ .quickturn(false)
+ .steering(0.0)
+ .throttle(0.5)
+ .left_goal(left_initial_position)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position)
+ .right_velocity_goal(0)
+ .Send();
+ time::SleepFor(time::Time::InSeconds(2.0));
+
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .highgear(false)
+ .quickturn(false)
+ .steering(0.0)
+ .throttle(0.0)
+ .left_goal(left_initial_position)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position)
+ .right_velocity_goal(0)
+ .Send();
}
} // namespace autonomous
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/autonomous/auto.h b/y2014_bot3/autonomous/auto.h
index 896d22c..387d2a4 100644
--- a/y2014_bot3/autonomous/auto.h
+++ b/y2014_bot3/autonomous/auto.h
@@ -1,12 +1,12 @@
-#ifndef BOT3_AUTONOMOUS_AUTO_H_
-#define BOT3_AUTONOMOUS_AUTO_H_
+#ifndef Y2014_BOT3_AUTONOMOUS_AUTO_H_
+#define Y2014_BOT3_AUTONOMOUS_AUTO_H_
-namespace bot3 {
+namespace y2014_bot3 {
namespace autonomous {
void HandleAuto();
} // namespace autonomous
-} // namespace bot3
+} // namespace y2014_bot3
-#endif // BOT3_AUTONOMOUS_AUTO_H_
+#endif // Y2014_BOT3_AUTONOMOUS_AUTO_H_
diff --git a/y2014_bot3/autonomous/auto.q b/y2014_bot3/autonomous/auto.q
new file mode 100644
index 0000000..68fcd5f
--- /dev/null
+++ b/y2014_bot3/autonomous/auto.q
@@ -0,0 +1,7 @@
+package y2014_bot3.autonomous;
+
+message AutoControl {
+ // True if auto mode should be running, false otherwise.
+ bool run_auto;
+};
+queue AutoControl autonomous;
diff --git a/y2014_bot3/autonomous/auto_main.cc b/y2014_bot3/autonomous/auto_main.cc
index eb8f865..7966d97 100644
--- a/y2014_bot3/autonomous/auto_main.cc
+++ b/y2014_bot3/autonomous/auto_main.cc
@@ -3,35 +3,35 @@
#include "aos/common/time.h"
#include "aos/linux_code/init.h"
#include "aos/common/logging/logging.h"
-#include "bot3/autonomous/auto.h"
-#include "frc971/autonomous/auto.q.h"
+#include "y2014_bot3/autonomous/auto.q.h"
+#include "y2014_bot3/autonomous/auto.h"
using ::aos::time::Time;
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init();
- LOG(INFO, "Auto main started\n");
- ::frc971::autonomous::autonomous.FetchLatest();
- while (!::frc971::autonomous::autonomous.get()) {
- ::frc971::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Auto main started.\n");
+ ::y2014_bot3::autonomous::autonomous.FetchLatest();
+ while (!::y2014_bot3::autonomous::autonomous.get()) {
+ ::y2014_bot3::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
}
while (true) {
- while (!::frc971::autonomous::autonomous->run_auto) {
- ::frc971::autonomous::autonomous.FetchNextBlocking();
+ while (!::y2014_bot3::autonomous::autonomous->run_auto) {
+ ::y2014_bot3::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();
- ::bot3::autonomous::HandleAuto();
+ ::y2014_bot3::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();
+ while (::y2014_bot3::autonomous::autonomous->run_auto) {
+ ::y2014_bot3::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
}
LOG(INFO, "Waiting for auto to start back up.\n");
diff --git a/y2014_bot3/autonomous/autonomous.gyp b/y2014_bot3/autonomous/autonomous.gyp
deleted file mode 100644
index 01d95d0..0000000
--- a/y2014_bot3/autonomous/autonomous.gyp
+++ /dev/null
@@ -1,50 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'auto_queue',
- 'type': 'static_library',
- 'sources': [
- '<(DEPTH)/frc971/autonomous/auto.q',
- ],
- 'variables': {
- 'header_path': 'frc971/autonomous',
- },
- 'includes': ['../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'auto_lib',
- 'type': 'static_library',
- 'sources': [
- 'auto.cc',
- ],
- 'dependencies': [
- 'auto_queue',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/bot3/control_loops/rollers/rollers.gyp:rollers_loop',
- '<(DEPTH)/bot3/actions/actions.gyp:action_client',
- '<(DEPTH)/bot3/actions/actions.gyp:drivetrain_action_lib',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/util/util.gyp:phased_loop',
- '<(AOS)/common/util/util.gyp:trapezoid_profile',
- '<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/controls/controls.gyp:control_loop',
- ],
- },
- {
- 'target_name': 'auto',
- 'type': 'executable',
- 'sources': [
- 'auto_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- 'auto_queue',
- 'auto_lib',
- ],
- },
- ],
-}