Switched joysticks over to ready and then shoot, wired up catch action, wrote auto.
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
index b2db6fd..676da44 100644
--- a/frc971/actions/action.h
+++ b/frc971/actions/action.h
@@ -1,3 +1,6 @@
+#ifndef FRC971_ACTIONS_ACTION_H_
+#define FRC971_ACTIONS_ACTION_H_
+
#include <stdio.h>
#include <functional>
@@ -93,3 +96,4 @@
} // namespace actions
} // namespace frc971
+#endif // FRC971_ACTIONS_ACTION_H_
diff --git a/frc971/actions/action_client.h b/frc971/actions/action_client.h
new file mode 100644
index 0000000..2a7d65f
--- /dev/null
+++ b/frc971/actions/action_client.h
@@ -0,0 +1,117 @@
+#ifndef FRC971_ACTIONS_ACTION_CLIENT_H_
+#define FRC971_ACTIONS_ACTION_CLIENT_H_
+
+#include <type_traits>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/queue.h"
+
+namespace frc971 {
+
+class Action {
+ public:
+ // Cancels the action.
+ void Cancel() { DoCancel(); }
+ // Returns true if the action is currently running.
+ bool Running() { return DoRunning(); }
+ // Starts the action.
+ void Start() { DoStart(); }
+
+ // Waits until the action has finished.
+ void WaitUntilDone() { DoWaitUntilDone(); }
+
+ virtual ~Action() {}
+
+ private:
+ virtual void DoCancel() = 0;
+ virtual bool DoRunning() = 0;
+ virtual void DoStart() = 0;
+ virtual void DoWaitUntilDone() = 0;
+};
+
+// Templated subclass to hold the type information.
+template <typename T>
+class TypedAction : public Action {
+ public:
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+
+ TypedAction(T *queue_group)
+ : queue_group_(queue_group),
+ goal_(queue_group_->goal.MakeMessage()),
+ has_started_(false) {}
+
+ // Returns the current goal that will be sent when the action is sent.
+ GoalType *GetGoal() { return goal_.get(); }
+
+ virtual ~TypedAction() {
+ LOG(INFO, "Calling destructor\n");
+ DoCancel();
+ }
+
+ private:
+ // Cancels the action.
+ virtual void DoCancel() {
+ LOG(INFO, "Canceling action on queue %s\n", queue_group_->goal.name());
+ queue_group_->goal.MakeWithBuilder().run(false).Send();
+ }
+
+ // Returns true if the action is running or we don't have an initial response
+ // back from it to signal whether or not it is running.
+ virtual bool DoRunning() {
+ if (has_started_) {
+ queue_group_->status.FetchLatest();
+ } else if (queue_group_->status.FetchLatest()) {
+ if (queue_group_->status->running) {
+ // Wait until it reports that it is running to start.
+ has_started_ = true;
+ }
+ }
+ return !has_started_ ||
+ (queue_group_->status.get() && queue_group_->status->running);
+ }
+
+ // Returns true if the action is running or we don't have an initial response
+ // back from it to signal whether or not it is running.
+ virtual void DoWaitUntilDone() {
+ queue_group_->status.FetchLatest();
+ while (true) {
+ if (has_started_) {
+ queue_group_->status.FetchNextBlocking();
+ } else {
+ queue_group_->status.FetchNextBlocking();
+ if (queue_group_->status->running) {
+ // Wait until it reports that it is running to start.
+ has_started_ = true;
+ }
+ }
+ if (has_started_ &&
+ (queue_group_->status.get() && !queue_group_->status->running)) {
+ return;
+ }
+ }
+ }
+
+ // Starts the action if a goal has been created.
+ virtual void DoStart() {
+ if (goal_) {
+ goal_->run = true;
+ goal_.Send();
+ has_started_ = false;
+ LOG(INFO, "Starting action\n");
+ } else {
+ has_started_ = true;
+ }
+ }
+
+ T *queue_group_;
+ ::aos::ScopedMessagePtr<GoalType> goal_;
+ // Track if we have seen a response to the start message.
+ // If we haven't, we are considered running regardless.
+ bool has_started_;
+};
+
+} // namespace frc971
+
+#endif // FRC971_ACTIONS_ACTION_CLIENT_H_
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
index c38c65b..7cfb7a5 100644
--- a/frc971/actions/actions.gyp
+++ b/frc971/actions/actions.gyp
@@ -1,6 +1,21 @@
{
'targets': [
{
+ 'target_name': 'action_client',
+ 'type': 'static_library',
+ 'sources': [
+ #'action_client.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ },
+ {
'target_name': 'shoot_action_queue',
'type': 'static_library',
'sources': ['shoot_action.q'],
@@ -15,6 +30,21 @@
],
'includes': ['../../aos/build/queues.gypi'],
},
+ {
+ 'target_name': 'drivetrain_action_queue',
+ 'type': 'static_library',
+ 'sources': ['drivetrain_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
{
'target_name': 'selfcatch_action_queue',
'type': 'static_library',
@@ -46,20 +76,6 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
- 'target_name': 'action',
- 'type': 'static_library',
- 'dependencies': [
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(AOS)/common/common.gyp:timing',
- '<(AOS)/build/aos.gyp:logging',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(AOS)/common/common.gyp:timing',
- '<(AOS)/build/aos.gyp:logging',
- ]
- },
- {
'target_name': 'shoot_action_lib',
'type': 'static_library',
'sources': [
@@ -74,6 +90,56 @@
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ 'action_client',
+ 'action',
+ ],
+ 'export_dependent_settings': [
+ 'action',
+ 'shoot_action_queue',
+ 'action_client',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain_action.cc',
+ ],
+ 'dependencies': [
+ 'drivetrain_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ 'action_client',
+ 'action',
+ '<(EXTERNALS):eigen',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ ],
+ 'export_dependent_settings': [
+ 'action',
+ 'drivetrain_action_queue',
+ 'action_client',
+ ],
+ },
+ {
+ 'target_name': 'action',
+ 'type': 'static_library',
+ 'sources': [
+ #'action.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/network/network.gyp:team_number',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/network/network.gyp:team_number',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
],
},
{
@@ -120,7 +186,20 @@
'<(AOS)/linux_code/linux_code.gyp:init',
'shoot_action_queue',
'shoot_action_lib',
- 'action',
+ 'action',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'drivetrain_action_queue',
+ 'drivetrain_action_lib',
+ 'action',
],
},
{
@@ -133,7 +212,7 @@
'<(AOS)/linux_code/linux_code.gyp:init',
'selfcatch_action_queue',
'selfcatch_action_lib',
- 'action',
+ 'action',
],
},
{
diff --git a/frc971/actions/catch_action.cc b/frc971/actions/catch_action.cc
index deb9ad7..65f0487 100644
--- a/frc971/actions/catch_action.cc
+++ b/frc971/actions/catch_action.cc
@@ -14,113 +14,113 @@
: actions::ActionBase<actions::CatchActionGroup>(s) {}
void CatchAction::RunAction() {
-
// Set claw angle.
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- catch_action.goal->catch_angle)
- .separation_angle(kCatchSeparation).intake(kCatchIntake)
- .centering(kCatchCentering).Send()) {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(action_q_->goal->catch_angle)
+ .separation_angle(kCatchSeparation)
+ .intake(kCatchIntake)
+ .centering(kCatchCentering)
+ .Send()) {
LOG(WARNING, "sending claw goal failed\n");
return;
}
+ LOG(INFO, "Waiting for the claw to be ready\n");
// wait for claw to be ready
if (WaitUntil(::std::bind(&CatchAction::DoneSetupCatch, this))) return;
-
- // Set claw angle.
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- catch_action.goal->catch_angle).separation_angle(kCatchSeparation)
- .intake(kCatchIntake).centering(kCatchCentering).Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- return;
- }
+ LOG(INFO, "Waiting for the sonar\n");
// wait for the sonar to trigger
if (WaitUntil(::std::bind(&CatchAction::DoneFoundSonar, this))) return;
+ LOG(INFO, "Closing the claw\n");
+
// close the claw
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- kFinishAngle).separation_angle(0.0).intake(kCatchIntake)
- .centering(kCatchCentering).Send()) {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(action_q_->goal->catch_angle + kCatchSeparation / 2.0)
+ .separation_angle(0.0)
+ .intake(kCatchIntake)
+ .centering(kCatchCentering)
+ .Send()) {
LOG(WARNING, "sending claw goal failed\n");
return;
}
// claw now closed
if (WaitUntil(::std::bind(&CatchAction::DoneClawWithBall, this))) return;
- // ball is fully in
- if (WaitUntil(::std::bind(&CatchAction::DoneBallIn, this))) return;
- // head to a finshed pose
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- kFinishAngle)
- .separation_angle(0.0).intake(0.0).centering(0.0).Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- return;
- }
-
- // thats it
- if (WaitUntil(::std::bind(&CatchAction::DoneClawWithBall, this))) return;
-
- // done with action
return;
}
-bool CatchAction::DoneBallIn() {
+/*bool CatchAction::DoneBallIn() {
if (!sensors::othersensors.FetchLatest()) {
- sensors::othersensors.FetchNextBlocking();
+ sensors::othersensors.FetchNextBlocking();
}
if (sensors::othersensors->travis_hall_effect_distance > 0.005) {
LOG(INFO, "Ball in at %.2f.\n",
sensors::othersensors->travis_hall_effect_distance);
- return true;
- }
- return false;
-}
-
-bool CatchAction::DoneClawWithBall() {
- if (!control_loops::claw_queue_group.status.FetchLatest()) {
- control_loops::claw_queue_group.status.FetchNextBlocking();
- }
- // Make sure that both the shooter and claw have reached the necessary
- // states.
- if (control_loops::claw_queue_group.status->done_with_ball) {
- LOG(INFO, "Claw at goal.\n");
return true;
}
return false;
+}*/
+
+bool CatchAction::DoneClawWithBall() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+
+ bool ans =
+ control_loops::claw_queue_group.status->zeroed &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom_velocity) <
+ 0.5) &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) < 0.10) &&
+ (::std::abs(control_loops::claw_queue_group.status->separation -
+ control_loops::claw_queue_group.goal->separation_angle) <
+ 0.4);
+
+ if (!ans) {
+ LOG(INFO,
+ "Claw is ready %d zeroed %d bottom_velocity %f bottom %f sep %f\n", ans,
+ control_loops::claw_queue_group.status->zeroed,
+ ::std::abs(control_loops::claw_queue_group.status->bottom_velocity),
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle),
+ ::std::abs(control_loops::claw_queue_group.status->separation -
+ control_loops::claw_queue_group.goal->separation_angle));
+ }
+ return ans;
}
bool CatchAction::DoneFoundSonar() {
if (!sensors::othersensors.FetchLatest()) {
- sensors::othersensors.FetchNextBlocking();
+ sensors::othersensors.FetchNextBlocking();
}
+ LOG(INFO, "Sonar at %.2f.\n", sensors::othersensors->sonar_distance);
if (sensors::othersensors->sonar_distance > 0.3 &&
sensors::othersensors->sonar_distance < kSonarTriggerDist) {
- LOG(INFO, "Hit Sonar at %.2f.\n", sensors::othersensors->sonar_distance);
- return true;
+ return true;
}
return false;
}
bool CatchAction::DoneSetupCatch() {
if (!control_loops::claw_queue_group.status.FetchLatest()) {
- control_loops::claw_queue_group.status.FetchNextBlocking();
+ control_loops::claw_queue_group.status.FetchNextBlocking();
}
- if (!control_loops::claw_queue_group.goal.FetchLatest()) {
- LOG(ERROR, "Failed to fetch claw goal.\n");
- }
+
// Make sure that the shooter and claw has reached the necessary state.
// Check the current positions of the various mechanisms to make sure that we
// avoid race conditions where we send it a new goal but it still thinks that
// it has the old goal and thinks that it is already done.
bool claw_angle_correct =
::std::abs(control_loops::claw_queue_group.status->bottom -
- control_loops::claw_queue_group.goal->bottom_angle) <
- 0.005;
+ action_q_->goal->catch_angle) < 0.15;
+ bool open_enough =
+ control_loops::claw_queue_group.status->separation > kCatchMinSeparation;
- if (control_loops::claw_queue_group.status->done && claw_angle_correct) {
+ if (claw_angle_correct && open_enough) {
LOG(INFO, "Claw ready for catching.\n");
return true;
}
diff --git a/frc971/actions/catch_action.h b/frc971/actions/catch_action.h
index bb875f8..cdecef0 100644
--- a/frc971/actions/catch_action.h
+++ b/frc971/actions/catch_action.h
@@ -12,9 +12,10 @@
// Actually executes the action of moving the claw into position and closing
// it.
- void RunAction();
+ virtual void RunAction();
static constexpr double kCatchSeparation = 1.0;
+ static constexpr double kCatchMinSeparation = 0.6;
static constexpr double kCatchIntake = 12.0;
static constexpr double kSonarTriggerDist = 0.8;
static constexpr double kCatchCentering = 12.0;
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
new file mode 100644
index 0000000..fb16185
--- /dev/null
+++ b/frc971/actions/drivetrain_action.cc
@@ -0,0 +1,65 @@
+#include <functional>
+#include <numeric>
+
+#include <Eigen/Dense>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/trapezoid_profile.h"
+
+#include "frc971/actions/drivetrain_action.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+namespace frc971 {
+namespace actions {
+
+DrivetrainAction::DrivetrainAction(actions::DrivetrainActionQueueGroup* s)
+ : actions::ActionBase<actions::DrivetrainActionQueueGroup>(s) {}
+
+void DrivetrainAction::RunAction() {
+ const double yoffset = action_q_->goal->y_offset;
+ LOG(INFO, "Going to move %f\n", yoffset);
+
+ // Measured conversion to get the distance right.
+ ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
+ ::Eigen::Matrix<double, 2, 1> profile_goal_state;
+ const double goal_velocity = 0.0;
+ const double epsilon = 0.01;
+
+ profile.set_maximum_acceleration(2.0);
+ profile.set_maximum_velocity(action_q_->goal->maximum_velocity);
+
+ while (true) {
+ // wait until next 10ms tick
+ ::aos::time::PhasedLoop10MS(5000);
+ profile_goal_state = profile.Update(yoffset, goal_velocity);
+
+ if (::std::abs(profile_goal_state(0, 0) - yoffset) < epsilon) break;
+ if (ShouldCancel()) return;
+
+ LOG(DEBUG, "Driving left to %f, right to %f\n",
+ profile_goal_state(0, 0) + action_q_->goal->left_initial_position,
+ profile_goal_state(0, 0) + action_q_->goal->right_initial_position);
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .highgear(false)
+ .left_goal(profile_goal_state(0, 0) + action_q_->goal->left_initial_position)
+ .right_goal(profile_goal_state(0, 0) + action_q_->goal->right_initial_position)
+ .left_velocity_goal(profile_goal_state(1, 0))
+ .right_velocity_goal(profile_goal_state(1, 0))
+ .Send();
+ }
+ LOG(INFO, "Done moving\n");
+}
+
+::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+MakeDrivetrainAction() {
+ return ::std::unique_ptr<
+ TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>(
+ new TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>(
+ &::frc971::actions::drivetrain_action));
+}
+
+} // namespace actions
+} // namespace frc971
diff --git a/frc971/actions/drivetrain_action.h b/frc971/actions/drivetrain_action.h
new file mode 100644
index 0000000..b164e22
--- /dev/null
+++ b/frc971/actions/drivetrain_action.h
@@ -0,0 +1,22 @@
+#include <memory>
+
+#include "frc971/actions/drivetrain_action.q.h"
+#include "frc971/actions/action.h"
+#include "frc971/actions/action_client.h"
+
+namespace frc971 {
+namespace actions {
+
+class DrivetrainAction : public ActionBase<actions::DrivetrainActionQueueGroup> {
+ public:
+ explicit DrivetrainAction(actions::DrivetrainActionQueueGroup* s);
+
+ virtual void RunAction();
+};
+
+// Makes a new DrivetrainAction action.
+::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+MakeDrivetrainAction();
+
+} // namespace actions
+} // namespace frc971
diff --git a/frc971/actions/drivetrain_action.q b/frc971/actions/drivetrain_action.q
new file mode 100644
index 0000000..cde518c
--- /dev/null
+++ b/frc971/actions/drivetrain_action.q
@@ -0,0 +1,22 @@
+package frc971.actions;
+
+queue_group DrivetrainActionQueueGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run;
+ double left_initial_position;
+ double right_initial_position;
+ double y_offset;
+ double maximum_velocity;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/frc971/actions/drivetrain_action_main.cc b/frc971/actions/drivetrain_action_main.cc
new file mode 100644
index 0000000..0251e25
--- /dev/null
+++ b/frc971/actions/drivetrain_action_main.cc
@@ -0,0 +1,19 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/actions/drivetrain_action.q.h"
+#include "frc971/actions/drivetrain_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actions::DrivetrainAction drivetrain(&::frc971::actions::drivetrain_action);
+ drivetrain.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/actions/selfcatch_action.h b/frc971/actions/selfcatch_action.h
index 0cb5938..c57d24a 100644
--- a/frc971/actions/selfcatch_action.h
+++ b/frc971/actions/selfcatch_action.h
@@ -12,7 +12,7 @@
// Actually execute the action of moving the claw and shooter into position
// and actually firing them.
- void RunAction();
+ virtual void RunAction();
// calc an offset to our requested shot based on robot speed
double SpeedToAngleOffset(double speed);
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
index 696481e..5a11582 100644
--- a/frc971/actions/shoot_action.cc
+++ b/frc971/actions/shoot_action.cc
@@ -46,7 +46,7 @@
// wait for record of shot having been fired
if (WaitUntil(::std::bind(&ShootAction::DoneShot, this))) return;
- // Open up the claw in preparation for shooting.
+ // Turn the intake off.
control_loops::claw_queue_group.goal.FetchLatest();
if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
@@ -62,8 +62,10 @@
}
bool ClawIsReady() {
- control_loops::claw_queue_group.goal.FetchLatest();
- control_loops::claw_queue_group.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.FetchLatest()) {
+ control_loops::claw_queue_group.goal.FetchLatest();
+ }
+
bool ans =
control_loops::claw_queue_group.status->zeroed &&
(::std::abs(control_loops::claw_queue_group.status->bottom_velocity) <
@@ -143,6 +145,14 @@
return false;
}
+::std::unique_ptr<TypedAction< ::frc971::actions::ShootActionQueueGroup>>
+MakeShootAction() {
+ return ::std::unique_ptr<
+ TypedAction< ::frc971::actions::ShootActionQueueGroup>>(
+ new TypedAction< ::frc971::actions::ShootActionQueueGroup>(
+ &::frc971::actions::shoot_action));
+}
+
} // namespace actions
} // namespace frc971
diff --git a/frc971/actions/shoot_action.h b/frc971/actions/shoot_action.h
index 733af4e..884c23a 100644
--- a/frc971/actions/shoot_action.h
+++ b/frc971/actions/shoot_action.h
@@ -1,5 +1,8 @@
+#include <memory>
+
#include "frc971/actions/shoot_action.q.h"
#include "frc971/actions/action.h"
+#include "frc971/actions/action_client.h"
namespace frc971 {
namespace actions {
@@ -11,7 +14,7 @@
// Actually execute the action of moving the claw and shooter into position
// and actually firing them.
- void RunAction();
+ virtual void RunAction();
// calc an offset to our requested shot based on robot speed
double SpeedToAngleOffset(double speed);
@@ -32,6 +35,9 @@
int previous_shots_;
};
+// Makes a new ShootAction action.
+::std::unique_ptr<TypedAction< ::frc971::actions::ShootActionQueueGroup>>
+MakeShootAction();
+
} // namespace actions
} // namespace frc971
-
diff --git a/frc971/actions/shoot_action_main.cc b/frc971/actions/shoot_action_main.cc
index 31cea1f..18df9aa 100644
--- a/frc971/actions/shoot_action_main.cc
+++ b/frc971/actions/shoot_action_main.cc
@@ -1,7 +1,5 @@
-#include "stdio.h"
+#include <stdio.h>
-#include "aos/common/control_loop/Timing.h"
-#include "aos/common/time.h"
#include "aos/linux_code/init.h"
#include "aos/common/logging/logging.h"
#include "frc971/actions/shoot_action.q.h"
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index ea2a74b..3549f07 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -1,4 +1,6 @@
-#include "stdio.h"
+#include <stdio.h>
+
+#include <memory>
#include "aos/common/control_loop/Timing.h"
#include "aos/common/time.h"
@@ -9,12 +11,19 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/actions/action_client.h"
+#include "frc971/actions/shoot_action.h"
+#include "frc971/actions/drivetrain_action.h"
using ::aos::time::Time;
namespace frc971 {
namespace autonomous {
+namespace time = ::aos::time;
+
static double left_initial_position, right_initial_position;
bool ShouldExitAuto() {
@@ -48,42 +57,6 @@
.Send();
}
-void SetDriveGoal(double yoffset, double maximum_velocity = 1.5) {
- LOG(INFO, "Going to move %f\n", yoffset);
-
- // Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
- ::Eigen::Matrix<double, 2, 1> driveTrainState;
- const double goal_velocity = 0.0;
- const double epsilon = 0.01;
-
- profile.set_maximum_acceleration(2.0);
- profile.set_maximum_velocity(maximum_velocity);
-
- while (true) {
- ::aos::time::PhasedLoop10MS(5000); // wait until next 10ms tick
- driveTrainState = profile.Update(yoffset, goal_velocity);
-
- if (::std::abs(driveTrainState(0, 0) - yoffset) < epsilon) break;
- if (ShouldExitAuto()) return;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- driveTrainState(0, 0) + left_initial_position,
- driveTrainState(0, 0) + right_initial_position);
- control_loops::drivetrain.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(false)
- .left_goal(driveTrainState(0, 0) + left_initial_position)
- .right_goal(driveTrainState(0, 0) + right_initial_position)
- .left_velocity_goal(driveTrainState(1, 0))
- .right_velocity_goal(driveTrainState(1, 0))
- .Send();
- }
- left_initial_position += yoffset;
- right_initial_position += yoffset;
- LOG(INFO, "Done moving\n");
-}
-
void DriveSpin(double radians) {
LOG(INFO, "going to spin %f\n", radians);
@@ -123,13 +96,82 @@
LOG(INFO, "Done moving\n");
}
-void HandleAuto() {
- LOG(INFO, "Handling auto mode\n");
+void PositionClawVertically(double intake_power = 0.0, double centering_power = 0.0) {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.0)
+ .separation_angle(0.0)
+ .intake(intake_power)
+ .centering(centering_power)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
- ResetDrivetrain();
+void PositionClawBackIntake() {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(-2.273474)
+ .separation_angle(0.0)
+ .intake(12.0)
+ .centering(12.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
- if (ShouldExitAuto()) return;
-
+void PositionClawForShot() {
+ // Turn the claw on, keep it straight up until the ball has been grabbed.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.9)
+ .separation_angle(0.1)
+ .intake(3.0)
+ .centering(1.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
+
+void SetShotPower(double power) {
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(power)
+ .shot_requested(false)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ }
+}
+
+void WaitUntilDoneOrCanceled(Action *action) {
+ while (true) {
+ // Poll the running bit and auto done bits.
+ ::aos::time::PhasedLoop10MS(5000);
+ if (!action->Running() || ShouldExitAuto()) {
+ return;
+ }
+ }
+}
+
+void Shoot() {
+ // Shoot.
+ auto shoot_action = actions::MakeShootAction();
+ shoot_action->Start();
+ WaitUntilDoneOrCanceled(shoot_action.get());
+}
+
+::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+SetDriveGoal(double distance, double maximum_velocity = 1.5) {
+ 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()->maximum_velocity = maximum_velocity;
+ drivetrain_action->Start();
+ left_initial_position += distance;
+ right_initial_position += distance;
+ return ::std::move(drivetrain_action);
+}
+
+void InitializeEncoders() {
control_loops::drivetrain.position.FetchLatest();
while (!control_loops::drivetrain.position.get()) {
LOG(WARNING, "No previous drivetrain position packet, trying to fetch again\n");
@@ -140,7 +182,61 @@
right_initial_position =
control_loops::drivetrain.position->right_encoder;
- StopDrivetrain();
+}
+
+void HandleAuto() {
+ LOG(INFO, "Handling auto mode\n");
+ ResetDrivetrain();
+
+ if (ShouldExitAuto()) return;
+ InitializeEncoders();
+
+ // Turn the claw on, keep it straight up until the ball has been grabbed.
+ PositionClawVertically(12.0, 4.0);
+ SetShotPower(100.0);
+
+ // Wait for the ball to enter the claw.
+ time::SleepFor(time::Time::InSeconds(0.5));
+ if (ShouldExitAuto()) return;
+ PositionClawForShot();
+
+ {
+ if (ShouldExitAuto()) return;
+ // Drive to the goal.
+ auto drivetrain_action = SetDriveGoal(3.0);
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+
+ // Shoot.
+ Shoot();
+ time::SleepFor(time::Time::InSeconds(0.1));
+
+ {
+ if (ShouldExitAuto()) return;
+ // Intake the new ball.
+ PositionClawBackIntake();
+ auto drivetrain_action = SetDriveGoal(-0.3);
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+
+ // Drive back.
+ {
+ auto drivetrain_action = SetDriveGoal(3.0);
+ time::SleepFor(time::Time::InSeconds(0.5));
+ if (ShouldExitAuto()) return;
+ PositionClawForShot();
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+
+ // Shoot
+ Shoot();
+ if (ShouldExitAuto()) return;
+
+ // Get ready to zero when we come back up.
+ PositionClawVertically(0.0, 0.0);
}
} // namespace autonomous
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 91b1b2f..980425f 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -25,11 +25,16 @@
'auto_queue',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/actions/actions.gyp:action_client',
+ '<(DEPTH)/frc971/actions/actions.gyp:shoot_action_lib',
+ '<(DEPTH)/frc971/actions/actions.gyp:drivetrain_action_lib',
],
'export_dependent_settings': [
'<(AOS)/common/common.gyp:controls',
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 9d29107..d2260c0 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -137,8 +137,8 @@
0.912207 * 2.0,
-0.849484,
1.42308,
- {-1.682379 * 2.0, 1.043334 * 2.0, -3.166136, 0.643334 * 2.0, {-1.7 * 2.0, -1.544662 * 2.0, -1.7 * 2.0, -1.547616 * 2.0}, {-0.130218 * 2.0, -0.019771 * 2.0, -0.132036 * 2.0, -0.018862 * 2.0}, {0.935842 * 2.0, 1.1 * 2.0, 0.932660 * 2.0, 1.1 * 2.0}},
- {-1.225821 * 2.0, 1.553752 * 2.0, -2.273474, 1.153752 * 2.0, {-1.3 * 2.0, -1.088331 * 2.0, -1.3 * 2.0, -1.088331 * 2.0}, {-0.134536 * 2.0, -0.018408 * 2.0, -0.136127 * 2.0, -0.019771 * 2.0}, {1.447396 * 2.0, 1.6 * 2.0, 1.443987 * 2.0, 1.6 * 2.0}},
+ {-3.364758, 2.086668, -3.166136, 1.95, {-1.7 * 2.0, -1.544662 * 2.0, -1.7 * 2.0, -1.547616 * 2.0}, {-0.130218 * 2.0, -0.019771 * 2.0, -0.132036 * 2.0, -0.018862 * 2.0}, {0.935842 * 2.0, 1.1 * 2.0, 0.932660 * 2.0, 1.1 * 2.0}},
+ {-2.451642, 3.107504, -2.273474, 2.750, {-1.3 * 2.0, -1.088331 * 2.0, -1.3 * 2.0, -1.088331 * 2.0}, {-0.134536 * 2.0, -0.018408 * 2.0, -0.136127 * 2.0, -0.019771 * 2.0}, {1.447396 * 2.0, 1.6 * 2.0, 1.443987 * 2.0, 1.6 * 2.0}},
0.020000 * 2.0, // claw_unimportant_epsilon
-0.200000 * 2.0, // start_fine_tune_pos
4.000000,
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 96325de..8033591 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -18,6 +18,9 @@
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/actions/actions.gyp:shoot_action_queue',
+ '<(DEPTH)/frc971/actions/actions.gyp:action_client',
+ '<(DEPTH)/frc971/actions/actions.gyp:catch_action_queue',
+ '<(DEPTH)/frc971/actions/actions.gyp:shoot_action_lib',
],
},
{
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index 78a90e5..2f79d96 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -15,6 +15,9 @@
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
#include "frc971/actions/shoot_action.q.h"
+#include "frc971/actions/action_client.h"
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/shoot_action.h"
using ::frc971::control_loops::drivetrain;
using ::frc971::sensors::othersensors;
@@ -33,20 +36,25 @@
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kFire(3, 11);
+const ButtonLocation kCatch(3, 10);
+
+const ButtonLocation kFire(3, 9);
const ButtonLocation kUnload(2, 11);
const ButtonLocation kReload(2, 6);
-const ButtonLocation kRollersOut(3, 12);
-const ButtonLocation kRollersIn(3, 10);
+const ButtonLocation kRollersOut(3, 8);
+const ButtonLocation kRollersIn(3, 3);
-const ButtonLocation kTuck(3, 8);
-const ButtonLocation kIntakeOpenPosition(3, 9);
-const ButtonLocation kIntakePosition(3, 7);
+const ButtonLocation kTuck(3, 4);
+const ButtonLocation kIntakePosition(3, 5);
+const ButtonLocation kIntakeOpenPosition(3, 11);
+//const ButtonLocation kFlipRobot(3, 7);
+const JoystickAxis kFlipRobot(3, 3);
+// Truss shot. (3, 1)
-const ButtonLocation kLongShot(3, 5);
-const ButtonLocation kMediumShot(3, 3);
-const ButtonLocation kShortShot(3, 6);
+const ButtonLocation kLongShot(3, 7);
+const ButtonLocation kMediumShot(3, 6);
+const ButtonLocation kShortShot(3, 2);
struct ClawGoal {
double angle;
@@ -64,99 +72,34 @@
const ClawGoal kIntakeGoal = {-2.273474, 0.0};
const ClawGoal kIntakeOpenGoal = {-2.0, 1.2};
+// TODO(austin): Tune these by hand...
+const ClawGoal kFlippedTuckGoal = {2.733474, -0.75};
+const ClawGoal kFlippedIntakeGoal = {2.0, 0.0};
+const ClawGoal kFlippedIntakeOpenGoal = {0.95, 1.0};
+
const double kIntakePower = 2.0;
+const double kShootSeparation = 0.08;
const ShotGoal kLongShotGoal = {
- {-M_PI / 2.0 + 0.43, 0.10}, 145, false, kIntakePower};
-const ShotGoal kMediumShotGoal = {{-0.9, 0.10}, 100, true, kIntakePower};
-const ShotGoal kShortShotGoal = {{-0.04, 0.10}, 40, false, kIntakePower};
+ {-M_PI / 2.0 + 0.43, kShootSeparation}, 145, false, kIntakePower};
+const ShotGoal kMediumShotGoal = {
+ {-0.9, kShootSeparation}, 100, true, kIntakePower};
+const ShotGoal kShortShotGoal = {
+ {-0.04, kShootSeparation}, 10, false, kIntakePower};
-class Action {
- public:
- // Cancels the action.
- void Cancel() { DoCancel(); }
- // Returns true if the action is currently running.
- bool Running() { return DoRunning(); }
- // Starts the action.
- void Start() { DoStart(); }
-
- virtual ~Action() {}
-
- private:
- virtual void DoCancel() = 0;
- virtual bool DoRunning() = 0;
- virtual void DoStart() = 0;
-};
-
-// Templated subclass to hold the type information.
-template <typename T>
-class TypedAction : public Action {
- public:
- typedef typename std::remove_reference<
- decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
- GoalType;
-
- TypedAction(T *queue_group)
- : queue_group_(queue_group),
- goal_(queue_group_->goal.MakeMessage()),
- has_started_(false) {}
-
- // Returns the current goal that will be sent when the action is sent.
- GoalType *GetGoal() { return goal_.get(); }
-
- virtual ~TypedAction() {
- LOG(INFO, "Calling destructor\n");
- DoCancel();
- }
-
- private:
- // Cancels the action.
- virtual void DoCancel() {
- LOG(INFO, "Canceling action\n");
- queue_group_->goal.MakeWithBuilder().run(false).Send();
- }
-
- // Returns true if the action is running or we don't have an initial response
- // back from it to signal whether or not it is running.
- virtual bool DoRunning() {
- if (has_started_) {
- queue_group_->status.FetchLatest();
- } else if (queue_group_->status.FetchLatest()) {
- if (queue_group_->status->running) {
- // Wait until it reports that it is running to start.
- has_started_ = true;
- }
- }
- return !has_started_ ||
- (queue_group_->status.get() && queue_group_->status->running);
- }
-
- // Starts the action if a goal has been created.
- virtual void DoStart() {
- if (goal_) {
- goal_->run = true;
- goal_.Send();
- has_started_ = false;
- LOG(INFO, "Starting action\n");
- } else {
- has_started_ = true;
- }
- }
-
- T *queue_group_;
- ::aos::ScopedMessagePtr<GoalType> goal_;
- // Track if we have seen a response to the start message.
- // If we haven't, we are considered running regardless.
- bool has_started_;
-};
+const ShotGoal kFlippedLongShotGoal = {
+ {M_PI / 2.0 - 0.43, kShootSeparation}, 145, false, kIntakePower};
+const ShotGoal kFlippedMediumShotGoal = {
+ {0.9, kShootSeparation}, 100, true, kIntakePower};
+const ShotGoal kFlippedShortShotGoal = {
+ {0.04, kShootSeparation}, 10, false, kIntakePower};
// Makes a new ShootAction action.
-::std::unique_ptr<TypedAction< ::frc971::actions::ShootActionQueueGroup>>
-MakeShootAction() {
- return ::std::unique_ptr<
- TypedAction< ::frc971::actions::ShootActionQueueGroup>>(
- new TypedAction< ::frc971::actions::ShootActionQueueGroup>(
- &::frc971::actions::shoot_action));
+::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>
+MakeCatchAction() {
+ return ::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>(
+ new TypedAction< ::frc971::actions::CatchActionGroup>(
+ &::frc971::actions::catch_action));
}
// A queue which queues Actions and cancels them.
@@ -334,40 +277,69 @@
intake_power_ = 0.0;
}
- if (data.IsPressed(kIntakeOpenPosition)) {
- action_queue_.CancelAllActions();
- SetGoal(kIntakeOpenGoal);
- } else if (data.IsPressed(kIntakePosition)) {
- action_queue_.CancelAllActions();
- SetGoal(kIntakeGoal);
- } else if (data.IsPressed(kTuck)) {
- action_queue_.CancelAllActions();
- SetGoal(kTuckGoal);
+ if (data.GetAxis(kFlipRobot) < 0.5) {
+ if (data.IsPressed(kIntakeOpenPosition)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kFlippedIntakeOpenGoal);
+ } else if (data.IsPressed(kIntakePosition)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kFlippedIntakeGoal);
+ } else if (data.IsPressed(kTuck)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kFlippedTuckGoal);
+ } else if (data.PosEdge(kLongShot)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kFlippedLongShotGoal);
+ } else if (data.PosEdge(kMediumShot)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kFlippedMediumShotGoal);
+ } else if (data.PosEdge(kShortShot)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kFlippedShortShotGoal);
+ }
+ } else {
+ if (data.IsPressed(kIntakeOpenPosition)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kIntakeOpenGoal);
+ } else if (data.IsPressed(kIntakePosition)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kIntakeGoal);
+ } else if (data.IsPressed(kTuck)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kTuckGoal);
+ } else if (data.PosEdge(kLongShot)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kLongShotGoal);
+ } else if (data.PosEdge(kMediumShot)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kMediumShotGoal);
+ } else if (data.PosEdge(kShortShot)) {
+ action_queue_.CancelAllActions();
+ SetGoal(kShortShotGoal);
+ }
}
- if (data.PosEdge(kLongShot)) {
- action_queue_.CancelAllActions();
- SetGoal(kLongShotGoal);
- } else if (data.PosEdge(kMediumShot)) {
- action_queue_.CancelAllActions();
- SetGoal(kMediumShotGoal);
- } else if (data.PosEdge(kShortShot)) {
- action_queue_.CancelAllActions();
- SetGoal(kShortShotGoal);
+ if (data.PosEdge(kCatch)) {
+ auto catch_action = MakeCatchAction();
+ catch_action->GetGoal()->catch_angle = goal_angle_;
+ action_queue_.QueueAction(::std::move(catch_action));
}
if (data.PosEdge(kFire)) {
- action_queue_.QueueAction(MakeShootAction());
+ action_queue_.QueueAction(actions::MakeShootAction());
}
action_queue_.Tick();
if (data.IsPressed(kUnload) || data.IsPressed(kReload)) {
action_queue_.CancelAllActions();
+ intake_power_ = 0.0;
+ velocity_compensation_ = false;
}
// Send out the claw and shooter goals if no actions are running.
if (!action_queue_.Running()) {
- // If the action just ended, turn the intake off and stop velocity compensating.
+ // If the action just ended, turn the intake off and stop velocity
+ // compensating.
if (was_running_) {
intake_power_ = 0.0;
velocity_compensation_ = false;
@@ -381,13 +353,16 @@
control_loops::drivetrain.status->robot_speed)
: 0.0);
+ bool intaking = data.IsPressed(kRollersIn) ||
+ data.IsPressed(kIntakePosition) ||
+ data.IsPressed(kIntakeOpenPosition);
if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
.bottom_angle(goal_angle)
.separation_angle(separation_angle_)
- .intake(data.IsPressed(kRollersIn)
- ? 12.0
- : (data.IsPressed(kRollersOut) ? -12.0 : intake_power_))
- .centering(data.IsPressed(kRollersIn) ? 12.0 : 0.0)
+ .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");
}
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index abd31dc..7d12b48 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -141,7 +141,10 @@
} else if (data->bad_gyro) {
LOG(ERROR, "bad gyro\n");
bad_gyro = true;
- othersensors.MakeWithBuilder().gyro_angle(0).Send();
+ othersensors.MakeWithBuilder()
+ .gyro_angle(0)
+ .sonar_distance(data->main.ultrasonic_pulse_length)
+ .Send();
} else if (data->old_gyro_reading) {
LOG(WARNING, "old/bad gyro reading\n");
bad_gyro = true;
@@ -152,6 +155,7 @@
if (!bad_gyro) {
othersensors.MakeWithBuilder()
.gyro_angle(gyro_translate(data->gyro_angle))
+ .sonar_distance(data->main.ultrasonic_pulse_length)
.Send();
}
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 7fe4c2f..3596191 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -16,6 +16,7 @@
'../actions/actions.gyp:shoot_action',
'../actions/actions.gyp:selfcatch_action',
'../actions/actions.gyp:catch_action',
+ '../actions/actions.gyp:drivetrain_action',
'../input/input.gyp:joystick_reader',
'../output/output.gyp:motor_writer',
'../input/input.gyp:sensor_receiver',
diff --git a/frc971/prime/scripts/start_list.txt b/frc971/prime/scripts/start_list.txt
index 043f616..94bf6bb 100644
--- a/frc971/prime/scripts/start_list.txt
+++ b/frc971/prime/scripts/start_list.txt
@@ -7,3 +7,5 @@
claw
shooter
shoot_action
+drivetrain_action
+catch_action