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/NO_BUILD_AMD64 b/NO_BUILD_AMD64
index 2a4c1cf..391b1cb 100644
--- a/NO_BUILD_AMD64
+++ b/NO_BUILD_AMD64
@@ -3,6 +3,8 @@
-//third_party/allwpilib_2016/...
-//third_party/ntcore_2016/...
-//frc971/wpilib/...
+-//y2012/wpilib/...
+-//y2012:download
-//y2014/wpilib/...
-//y2014:download
-//y2014_bot3/wpilib/...
@@ -11,5 +13,5 @@
-//y2015:download
-//y2015_bot3/wpilib/...
-//y2015_bot3:download
--//y2012/wpilib/...
--//y2012:download
+-//y2016/wpilib/...
+-//y2016:download
diff --git a/NO_BUILD_ROBORIO b/NO_BUILD_ROBORIO
index b8efe8f..f4800e2 100644
--- a/NO_BUILD_ROBORIO
+++ b/NO_BUILD_ROBORIO
@@ -1,5 +1,6 @@
-@slycot_repo//...
--//y2014/control_loops/python/...
--//y2015_bot3/control_loops/python/...
-//frc971/control_loops/python/...
-//y2012/control_loops/python/...
+-//y2014/control_loops/python/...
+-//y2015_bot3/control_loops/python/...
+-//y2016/control_loops/python/...
diff --git a/y2016/BUILD b/y2016/BUILD
index 2938aa6..19f30e0 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -15,8 +15,8 @@
'//aos/common/network:team_number',
'//aos/common:mutex',
'//frc971/control_loops:state_feedback_loop',
- '//y2014/control_loops/drivetrain:polydrivetrain_plants',
'//frc971:shifter_hall_effect',
+ '//y2016/control_loops/drivetrain:polydrivetrain_plants',
],
)
@@ -36,40 +36,20 @@
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/queues:gyro',
'//frc971/autonomous:auto_queue',
- '//y2014/control_loops/claw:claw_queue',
- '//y2014/control_loops/shooter:shooter_queue',
- '//y2014/actors:shoot_action_lib',
],
)
aos_downloader(
name = 'download',
start_srcs = [
- ':hot_goal_reader',
':joystick_reader',
- '//y2014/control_loops/drivetrain:drivetrain',
- '//y2014/control_loops/claw:claw',
- '//y2014/control_loops/shooter:shooter',
- '//y2014/autonomous:auto',
- '//y2014/actors:binaries',
'//aos:prime_start_binaries',
- '//y2014/wpilib:wpilib_interface',
+ '//y2016/control_loops/drivetrain:drivetrain',
+ '//y2016/autonomous:auto',
+ '//y2016/actors:binaries',
+ '//y2016/wpilib:wpilib_interface',
],
srcs = [
'//aos:prime_binaries',
],
)
-
-cc_binary(
- name = 'hot_goal_reader',
- srcs = [
- 'hot_goal_reader.cc',
- ],
- deps = [
- '//aos/common:time',
- '//aos/common/logging',
- '//aos/common/logging:queue_logging',
- '//aos/linux_code:init',
- '//y2014/queues:hot_goal',
- ],
-)
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index ef94367..1cda783 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -6,49 +6,6 @@
name = 'binaries',
srcs = [
':drivetrain_action',
- ':shoot_action',
- ],
-)
-
-queue_library(
- name = 'shoot_action_queue',
- srcs = [
- 'shoot_action.q',
- ],
- deps = [
- '//aos/common/actions:action_queue',
- ],
-)
-
-cc_library(
- name = 'shoot_action_lib',
- srcs = [
- 'shoot_actor.cc',
- ],
- hdrs = [
- 'shoot_actor.h',
- ],
- deps = [
- ':shoot_action_queue',
- '//aos/common/actions:action_lib',
- '//y2014/queues:profile_params',
- '//aos/common/logging',
- '//y2014/control_loops/shooter:shooter_queue',
- '//y2014/control_loops/claw:claw_queue',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//y2014:constants',
- ],
-)
-
-cc_binary(
- name = 'shoot_action',
- srcs = [
- 'shoot_actor_main.cc',
- ],
- deps = [
- '//aos/linux_code:init',
- ':shoot_action_lib',
- ':shoot_action_queue',
],
)
@@ -72,17 +29,17 @@
],
deps = [
':drivetrain_action_queue',
- '//y2014:constants',
'//aos/common:time',
'//aos/common:math',
'//aos/common/util:phased_loop',
'//aos/common/logging',
'//aos/common/actions:action_lib',
'//aos/common/logging:queue_logging',
- '//third_party/eigen',
'//aos/common/util:trapezoid_profile',
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/control_loops:state_feedback_loop',
+ '//third_party/eigen',
+ '//y2016:constants',
],
)
@@ -92,8 +49,8 @@
'drivetrain_actor_main.cc',
],
deps = [
- '//aos/linux_code:init',
':drivetrain_action_lib',
':drivetrain_action_queue',
+ '//aos/linux_code:init',
],
)
diff --git a/y2016/actors/drivetrain_action.q b/y2016/actors/drivetrain_action.q
index 2f3eb15..9bbebb2 100644
--- a/y2016/actors/drivetrain_action.q
+++ b/y2016/actors/drivetrain_action.q
@@ -1,4 +1,4 @@
-package y2014.actors;
+package y2016.actors;
import "aos/common/actions/actions.q";
diff --git a/y2016/actors/drivetrain_actor.cc b/y2016/actors/drivetrain_actor.cc
index 4cb69ca..2276108 100644
--- a/y2016/actors/drivetrain_actor.cc
+++ b/y2016/actors/drivetrain_actor.cc
@@ -1,4 +1,4 @@
-#include "y2014/actors/drivetrain_actor.h"
+#include "y2016/actors/drivetrain_actor.h"
#include <functional>
#include <numeric>
@@ -11,14 +11,14 @@
#include "aos/common/commonmath.h"
#include "aos/common/time.h"
-#include "y2014/actors/drivetrain_actor.h"
-#include "y2014/constants.h"
+#include "y2016/actors/drivetrain_actor.h"
+#include "y2016/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-namespace y2014 {
+namespace y2016 {
namespace actors {
-DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
+DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup *s)
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
loop_(constants::GetValues().make_drivetrain_loop()) {
loop_.set_controller_index(3);
@@ -52,7 +52,7 @@
::frc971::control_loops::drivetrain_queue.status.FetchLatest();
if (::frc971::control_loops::drivetrain_queue.status.get()) {
- const auto& status = *::frc971::control_loops::drivetrain_queue.status;
+ const auto &status = *::frc971::control_loops::drivetrain_queue.status;
if (::std::abs(status.uncapped_left_voltage -
status.uncapped_right_voltage) > 24) {
LOG(DEBUG, "spinning in place\n");
@@ -65,6 +65,7 @@
} 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) {
@@ -72,6 +73,7 @@
} 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) {
@@ -79,7 +81,9 @@
} else {
dx_right = 0;
}
+
double dx;
+
if (dx_left == 0 && dx_right == 0) {
dx = 0;
} else if (dx_left != 0 && dx_right != 0 &&
@@ -92,6 +96,7 @@
} else {
dx = dx_right;
}
+
if (dx != 0) {
LOG(DEBUG, "adjusting goal by %f\n", dx);
profile.MoveGoal(-dx);
@@ -130,24 +135,28 @@
}
if (ShouldCancel()) return true;
::frc971::control_loops::drivetrain_queue.status.FetchLatest();
+
while (!::frc971::control_loops::drivetrain_queue.status.get()) {
LOG(WARNING,
"No previous drivetrain status packet, trying to fetch again\n");
::frc971::control_loops::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(
- ::frc971::control_loops::drivetrain_queue.status->filtered_left_position -
- (left_goal_state(0, 0) + params.left_initial_position));
- const double right_error = ::std::abs(
- ::frc971::control_loops::drivetrain_queue.status->filtered_right_position -
- (right_goal_state(0, 0) + params.right_initial_position));
- const double velocity_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status->robot_speed);
+ const double left_error =
+ ::std::abs(::frc971::control_loops::drivetrain_queue.status
+ ->filtered_left_position -
+ (left_goal_state(0, 0) + params.left_initial_position));
+ const double right_error =
+ ::std::abs(::frc971::control_loops::drivetrain_queue.status
+ ->filtered_right_position -
+ (right_goal_state(0, 0) + params.right_initial_position));
+ const double velocity_error = ::std::abs(
+ ::frc971::control_loops::drivetrain_queue.status->robot_speed);
if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
velocity_error < 0.2) {
break;
@@ -157,15 +166,16 @@
}
::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
}
+
LOG(INFO, "Done moving\n");
return true;
}
::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2014::actors::DrivetrainActionParams& params) {
+ const ::y2016::actors::DrivetrainActionParams ¶ms) {
return ::std::unique_ptr<DrivetrainAction>(
- new DrivetrainAction(&::y2014::actors::drivetrain_action, params));
+ new DrivetrainAction(&::y2016::actors::drivetrain_action, params));
}
} // namespace actors
-} // namespace y2014
+} // namespace y2016
diff --git a/y2016/actors/drivetrain_actor.h b/y2016/actors/drivetrain_actor.h
index 2a9e307..0ab3bf2 100644
--- a/y2016/actors/drivetrain_actor.h
+++ b/y2016/actors/drivetrain_actor.h
@@ -1,5 +1,5 @@
-#ifndef Y2014_ACTIONS_DRIVETRAIN_ACTION_H_
-#define Y2014_ACTIONS_DRIVETRAIN_ACTION_H_
+#ifndef Y2016_ACTORS_DRIVETRAIN_ACTOR_H_
+#define Y2016_ACTORS_DRIVETRAIN_ACTOR_H_
#include <memory>
@@ -7,15 +7,15 @@
#include "aos/common/actions/actions.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2014/actors/drivetrain_action.q.h"
+#include "y2016/actors/drivetrain_action.q.h"
-namespace y2014 {
+namespace y2016 {
namespace actors {
class DrivetrainActor
: public ::aos::common::actions::ActorBase<DrivetrainActionQueueGroup> {
public:
- explicit DrivetrainActor(DrivetrainActionQueueGroup* s);
+ explicit DrivetrainActor(DrivetrainActionQueueGroup *s);
bool RunAction(const actors::DrivetrainActionParams ¶ms) override;
@@ -28,9 +28,9 @@
// Makes a new DrivetrainActor action.
::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2014::actors::DrivetrainActionParams& params);
+ const ::y2016::actors::DrivetrainActionParams ¶ms);
} // namespace actors
-} // namespace y2014
+} // namespace y2016
#endif
diff --git a/y2016/actors/drivetrain_actor_main.cc b/y2016/actors/drivetrain_actor_main.cc
index 4cc0070..0fe9e71 100644
--- a/y2016/actors/drivetrain_actor_main.cc
+++ b/y2016/actors/drivetrain_actor_main.cc
@@ -1,16 +1,16 @@
#include <stdio.h>
#include "aos/linux_code/init.h"
-#include "y2014/actors/drivetrain_action.q.h"
-#include "y2014/actors/drivetrain_actor.h"
+#include "y2016/actors/drivetrain_action.q.h"
+#include "y2016/actors/drivetrain_actor.h"
using ::aos::time::Time;
-int main(int /*argc*/, char * /*argv*/[]) {
+int main(int /*argc*/, char* /*argv*/ []) {
::aos::Init(-1);
- ::y2014::actors::DrivetrainActor drivetrain(
- &::y2014::actors::drivetrain_action);
+ ::y2016::actors::DrivetrainActor drivetrain(
+ &::y2016::actors::drivetrain_action);
drivetrain.Run();
::aos::Cleanup();
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;
}
-
diff --git a/y2016/constants.cc b/y2016/constants.cc
index 8cf9076..52285f7 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -1,4 +1,4 @@
-#include "y2014/constants.h"
+#include "y2016/constants.h"
#include <math.h>
#include <stdint.h>
@@ -15,21 +15,21 @@
#include "aos/common/network/team_number.h"
#include "aos/common/mutex.h"
-#include "y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
-namespace y2014 {
+namespace y2016 {
namespace constants {
namespace {
const uint16_t kCompTeamNumber = 971;
const uint16_t kPracticeTeamNumber = 9971;
-const uint16_t kRoboRioTeamNumber = 254;
+// TODO(constants): Update these to what we're using this year.
const double kCompDrivetrainEncoderRatio =
(18.0 / 50.0) /*output reduction*/ * (56.0 / 30.0) /*encoder gears*/;
const double kCompLowGearRatio = 18.0 / 60.0 * 18.0 / 50.0;
@@ -40,19 +40,16 @@
const double kPracticeHighGearRatio = kCompHighGearRatio;
const ShifterHallEffect kCompLeftDriveShifter{2.61, 2.33, 4.25, 3.28, 0.2, 0.7};
-const ShifterHallEffect kCompRightDriveShifter{2.94, 4.31, 4.32, 3.25, 0.2, 0.7};
+const ShifterHallEffect kCompRightDriveShifter{2.94, 4.31, 4.32,
+ 3.25, 0.2, 0.7};
-const ShifterHallEffect kPracticeLeftDriveShifter{2.80, 3.05, 4.15, 3.2, 0.2, 0.7};
-const ShifterHallEffect kPracticeRightDriveShifter{2.90, 3.75, 3.80, 2.98, 0.2, 0.7};
+const ShifterHallEffect kPracticeLeftDriveShifter{2.80, 3.05, 4.15,
+ 3.2, 0.2, 0.7};
+const ShifterHallEffect kPracticeRightDriveShifter{2.90, 3.75, 3.80,
+ 2.98, 0.2, 0.7};
const double kRobotWidth = 25.0 / 100.0 * 2.54;
-const double shooter_zeroing_speed = 0.05;
-const double shooter_unload_speed = 0.08;
-
-// Smaller (more negative) = opening.
-const double kCompTopClawOffset = -0.120;
-
const Values *DoGetValuesForTeam(uint16_t team) {
switch (team) {
case 1: // for tests
@@ -62,34 +59,10 @@
kCompHighGearRatio,
kCompLeftDriveShifter,
kCompRightDriveShifter,
- false,
0.5,
- ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
- ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
- 5.0, // drivetrain max speed
-
- // ShooterLimits
- {-0.00127, 0.298196, -0.0017, 0.305054, 0.0149098,
- {-0.001778, 0.000762, 0, 0},
- {-0.001778, 0.008906, 0, 0},
- {0.006096, 0.026416, 0, 0},
- shooter_zeroing_speed,
- shooter_unload_speed
- },
- {0.5,
- 0.1,
- 0.1,
- 0.0,
- 1.57,
- 0.05,
- 1.5,
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, -0.1, 0.05}, {1.0, 1.1, 1.0, 1.1}, {2.0, 2.1, 2.0, 2.1}},
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, -0.1, 0.05}, {1.0, 1.1, 1.0, 1.1}, {2.0, 2.1, 2.0, 2.1}},
- 0.01, // claw_unimportant_epsilon
- 0.9, // start_fine_tune_pos
- 4.0,
- },
- {0.07, 0.15}, // shooter_action
+ ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
+ 5.0, // drivetrain max speed
};
break;
case kCompTeamNumber:
@@ -99,87 +72,23 @@
kCompHighGearRatio,
kCompLeftDriveShifter,
kCompRightDriveShifter,
- false,
kRobotWidth,
- ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
- ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
- 5.0, // drivetrain max speed
-
- // ShooterLimits
- {-0.001041, 0.296019, -0.001488, 0.302717, 0.0149098,
- {-0.002, 0.000446, -0.002, 0.000446},
- {-0.002, 0.009078, -0.002, 0.009078},
- {0.003870, 0.026194, 0.003869, 0.026343},
- shooter_zeroing_speed,
- shooter_unload_speed
- },
- {0.800000,
- 0.400000,
- 0.000000,
- -1.220821,
- 1.822142,
- -0.849484,
- 1.42309,
- // 0.0371
- {-3.3284, 2.0917, -3.1661, 1.95,
- {-3.4, -3.02 + kCompTopClawOffset, -3.4, -2.9876 + kCompTopClawOffset},
- {-0.1433 + kCompTopClawOffset, 0.0670 + kCompTopClawOffset, -0.1460 + kCompTopClawOffset, 0.0648 + kCompTopClawOffset},
- {1.9952 + kCompTopClawOffset, 2.2, 1.9898 + kCompTopClawOffset, 2.2}},
- {-2.453460, 3.082960, -2.453460, 3.082960,
- {-2.6, -2.185752, -2.6, -2.184843},
- {-0.322249, -0.053177, -0.332248, -0.059086},
- {2.892065, 3.2, 2.888429, 3.2}},
- 0.040000, // claw_unimportant_epsilon
- -0.400000, // start_fine_tune_pos
- 4.000000,
- },
- //TODO(james): Get realer numbers for shooter_action.
- {0.07, 0.15}, // shooter_action
+ ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
+ 5.0, // drivetrain max speed
};
break;
case kPracticeTeamNumber:
- case kRoboRioTeamNumber:
return new Values{
kPracticeDrivetrainEncoderRatio,
kPracticeLowGearRatio,
kPracticeHighGearRatio,
kPracticeLeftDriveShifter,
kPracticeRightDriveShifter,
- false,
kRobotWidth,
- ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
- ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
- 5.0, // drivetrain max speed
-
- // ShooterLimits
- {-0.001042, 0.294084, -0.001935, 0.303460, 0.0138401,
- {-0.002, 0.000446, -0.002, 0.000446},
- {-0.002, 0.009078, -0.002, 0.009078},
- {0.003869, 0.026194, 0.003869, 0.026194},
- shooter_zeroing_speed,
- shooter_unload_speed
- },
- {0.400000 * 2.0,
- 0.200000 * 2.0,
- 0.000000 * 2.0,
- -0.762218 * 2.0,
- 1.767146,
- -0.849484,
- 1.42308,
- {-3.364758, 2.086668, -3.166136, 1.95,
- {-1.7 * 2.0, -1.544662 * 2.0 + 0.139081, -1.7 * 2.0, -1.547616 * 2.0 + 0.139081+ 0.013636},
- {-0.115446 * 2.0, 0.030452 * 2.0, -0.120900 * 2.0, 0.023862 * 2.0},
- {0.977884 * 2.0, 1.4 * 2.0, 0.963113 * 2.0, 1.4 * 2.0}},
- {-2.451642, 3.107504, -2.273474, 2.750,
- {-1.5 * 2.0, -1.027199 * 2.0, -1.5 * 2.0, -1.037880 * 2.0},
- {-0.116355 * 2.0, 0.017726 * 2.0, -0.125673 * 2.0, 0.008636 * 2.0},
- {2.894792 + 0.122719, 3.2, 2.887974 + 0.122719 - 0.029088, 3.2}},
- 0.040000, // claw_unimportant_epsilon
- -0.400000, // start_fine_tune_pos
- 4.000000,
- },
- //TODO(james): Get realer numbers for shooter_action.
- {0.07, 0.15}, // shooter_action
+ ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
+ 5.0, // drivetrain max speed
};
break;
default:
@@ -218,4 +127,4 @@
}
} // namespace constants
-} // namespace y2014
+} // namespace y2016
diff --git a/y2016/constants.h b/y2016/constants.h
index a31e9c0..5f55938 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -1,12 +1,12 @@
-#ifndef Y2014_CONSTANTS_H_
-#define Y2014_CONSTANTS_H_
+#ifndef Y2016_CONSTANTS_H_
+#define Y2016_CONSTANTS_H_
#include <stdint.h>
-#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/shifter_hall_effect.h"
+#include "frc971/control_loops/state_feedback_loop.h"
-namespace y2014 {
+namespace y2016 {
namespace constants {
using ::frc971::constants::ShifterHallEffect;
@@ -20,14 +20,6 @@
// This structure contains current values for all of the things that change.
struct Values {
- // This is useful for representing the 2 sides of a hall effect sensor etc.
- struct AnglePair {
- // The angles for increasing values (posedge on lower, negedge on upper).
- double lower_angle, upper_angle;
- // The angles for decreasing values (negedge on lower, posedge on upper).
- double lower_decreasing_angle, upper_decreasing_angle;
- };
-
// The ratio from the encoder shaft to the drivetrain wheels.
double drivetrain_encoder_ratio;
@@ -36,7 +28,6 @@
double low_gear_ratio;
double high_gear_ratio;
ShifterHallEffect left_drive, right_drive;
- bool clutch_transmission;
double turn_width;
@@ -44,92 +35,6 @@
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
double drivetrain_max_speed;
-
- struct ZeroingConstants {
- // The number of samples in the moving average filter.
- int average_filter_size;
- // The difference in scaled units between two index pulses.
- double index_difference;
- // The absolute position in scaled units of one of the index pulses.
- double measured_index_position;
- // Value between 0 and 1 which determines a fraction of the index_diff
- // you want to use.
- double allowable_encoder_error;
- };
-
- // Defines a range of motion for a subsystem.
- // These are all absolute positions in scaled units.
- struct Range {
- double lower_limit;
- double upper_limit;
- double lower_hard_limit;
- double upper_hard_limit;
- };
-
- struct Shooter {
- double lower_limit;
- double upper_limit;
- double lower_hard_limit;
- double upper_hard_limit;
- // If the plunger is further back than this position, it is safe for the
- // latch to be down. Anything else would be considered a collision.
- double latch_max_safe_position;
- AnglePair plunger_back;
- AnglePair pusher_distal;
- AnglePair pusher_proximal;
- double zeroing_speed;
- double unload_speed;
- };
-
- Shooter shooter;
-
- struct Claws {
- double claw_zeroing_off_speed;
- double claw_zeroing_speed;
- double claw_zeroing_separation;
-
- // claw separation that would be considered a collision
- double claw_min_separation;
- double claw_max_separation;
-
- // We should never get closer/farther than these.
- double soft_min_separation;
- double soft_max_separation;
-
- // Three hall effects are known as front, calib and back
- typedef Values::AnglePair AnglePair;
-
- struct Claw {
- double lower_hard_limit;
- double upper_hard_limit;
- double lower_limit;
- double upper_limit;
- AnglePair front;
- AnglePair calibration;
- AnglePair back;
- };
-
- Claw upper_claw;
- Claw lower_claw;
-
- double claw_unimportant_epsilon;
- double start_fine_tune_pos;
- double max_zeroing_voltage;
- };
- Claws claw;
-
- // Has all the constants for the ShootAction class.
- struct ShooterAction {
- // Minimum separation required between the claws in order to be able to
- // shoot.
- double claw_shooting_separation;
-
- // Goal to send to the claw when opening it up in preparation for shooting;
- // should be larger than claw_shooting_separation so that we can shoot
- // promptly.
- double claw_separation_goal;
- };
- ShooterAction shooter_action;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
@@ -141,6 +46,6 @@
const Values &GetValuesForTeam(uint16_t team_number);
} // namespace constants
-} // namespace y2014
+} // namespace y2016
-#endif // Y2014_CONSTANTS_H_
+#endif // Y2016_CONSTANTS_H_
diff --git a/y2016/control_loops/drivetrain/BUILD b/y2016/control_loops/drivetrain/BUILD
index 9d7edb6..1150426 100644
--- a/y2016/control_loops/drivetrain/BUILD
+++ b/y2016/control_loops/drivetrain/BUILD
@@ -5,9 +5,9 @@
genrule(
name = 'genrule_drivetrain',
visibility = ['//visibility:private'],
- cmd = '$(location //y2014/control_loops/python:drivetrain) $(OUTS)',
+ cmd = '$(location //y2016/control_loops/python:drivetrain) $(OUTS)',
tools = [
- '//y2014/control_loops/python:drivetrain',
+ '//y2016/control_loops/python:drivetrain',
],
outs = [
'drivetrain_dog_motor_plant.h',
@@ -20,9 +20,9 @@
genrule(
name = 'genrule_polydrivetrain',
visibility = ['//visibility:private'],
- cmd = '$(location //y2014/control_loops/python:polydrivetrain) $(OUTS)',
+ cmd = '$(location //y2016/control_loops/python:polydrivetrain) $(OUTS)',
tools = [
- '//y2014/control_loops/python:polydrivetrain',
+ '//y2016/control_loops/python:polydrivetrain',
],
outs = [
'polydrivetrain_dog_motor_plant.h',
@@ -59,8 +59,8 @@
],
deps = [
':polydrivetrain_plants',
- '//y2014:constants',
'//frc971/control_loops/drivetrain:drivetrain_config',
+ '//y2016:constants',
],
)
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index b4ef447..2e5138c 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -1,36 +1,46 @@
-#include "y2014/control_loops/drivetrain/drivetrain_base.h"
+#include "y2016/control_loops/drivetrain/drivetrain_base.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "y2014/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
-#include "y2014/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2016/constants.h"
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
-namespace y2014 {
+namespace y2016 {
namespace control_loops {
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
- ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
- ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
- ::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
- drivetrain::kDt, drivetrain::kStallTorque, drivetrain::kStallCurrent,
- drivetrain::kFreeSpeedRPM, drivetrain::kFreeCurrent, drivetrain::kJ,
- drivetrain::kMass, drivetrain::kRobotRadius, drivetrain::kWheelRadius,
- drivetrain::kR, drivetrain::kV, drivetrain::kT,
+ drivetrain::kDt,
+ drivetrain::kStallTorque,
+ drivetrain::kStallCurrent,
+ drivetrain::kFreeSpeedRPM,
+ drivetrain::kFreeCurrent,
+ drivetrain::kJ,
+ drivetrain::kMass,
+ drivetrain::kRobotRadius,
+ drivetrain::kWheelRadius,
+ drivetrain::kR,
+ drivetrain::kV,
+ drivetrain::kT,
- constants::GetValues().turn_width, constants::GetValues().high_gear_ratio,
+ constants::GetValues().turn_width,
+ constants::GetValues().high_gear_ratio,
constants::GetValues().low_gear_ratio,
- constants::GetValues().left_drive, constants::GetValues().right_drive};
+ constants::GetValues().left_drive,
+ constants::GetValues().right_drive};
return kDrivetrainConfig;
};
} // namespace control_loops
-} // namespace y2014
+} // namespace y2016
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.h b/y2016/control_loops/drivetrain/drivetrain_base.h
index 3a9d70e..816ed21 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.h
+++ b/y2016/control_loops/drivetrain/drivetrain_base.h
@@ -1,16 +1,16 @@
-#ifndef Y2014_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
-#define Y2014_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#ifndef Y2016_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2016_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
-namespace y2014 {
+namespace y2016 {
namespace control_loops {
const DrivetrainConfig &GetDrivetrainConfig();
} // namespace control_loops
-} // namespace y2014
+} // namespace y2016
-#endif // Y2014_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#endif // Y2016_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2016/control_loops/drivetrain/drivetrain_main.cc b/y2016/control_loops/drivetrain/drivetrain_main.cc
index 52dafcc..9c678bf 100644
--- a/y2016/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_main.cc
@@ -1,14 +1,14 @@
#include "aos/linux_code/init.h"
-#include "y2014/control_loops/drivetrain/drivetrain_base.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "y2016/control_loops/drivetrain/drivetrain_base.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
int main() {
::aos::Init();
DrivetrainLoop drivetrain =
- DrivetrainLoop(::y2014::control_loops::GetDrivetrainConfig());
+ DrivetrainLoop(::y2016::control_loops::GetDrivetrainConfig());
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2016/control_loops/python/BUILD b/y2016/control_loops/python/BUILD
index 84e73a5..b424625 100644
--- a/y2016/control_loops/python/BUILD
+++ b/y2016/control_loops/python/BUILD
@@ -1,4 +1,4 @@
-package(default_visibility = ['//y2014:__subpackages__'])
+package(default_visibility = ['//y2016:__subpackages__'])
py_binary(
name = 'drivetrain',
@@ -37,28 +37,3 @@
'//frc971/control_loops/python:controls',
],
)
-
-py_binary(
- name = 'claw',
- srcs = [
- 'claw.py',
- ],
- deps = [
- ':polydrivetrain_lib',
- '//external:python-gflags',
- '//external:python-glog',
- '//frc971/control_loops/python:controls',
- ]
-)
-
-py_binary(
- name = 'shooter',
- srcs = [
- 'shooter.py',
- ],
- deps = [
- '//external:python-gflags',
- '//external:python-glog',
- '//frc971/control_loops/python:controls',
- ]
-)
diff --git a/y2016/control_loops/python/drivetrain.py b/y2016/control_loops/python/drivetrain.py
index 2a93285..f67662c 100755
--- a/y2016/control_loops/python/drivetrain.py
+++ b/y2016/control_loops/python/drivetrain.py
@@ -14,6 +14,8 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+#TODO(constants): All of the constants need to be updated for 2016.
+
class CIM(control_loop.ControlLoop):
def __init__(self):
super(CIM, self).__init__("CIM")
@@ -309,7 +311,7 @@
if len(argv) != 5:
print "Expected .h file name and .cc file name"
else:
- namespaces = ['y2014', 'control_loops', 'drivetrain']
+ namespaces = ['y2016', 'control_loops', 'drivetrain']
dog_loop_writer = control_loop.ControlLoopWriter(
"Drivetrain", [drivetrain_low_low, drivetrain_low_high,
drivetrain_high_low, drivetrain_high_high],
diff --git a/y2016/control_loops/python/polydrivetrain.py b/y2016/control_loops/python/polydrivetrain.py
index 93f3884..eb0890e 100755
--- a/y2016/control_loops/python/polydrivetrain.py
+++ b/y2016/control_loops/python/polydrivetrain.py
@@ -3,7 +3,7 @@
import numpy
import sys
from frc971.control_loops.python import polytope
-from y2014.control_loops.python import drivetrain
+from y2016.control_loops.python import drivetrain
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
from matplotlib import pylab
@@ -413,7 +413,7 @@
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name')
else:
- namespaces = ['y2014', 'control_loops', 'drivetrain']
+ namespaces = ['y2016', 'control_loops', 'drivetrain']
dog_loop_writer = control_loop.ControlLoopWriter(
"VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
vdrivetrain.drivetrain_low_high,
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 137856f..c50b092 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -12,156 +12,28 @@
#include "aos/common/actions/actions.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/constants.h"
+#include "y2016/constants.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/actors/shoot_actor.h"
using ::frc971::control_loops::drivetrain_queue;
-using ::frc971::sensors::gyro_reading;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::ControlBit;
-#define OLD_DS 0
-
-namespace y2014 {
+namespace y2016 {
namespace input {
namespace joysticks {
-const ButtonLocation kDriveControlLoopEnable1(1, 7),
- kDriveControlLoopEnable2(1, 11);
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kCatch(3, 10);
-
-#if OLD_DS
-const ButtonLocation kFire(3, 11);
-const ButtonLocation kUnload(1, 4);
-const ButtonLocation kReload(1, 2);
-
-const ButtonLocation kRollersOut(3, 12);
-const ButtonLocation kRollersIn(3, 7);
-
-const ButtonLocation kTuck(3, 9);
-const ButtonLocation kIntakePosition(3, 8);
-const ButtonLocation kIntakeOpenPosition(3, 10);
-const ButtonLocation kVerticalTuck(3, 1);
-const JoystickAxis kFlipRobot(3, 3);
-
-const ButtonLocation kLongShot(3, 5);
-const ButtonLocation kCloseShot(3, 2);
-const ButtonLocation kFenderShot(3, 3);
-const ButtonLocation kTrussShot(2, 11);
-const ButtonLocation kHumanPlayerShot(3, 2);
-#else
-const ButtonLocation kFire(3, 9);
-const ButtonLocation kUnload(1, 4);
-const ButtonLocation kReload(1, 2);
-
-const ButtonLocation kRollersOut(3, 8);
-const ButtonLocation kRollersIn(3, 3);
-
-const ButtonLocation kTuck(3, 4);
-const ButtonLocation kIntakePosition(3, 5);
-const ButtonLocation kIntakeOpenPosition(3, 11);
-const ButtonLocation kVerticalTuck(2, 6);
-const JoystickAxis kFlipRobot(3, 3);
-
-const ButtonLocation kLongShot(3, 7);
-const ButtonLocation kCloseShot(3, 6);
-const ButtonLocation kFenderShot(3, 2);
-const ButtonLocation kTrussShot(2, 11);
-const ButtonLocation kHumanPlayerShot(3, 1);
-#endif
-
-const ButtonLocation kUserLeft(2, 7);
-const ButtonLocation kUserRight(2, 10);
-
-const JoystickAxis kAdjustClawGoal(3, 2);
-const JoystickAxis kAdjustClawSeparation(3, 1);
-
-struct ClawGoal {
- double angle;
- double separation;
-};
-
-struct ShotGoal {
- ClawGoal claw;
- double shot_power;
- double velocity_compensation;
- double intake_power;
-};
-
-const double kIntakePower = 4.0;
-// In case we have to quickly adjust it.
-const double kGrabSeparation = 0;
-const double kShootSeparation = 0.11 + kGrabSeparation;
-
-const ClawGoal kTuckGoal = {-2.273474, -0.749484};
-const ClawGoal kVerticalTuckGoal = {0, kGrabSeparation};
-const ClawGoal kIntakeGoal = {-2.24, kGrabSeparation};
-const ClawGoal kIntakeOpenGoal = {-2.0, 1.1};
-
-// TODO(austin): Tune these by hand...
-const ClawGoal kFlippedTuckGoal = {2.733474, -0.75};
-const ClawGoal kFlippedIntakeGoal = {2.0, kGrabSeparation};
-const ClawGoal kFlippedIntakeOpenGoal = {0.95, 1.0};
-
-// 34" between near edge of colored line and rear edge of bumper.
-// Only works running?
-const ShotGoal kLongShotGoal = {
- {-1.08, kShootSeparation}, 145, 0.04, kIntakePower};
-// old 34" {-1.06, kShootSeparation}, 140, 0.04, kIntakePower};
-const ShotGoal kFlippedLongShotGoal = {
- {0.96, kShootSeparation}, 145, 0.09, kIntakePower};
-// old 34" {0.96, kShootSeparation}, 140, 0.09, kIntakePower};
-
-// 78" between near edge of colored line and rear edge of bumper.
-const ShotGoal kCloseShotGoal = {
- {-0.95, kShootSeparation}, 105, 0.2, kIntakePower};
-// 3/4" plunger {-0.90, kShootSeparation}, 105, 0.2, kIntakePower};
-const ShotGoal kFlippedMediumShotGoal = {
- {0.865, kShootSeparation}, 120, 0.2, kIntakePower};
-// 3/4" plunger {0.80, kShootSeparation}, 105, 0.2, kIntakePower};
-
-// Shot from the fender.
-const ShotGoal kFenderShotGoal = {
- {-0.68, kShootSeparation}, 115.0, 0.0, kIntakePower};
-const ShotGoal kFlippedShortShotGoal = {
- {0.63, kShootSeparation}, 115.0, 0.0, kIntakePower};
-
-const ShotGoal kHumanShotGoal = {
- {-0.90, kShootSeparation}, 140, 0.04, kIntakePower};
-const ShotGoal kFlippedHumanShotGoal = {
- {0.90, kShootSeparation}, 140, 0, kIntakePower};
-const ShotGoal kTrussShotGoal = {
- {-0.68, kShootSeparation}, 88.0, 0.4, kIntakePower};
-const ShotGoal kFlippedTrussShotGoal = {
- {0.68, kShootSeparation}, 92.0, 0.4, kIntakePower};
-
-const ShotGoal kFlippedDemoShotGoal = {
- {1.0, kShootSeparation}, 65.0, 0.0, kIntakePower};
-const ShotGoal kDemoShotGoal = {
- {-1.0, kShootSeparation}, 50.0, 0.0, kIntakePower};
-
-const ClawGoal k254PassGoal = {-1.95, kGrabSeparation};
-const ClawGoal kFlipped254PassGoal = {1.96, kGrabSeparation};
-
class Reader : public ::aos::input::JoystickInput {
public:
Reader()
: is_high_gear_(false),
- shot_power_(80.0),
- goal_angle_(0.0),
- separation_angle_(kGrabSeparation),
- velocity_compensation_(0.0),
- intake_power_(0.0),
was_running_(false) {}
void RunIteration(const ::aos::input::driver_station::Data &data) override {
@@ -188,46 +60,7 @@
double right_goal = 0.0;
const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
- const double kThrottleGain = 1.0 / 2.5;
- if (false && (data.IsPressed(kDriveControlLoopEnable1) ||
- data.IsPressed(kDriveControlLoopEnable2))) {
- // TODO(austin): Static sucks!
- static double distance = 0.0;
- static double angle = 0.0;
- static double filtered_goal_distance = 0.0;
- if (data.PosEdge(kDriveControlLoopEnable1) ||
- data.PosEdge(kDriveControlLoopEnable2)) {
- if (drivetrain_queue.position.FetchLatest() &&
- gyro_reading.FetchLatest()) {
- distance = (drivetrain_queue.position->left_encoder +
- drivetrain_queue.position->right_encoder) /
- 2.0 -
- throttle * kThrottleGain / 2.0;
- angle = gyro_reading->angle;
- filtered_goal_distance = distance;
- }
- }
- is_control_loop_driving = true;
- // const double gyro_angle = Gyro.View().angle;
- const double goal_theta = angle - wheel * 0.27;
- const double goal_distance = distance + throttle * kThrottleGain;
- const double robot_width = 22.0 / 100.0 * 2.54;
- const double kMaxVelocity = 0.6;
- if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
- filtered_goal_distance += kMaxVelocity * 0.02;
- } else if (goal_distance <
- -kMaxVelocity * 0.02 + filtered_goal_distance) {
- filtered_goal_distance -= kMaxVelocity * 0.02;
- } else {
- filtered_goal_distance = goal_distance;
- }
- left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
- right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
- is_high_gear_ = false;
-
- LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
- }
if (!drivetrain_queue.goal.MakeWithBuilder()
.steering(wheel)
.throttle(throttle)
@@ -241,253 +74,25 @@
.Send()) {
LOG(WARNING, "sending stick values failed\n");
}
+
if (data.PosEdge(kShiftHigh)) {
is_high_gear_ = false;
}
+
if (data.PosEdge(kShiftLow)) {
is_high_gear_ = true;
}
}
- void SetGoal(ClawGoal goal) {
- goal_angle_ = goal.angle;
- separation_angle_ = goal.separation;
- moving_for_shot_ = false;
- velocity_compensation_ = 0.0;
- intake_power_ = 0.0;
- }
-
- void SetGoal(ShotGoal goal) {
- goal_angle_ = goal.claw.angle;
- shot_separation_angle_ = goal.claw.separation;
- separation_angle_ = kGrabSeparation;
- moving_for_shot_ = true;
- shot_power_ = goal.shot_power;
- velocity_compensation_ = goal.velocity_compensation;
- intake_power_ = goal.intake_power;
- }
-
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
if (!data.GetControlBit(ControlBit::kEnabled)) {
action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- }
- if (data.IsPressed(kRollersIn) || data.IsPressed(kRollersOut)) {
- intake_power_ = 0.0;
- separation_angle_ = kGrabSeparation;
- moving_for_shot_ = false;
+ LOG(DEBUG, "Canceling\n");
}
- static const double kAdjustClawGoalDeadband = 0.08;
- double claw_goal_adjust = data.GetAxis(kAdjustClawGoal);
- if (OLD_DS || ::std::abs(claw_goal_adjust) < kAdjustClawGoalDeadband) {
- claw_goal_adjust = 0;
- } else {
- claw_goal_adjust = (claw_goal_adjust -
- ((claw_goal_adjust < 0) ? -kAdjustClawGoalDeadband
- : kAdjustClawGoalDeadband)) *
- 0.035;
- }
- double claw_separation_adjust = data.GetAxis(kAdjustClawSeparation);
- if (OLD_DS ||
- ::std::abs(claw_separation_adjust) < kAdjustClawGoalDeadband) {
- claw_separation_adjust = 0;
- } else {
- claw_separation_adjust =
- (claw_separation_adjust -
- ((claw_separation_adjust < 0) ? -kAdjustClawGoalDeadband
- : kAdjustClawGoalDeadband)) *
- -0.035;
- }
-
-#if OLD_DS
- if (data.IsPressed(kFenderShot)) {
-#else
- if (data.GetAxis(kFlipRobot) > 0.9) {
-#endif
- claw_goal_adjust += claw_separation_adjust;
- claw_goal_adjust *= -1;
-
- if (data.IsPressed(kIntakeOpenPosition)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedIntakeOpenGoal);
- } else if (data.IsPressed(kIntakePosition)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedIntakeGoal);
- } else if (data.IsPressed(kVerticalTuck)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kVerticalTuckGoal);
- } else if (data.IsPressed(kTuck)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedTuckGoal);
- } else if (data.PosEdge(kLongShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedLongShotGoal);
- } else if (data.PosEdge(kCloseShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedMediumShotGoal);
- } else if (data.PosEdge(kFenderShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedShortShotGoal);
- } else if (data.PosEdge(kHumanPlayerShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedHumanShotGoal);
- } else if (data.PosEdge(kUserLeft)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlipped254PassGoal);
- } else if (data.PosEdge(kUserRight)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedDemoShotGoal);
- } else if (data.PosEdge(kTrussShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedTrussShotGoal);
- }
- } else {
- if (data.IsPressed(kIntakeOpenPosition)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kIntakeOpenGoal);
- } else if (data.IsPressed(kIntakePosition)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kIntakeGoal);
- } else if (data.IsPressed(kVerticalTuck)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kVerticalTuckGoal);
- } else if (data.IsPressed(kTuck)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kTuckGoal);
- } else if (data.PosEdge(kLongShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kLongShotGoal);
- } else if (data.PosEdge(kCloseShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kCloseShotGoal);
- } else if (data.PosEdge(kFenderShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFenderShotGoal);
- } else if (data.PosEdge(kHumanPlayerShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kHumanShotGoal);
- } else if (data.PosEdge(kUserLeft)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(k254PassGoal);
- } else if (data.PosEdge(kUserRight)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kDemoShotGoal);
- } else if (data.PosEdge(kTrussShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kTrussShotGoal);
- }
- }
-
- if (data.PosEdge(kFire)) {
- action_queue_.EnqueueAction(actors::MakeShootAction());
- } else if (data.NegEdge(kFire)) {
- action_queue_.CancelCurrentAction();
- }
-
- action_queue_.Tick();
- if (data.IsPressed(kUnload) || data.IsPressed(kReload)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- intake_power_ = 0.0;
- velocity_compensation_ = 0.0;
- }
-
- // Send out the claw and shooter goals if no actions are running.
- if (!action_queue_.Running()) {
- goal_angle_ += claw_goal_adjust;
- separation_angle_ += claw_separation_adjust;
-
- // If the action just ended, turn the intake off and stop velocity
- // compensating.
- if (was_running_) {
- intake_power_ = 0.0;
- velocity_compensation_ = 0.0;
- }
-
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
- double goal_angle = goal_angle_;
- if (::frc971::control_loops::drivetrain_queue.status.get()) {
- goal_angle += SpeedToAngleOffset(
- ::frc971::control_loops::drivetrain_queue.status->robot_speed);
- } else {
- LOG_INTERVAL(no_drivetrain_status_);
- }
-
- if (moving_for_shot_) {
- auto &claw_status = control_loops::claw_queue.status;
- claw_status.FetchLatest();
- if (claw_status.get()) {
- if (::std::abs(claw_status->bottom - goal_angle) < 0.2) {
- moving_for_shot_ = false;
- separation_angle_ = shot_separation_angle_;
- }
- }
- }
-
- double separation_angle = separation_angle_;
-
- if (data.IsPressed(kCatch)) {
- const double kCatchSeparation = 1.0;
- goal_angle -= kCatchSeparation / 2.0;
- separation_angle = kCatchSeparation;
- }
-
- bool intaking =
- data.IsPressed(kRollersIn) || data.IsPressed(kIntakePosition) ||
- data.IsPressed(kIntakeOpenPosition) || data.IsPressed(kCatch);
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(goal_angle)
- .separation_angle(separation_angle)
- .intake(intaking ? 12.0
- : (data.IsPressed(kRollersOut) ? -12.0
- : intake_power_))
- .centering(intaking ? 12.0 : 0.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
-
- if (!control_loops::shooter_queue.goal.MakeWithBuilder()
- .shot_power(shot_power_)
- .shot_requested(data.IsPressed(kFire))
- .unload_requested(data.IsPressed(kUnload))
- .load_requested(data.IsPressed(kReload))
- .Send()) {
- LOG(WARNING, "sending shooter goal failed\n");
- }
- }
was_running_ = action_queue_.Running();
}
- double SpeedToAngleOffset(double speed) {
- const ::y2014::constants::Values &values = ::y2014::constants::GetValues();
- // scale speed to a [0.0-1.0] on something close to the max
- // TODO(austin): Change the scale factor for different shots.
- return (speed / values.drivetrain_max_speed) * velocity_compensation_;
- }
-
private:
void StartAuto() {
LOG(INFO, "Starting auto mode\n");
@@ -500,14 +105,7 @@
}
bool is_high_gear_;
- double shot_power_;
- double goal_angle_;
- double separation_angle_, shot_separation_angle_;
- double velocity_compensation_;
- double intake_power_;
bool was_running_;
- bool moving_for_shot_ = false;
-
bool auto_running_ = false;
::aos::common::actions::ActionQueue action_queue_;
@@ -519,11 +117,11 @@
} // namespace joysticks
} // namespace input
-} // namespace y2014
+} // namespace y2016
int main() {
::aos::Init(-1);
- ::y2014::input::joysticks::Reader reader;
+ ::y2016::input::joysticks::Reader reader;
reader.Run();
::aos::Cleanup();
}
diff --git a/y2016/queues/BUILD b/y2016/queues/BUILD
new file mode 100644
index 0000000..3b282fd
--- /dev/null
+++ b/y2016/queues/BUILD
@@ -0,0 +1,10 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'profile_params',
+ srcs = [
+ 'profile_params.q',
+ ],
+)
diff --git a/y2016/queues/profile_params.q b/y2016/queues/profile_params.q
new file mode 100644
index 0000000..56b2ab3
--- /dev/null
+++ b/y2016/queues/profile_params.q
@@ -0,0 +1,6 @@
+package y2016;
+
+struct ProfileParams {
+ double velocity;
+ double acceleration;
+};
diff --git a/y2016/wpilib/BUILD b/y2016/wpilib/BUILD
index 120ff49..9521d74 100644
--- a/y2016/wpilib/BUILD
+++ b/y2016/wpilib/BUILD
@@ -6,15 +6,8 @@
'wpilib_interface.cc',
],
deps = [
- '//aos/linux_code:init',
'//aos/common:stl_mutex',
'//aos/common/logging',
- '//aos/externals:wpilib',
- '//y2014:constants',
- '//y2014/queues:auto_mode',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//y2014/control_loops/shooter:shooter_queue',
- '//y2014/control_loops/claw:claw_queue',
'//aos/common/controls:control_loop',
'//aos/common/util:log_interval',
'//aos/common:time',
@@ -22,6 +15,9 @@
'//aos/common/messages:robot_state',
'//aos/common/util:phased_loop',
'//aos/common/util:wrapping_counter',
+ '//aos/linux_code:init',
+ '//aos/externals:wpilib',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/wpilib:joystick_sender',
'//frc971/wpilib:loop_output_handler',
'//frc971/wpilib:buffered_pcm',
@@ -34,5 +30,6 @@
'//frc971/wpilib:logging_queue',
'//frc971/wpilib:wpilib_interface',
'//frc971/wpilib:pdp_fetcher',
+ '//y2016:constants',
],
)
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 6442a2a..7bcf0bc 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -33,10 +33,7 @@
#include "frc971/shifter_hall_effect.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/constants.h"
-#include "y2014/queues/auto_mode.q.h"
+#include "y2016/constants.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -55,10 +52,8 @@
#endif
using ::frc971::control_loops::drivetrain_queue;
-using ::y2014::control_loops::claw_queue;
-using ::y2014::control_loops::shooter_queue;
-namespace y2014 {
+namespace y2016 {
namespace wpilib {
// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
@@ -73,8 +68,7 @@
}
double drivetrain_translate(int32_t in) {
- return -static_cast<double>(in) /
- (256.0 /*cpr*/ * 4.0 /*4x*/) *
+ return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
constants::GetValues().drivetrain_encoder_ratio *
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
}
@@ -91,8 +85,9 @@
0.5 * (in_low - static_cast<float>(k.low_gear_low)) /
static_cast<float>(k.low_gear_middle - k.low_gear_low);
const float high_ratio =
- 0.5 + 0.5 * (in_high - static_cast<float>(k.high_gear_middle)) /
- static_cast<float>(k.high_gear_high - k.high_gear_middle);
+ 0.5 +
+ 0.5 * (in_high - static_cast<float>(k.high_gear_middle)) /
+ static_cast<float>(k.high_gear_high - k.high_gear_middle);
// Return low when we are below 1/2, and high when we are above 1/2.
if (low_ratio + high_ratio < 1.0) {
@@ -102,22 +97,11 @@
}
}
-double claw_translate(int32_t in) {
- return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) /
- (18.0 / 48.0 /*encoder gears*/) / (12.0 / 60.0 /*chain reduction*/) *
- (M_PI / 180.0) * 2.0;
-}
-
-double shooter_translate(int32_t in) {
- return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- 16 /*sprocket teeth*/ * 0.375 /*chain pitch*/
- * (2.54 / 100.0 /*in to m*/);
-}
-
+// TODO(constants): Update.
static const double kMaximumEncoderPulsesPerSecond =
5600.0 /* free speed RPM */ * 14.0 / 48.0 /* bottom gear reduction */ *
- 18.0 / 32.0 /* big belt reduction */ *
- 18.0 / 66.0 /* top gear reduction */ * 48.0 / 18.0 /* encoder gears */ /
+ 18.0 / 32.0 /* big belt reduction */ * 18.0 /
+ 66.0 /* top gear reduction */ * 48.0 / 18.0 /* encoder gears */ /
60.0 /* seconds / minute */ * 256.0 /* CPR */;
class SensorReader {
@@ -130,10 +114,6 @@
hall_filter_.SetPeriodNanoSeconds(100000);
}
- void set_auto_selector_analog(::std::unique_ptr<AnalogInput> analog) {
- auto_selector_analog_ = ::std::move(analog);
- }
-
void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
drivetrain_left_encoder_ = ::std::move(encoder);
drivetrain_left_encoder_->SetMaxPeriod(0.005);
@@ -160,89 +140,13 @@
low_left_drive_hall_ = ::std::move(analog);
}
- void set_top_claw_encoder(::std::unique_ptr<Encoder> encoder) {
- encoder_filter_.Add(encoder.get());
- top_reader_.set_encoder(::std::move(encoder));
- }
-
- void set_top_claw_front_hall(::std::unique_ptr<DigitalInput> hall) {
- hall_filter_.Add(hall.get());
- top_reader_.set_front_hall(::std::move(hall));
- }
-
- void set_top_claw_calibration_hall(::std::unique_ptr<DigitalInput> hall) {
- hall_filter_.Add(hall.get());
- top_reader_.set_calibration_hall(::std::move(hall));
- }
-
- void set_top_claw_back_hall(::std::unique_ptr<DigitalInput> hall) {
- hall_filter_.Add(hall.get());
- top_reader_.set_back_hall(::std::move(hall));
- }
-
- void set_bottom_claw_encoder(::std::unique_ptr<Encoder> encoder) {
- encoder_filter_.Add(encoder.get());
- bottom_reader_.set_encoder(::std::move(encoder));
- }
-
- void set_bottom_claw_front_hall(::std::unique_ptr<DigitalInput> hall) {
- hall_filter_.Add(hall.get());
- bottom_reader_.set_front_hall(::std::move(hall));
- }
-
- void set_bottom_claw_calibration_hall(::std::unique_ptr<DigitalInput> hall) {
- hall_filter_.Add(hall.get());
- bottom_reader_.set_calibration_hall(::std::move(hall));
- }
-
- void set_bottom_claw_back_hall(::std::unique_ptr<DigitalInput> hall) {
- hall_filter_.Add(hall.get());
- bottom_reader_.set_back_hall(::std::move(hall));
- }
-
- void set_shooter_encoder(::std::unique_ptr<Encoder> encoder) {
- encoder_filter_.Add(encoder.get());
- shooter_encoder_ = ::std::move(encoder);
- }
-
- void set_shooter_proximal(::std::unique_ptr<DigitalInput> hall) {
- hall_filter_.Add(hall.get());
- shooter_proximal_ = ::std::move(hall);
- }
-
- void set_shooter_distal(::std::unique_ptr<DigitalInput> hall) {
- hall_filter_.Add(hall.get());
- shooter_distal_ = ::std::move(hall);
- }
-
- void set_shooter_plunger(::std::unique_ptr<DigitalInput> hall) {
- hall_filter_.Add(hall.get());
- shooter_plunger_ = ::std::move(hall);
- shooter_plunger_reader_ =
- make_unique<::frc971::wpilib::DMADigitalReader>(shooter_plunger_.get());
- }
-
- void set_shooter_latch(::std::unique_ptr<DigitalInput> hall) {
- hall_filter_.Add(hall.get());
- shooter_latch_ = ::std::move(hall);
- shooter_latch_reader_ =
- make_unique<::frc971::wpilib::DMADigitalReader>(shooter_latch_.get());
- }
-
// All of the DMA-related set_* calls must be made before this, and it doesn't
// hurt to do all of them.
- void set_dma(::std::unique_ptr<DMA> dma) {
- shooter_proximal_counter_ = make_unique<::frc971::wpilib::DMAEdgeCounter>(
- shooter_encoder_.get(), shooter_proximal_.get());
- shooter_distal_counter_ = make_unique<::frc971::wpilib::DMAEdgeCounter>(
- shooter_encoder_.get(), shooter_distal_.get());
+ // TODO(comran): Add 2016 things down below for dma synchronization.
+ void set_dma(::std::unique_ptr<DMA> dma) {
dma_synchronizer_.reset(
new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
- dma_synchronizer_->Add(shooter_proximal_counter_.get());
- dma_synchronizer_->Add(shooter_distal_counter_.get());
- dma_synchronizer_->Add(shooter_plunger_reader_.get());
- dma_synchronizer_->Add(shooter_latch_reader_.get());
}
void operator()() {
@@ -256,8 +160,6 @@
&DriverStation::GetInstance();
#endif
- top_reader_.Start();
- bottom_reader_.Start();
dma_synchronizer_->Start();
::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
@@ -273,9 +175,6 @@
}
RunIteration();
}
-
- top_reader_.Quit();
- bottom_reader_.Quit();
}
void RunIteration() {
@@ -310,140 +209,17 @@
drivetrain_message.Send();
}
- ::y2014::sensors::auto_mode.MakeWithBuilder()
- .voltage(auto_selector_analog_->GetVoltage())
- .Send();
-
dma_synchronizer_->RunIteration();
-
- {
- auto shooter_message = shooter_queue.position.MakeMessage();
- shooter_message->position = shooter_translate(shooter_encoder_->GetRaw());
- shooter_message->plunger = !shooter_plunger_reader_->value();
- shooter_message->latch = !shooter_latch_reader_->value();
- CopyShooterPosedgeCounts(shooter_proximal_counter_.get(),
- &shooter_message->pusher_proximal);
- CopyShooterPosedgeCounts(shooter_distal_counter_.get(),
- &shooter_message->pusher_distal);
-
- shooter_message.Send();
- }
-
- {
- auto claw_message = claw_queue.position.MakeMessage();
- top_reader_.RunIteration(&claw_message->top);
- bottom_reader_.RunIteration(&claw_message->bottom);
-
- claw_message.Send();
- }
}
void Quit() { run_ = false; }
private:
- class HalfClawReader {
- public:
- HalfClawReader(bool reversed) : reversed_(reversed) {}
-
- void set_encoder(::std::unique_ptr<Encoder> encoder) {
- encoder_ = ::std::move(encoder);
- }
-
- void set_front_hall(::std::unique_ptr<DigitalInput> front_hall) {
- front_hall_ = ::std::move(front_hall);
- }
-
- void set_calibration_hall(
- ::std::unique_ptr<DigitalInput> calibration_hall) {
- calibration_hall_ = ::std::move(calibration_hall);
- }
-
- void set_back_hall(::std::unique_ptr<DigitalInput> back_hall) {
- back_hall_ = ::std::move(back_hall);
- }
-
- void Start() {
- front_counter_ = make_unique<::frc971::wpilib::EdgeCounter>(
- encoder_.get(), front_hall_.get());
- synchronizer_.Add(front_counter_.get());
- calibration_counter_ = make_unique<::frc971::wpilib::EdgeCounter>(
- encoder_.get(), calibration_hall_.get());
- synchronizer_.Add(calibration_counter_.get());
- back_counter_ = make_unique<::frc971::wpilib::EdgeCounter>(
- encoder_.get(), back_hall_.get());
- synchronizer_.Add(back_counter_.get());
- synchronized_encoder_ =
- make_unique<::frc971::wpilib::InterruptSynchronizedEncoder>(
- encoder_.get());
- synchronizer_.Add(synchronized_encoder_.get());
-
- synchronizer_.Start();
- }
-
- void Quit() { synchronizer_.Quit(); }
-
- void RunIteration(control_loops::HalfClawPosition *half_claw_position) {
- const double multiplier = reversed_ ? -1.0 : 1.0;
-
- synchronizer_.RunIteration();
-
- CopyPosition(front_counter_.get(), &half_claw_position->front);
- CopyPosition(calibration_counter_.get(),
- &half_claw_position->calibration);
- CopyPosition(back_counter_.get(), &half_claw_position->back);
- half_claw_position->position =
- multiplier * claw_translate(synchronized_encoder_->get());
- }
-
- private:
- void CopyPosition(const ::frc971::wpilib::EdgeCounter *counter,
- ::frc971::HallEffectStruct *out) {
- const double multiplier = reversed_ ? -1.0 : 1.0;
-
- out->current = !counter->polled_value();
- out->posedge_count = counter->negative_interrupt_count();
- out->negedge_count = counter->positive_interrupt_count();
- out->negedge_value =
- multiplier * claw_translate(counter->last_positive_encoder_value());
- out->posedge_value =
- multiplier * claw_translate(counter->last_negative_encoder_value());
- }
-
- ::frc971::wpilib::InterruptSynchronizer synchronizer_{55};
-
- ::std::unique_ptr<::frc971::wpilib::EdgeCounter> front_counter_;
- ::std::unique_ptr<::frc971::wpilib::EdgeCounter> calibration_counter_;
- ::std::unique_ptr<::frc971::wpilib::EdgeCounter> back_counter_;
- ::std::unique_ptr<::frc971::wpilib::InterruptSynchronizedEncoder>
- synchronized_encoder_;
-
- ::std::unique_ptr<Encoder> encoder_;
- ::std::unique_ptr<DigitalInput> front_hall_;
- ::std::unique_ptr<DigitalInput> calibration_hall_;
- ::std::unique_ptr<DigitalInput> back_hall_;
-
- const bool reversed_;
- };
-
- void CopyShooterPosedgeCounts(
- const ::frc971::wpilib::DMAEdgeCounter *counter,
- ::frc971::PosedgeOnlyCountedHallEffectStruct *output) {
- output->current = !counter->polled_value();
- // These are inverted because the hall effects give logical false when
- // there's a magnet in front of them.
- output->posedge_count = counter->negative_count();
- output->negedge_count = counter->positive_count();
- output->posedge_value =
- shooter_translate(counter->last_negative_encoder_value());
- }
-
int32_t my_pid_;
DriverStation *ds_;
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
- ::std::unique_ptr<AnalogInput> auto_selector_analog_;
-
::std::unique_ptr<Encoder> drivetrain_left_encoder_;
::std::unique_ptr<Encoder> drivetrain_right_encoder_;
::std::unique_ptr<AnalogInput> low_left_drive_hall_;
@@ -451,16 +227,6 @@
::std::unique_ptr<AnalogInput> low_right_drive_hall_;
::std::unique_ptr<AnalogInput> high_right_drive_hall_;
- HalfClawReader top_reader_{false}, bottom_reader_{true};
-
- ::std::unique_ptr<Encoder> shooter_encoder_;
- ::std::unique_ptr<DigitalInput> shooter_proximal_, shooter_distal_;
- ::std::unique_ptr<DigitalInput> shooter_plunger_, shooter_latch_;
- ::std::unique_ptr<::frc971::wpilib::DMAEdgeCounter> shooter_proximal_counter_,
- shooter_distal_counter_;
- ::std::unique_ptr<::frc971::wpilib::DMADigitalReader> shooter_plunger_reader_,
- shooter_latch_reader_;
-
::std::atomic<bool> run_{true};
DigitalGlitchFilter encoder_filter_, hall_filter_;
};
@@ -469,7 +235,6 @@
public:
SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
: pcm_(pcm),
- shooter_(".y2014.control_loops.shooter_queue.output"),
drivetrain_(".frc971.control_loops.drivetrain_queue.output") {}
void set_pressure_switch(::std::unique_ptr<DigitalInput> pressure_switch) {
@@ -490,16 +255,6 @@
drivetrain_right_ = ::std::move(s);
}
- void set_shooter_latch(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- shooter_latch_ = ::std::move(s);
- }
-
- void set_shooter_brake(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- shooter_brake_ = ::std::move(s);
- }
-
void operator()() {
::aos::SetCurrentThreadName("Solenoids");
::aos::SetCurrentThreadRealtimePriority(27);
@@ -516,15 +271,6 @@
}
{
- shooter_.FetchLatest();
- if (shooter_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *shooter_);
- shooter_latch_->Set(!shooter_->latch_piston);
- shooter_brake_->Set(!shooter_->brake_piston);
- }
- }
-
- {
drivetrain_.FetchLatest();
if (drivetrain_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
@@ -559,13 +305,10 @@
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_left_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_right_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> shooter_latch_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> shooter_brake_;
::std::unique_ptr<DigitalInput> pressure_switch_;
::std::unique_ptr<Relay> compressor_relay_;
- ::aos::Queue<::y2014::control_loops::ShooterQueue::Output> shooter_;
::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
::std::atomic<bool> run_{true};
@@ -603,91 +346,6 @@
::std::unique_ptr<Talon> right_drivetrain_talon_;
};
-class ShooterWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_shooter_talon(::std::unique_ptr<Talon> t) {
- shooter_talon_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::y2014::control_loops::shooter_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2014::control_loops::shooter_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- shooter_talon_->Set(queue->voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "shooter output too old\n");
- shooter_talon_->Disable();
- }
-
- ::std::unique_ptr<Talon> shooter_talon_;
-};
-
-class ClawWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_top_claw_talon(::std::unique_ptr<Talon> t) {
- top_claw_talon_ = ::std::move(t);
- }
-
- void set_bottom_claw_talon(::std::unique_ptr<Talon> t) {
- bottom_claw_talon_ = ::std::move(t);
- }
-
- void set_left_tusk_talon(::std::unique_ptr<Talon> t) {
- left_tusk_talon_ = ::std::move(t);
- }
-
- void set_right_tusk_talon(::std::unique_ptr<Talon> t) {
- right_tusk_talon_ = ::std::move(t);
- }
-
- void set_intake1_talon(::std::unique_ptr<Talon> t) {
- intake1_talon_ = ::std::move(t);
- }
-
- void set_intake2_talon(::std::unique_ptr<Talon> t) {
- intake2_talon_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::y2014::control_loops::claw_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2014::control_loops::claw_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- intake1_talon_->Set(queue->intake_voltage / 12.0);
- intake2_talon_->Set(queue->intake_voltage / 12.0);
- bottom_claw_talon_->Set(-queue->bottom_claw_voltage / 12.0);
- top_claw_talon_->Set(queue->top_claw_voltage / 12.0);
- left_tusk_talon_->Set(queue->tusk_voltage / 12.0);
- right_tusk_talon_->Set(-queue->tusk_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "claw output too old\n");
- intake1_talon_->Disable();
- intake2_talon_->Disable();
- bottom_claw_talon_->Disable();
- top_claw_talon_->Disable();
- left_tusk_talon_->Disable();
- right_tusk_talon_->Disable();
- }
-
- ::std::unique_ptr<Talon> top_claw_talon_;
- ::std::unique_ptr<Talon> bottom_claw_talon_;
- ::std::unique_ptr<Talon> left_tusk_talon_;
- ::std::unique_ptr<Talon> right_tusk_talon_;
- ::std::unique_ptr<Talon> intake1_talon_;
- ::std::unique_ptr<Talon> intake2_talon_;
-};
-
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
::std::unique_ptr<Encoder> make_encoder(int index) {
@@ -706,12 +364,7 @@
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
SensorReader reader;
- // Create this first to make sure it ends up in one of the lower-numbered
- // FPGA slots so we can use it with DMA.
- auto shooter_encoder_temp = make_encoder(2);
-
- reader.set_auto_selector_analog(make_unique<AnalogInput>(4));
-
+ // TODO(constants): Update these input numbers.
reader.set_drivetrain_left_encoder(make_encoder(0));
reader.set_drivetrain_right_encoder(make_encoder(1));
reader.set_high_left_drive_hall(make_unique<AnalogInput>(1));
@@ -719,22 +372,6 @@
reader.set_high_right_drive_hall(make_unique<AnalogInput>(2));
reader.set_low_right_drive_hall(make_unique<AnalogInput>(3));
- reader.set_top_claw_encoder(make_encoder(3));
- reader.set_top_claw_front_hall(make_unique<DigitalInput>(4)); // R2
- reader.set_top_claw_calibration_hall(make_unique<DigitalInput>(3)); // R3
- reader.set_top_claw_back_hall(make_unique<DigitalInput>(5)); // R1
-
- reader.set_bottom_claw_encoder(make_encoder(4));
- reader.set_bottom_claw_front_hall(make_unique<DigitalInput>(1)); // L2
- reader.set_bottom_claw_calibration_hall(make_unique<DigitalInput>(0)); // L3
- reader.set_bottom_claw_back_hall(make_unique<DigitalInput>(2)); // L1
-
- reader.set_shooter_encoder(::std::move(shooter_encoder_temp));
- reader.set_shooter_proximal(make_unique<DigitalInput>(6)); // S1
- reader.set_shooter_distal(make_unique<DigitalInput>(7)); // S2
- reader.set_shooter_plunger(make_unique<DigitalInput>(8)); // S3
- reader.set_shooter_latch(make_unique<DigitalInput>(9)); // S4
-
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
@@ -748,26 +385,11 @@
::std::unique_ptr<Talon>(new Talon(2)));
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- ::y2014::wpilib::ClawWriter claw_writer;
- claw_writer.set_top_claw_talon(::std::unique_ptr<Talon>(new Talon(1)));
- claw_writer.set_bottom_claw_talon(::std::unique_ptr<Talon>(new Talon(0)));
- claw_writer.set_left_tusk_talon(::std::unique_ptr<Talon>(new Talon(4)));
- claw_writer.set_right_tusk_talon(::std::unique_ptr<Talon>(new Talon(3)));
- claw_writer.set_intake1_talon(::std::unique_ptr<Talon>(new Talon(7)));
- claw_writer.set_intake2_talon(::std::unique_ptr<Talon>(new Talon(8)));
- ::std::thread claw_writer_thread(::std::ref(claw_writer));
-
- ::y2014::wpilib::ShooterWriter shooter_writer;
- shooter_writer.set_shooter_talon(::std::unique_ptr<Talon>(new Talon(6)));
- ::std::thread shooter_writer_thread(::std::ref(shooter_writer));
-
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(pcm);
solenoid_writer.set_drivetrain_left(pcm->MakeSolenoid(6));
solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(7));
- solenoid_writer.set_shooter_latch(pcm->MakeSolenoid(5));
- solenoid_writer.set_shooter_brake(pcm->MakeSolenoid(4));
solenoid_writer.set_pressure_switch(make_unique<DigitalInput>(25));
solenoid_writer.set_compressor_relay(make_unique<Relay>(0));
@@ -796,10 +418,6 @@
drivetrain_writer.Quit();
drivetrain_writer_thread.join();
- shooter_writer.Quit();
- shooter_writer_thread.join();
- claw_writer.Quit();
- claw_writer_thread.join();
solenoid_writer.Quit();
solenoid_thread.join();
@@ -808,7 +426,6 @@
};
} // namespace wpilib
-} // namespace y2014
+} // namespace y2016
-
-AOS_ROBOT_CLASS(::y2014::wpilib::WPILibRobot);
+AOS_ROBOT_CLASS(::y2016::wpilib::WPILibRobot);