Merged in the stuff most recently on the robot.
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
new file mode 100644
index 0000000..fe17676
--- /dev/null
+++ b/frc971/actions/action.h
@@ -0,0 +1,104 @@
+#ifndef FRC971_ACTIONS_ACTION_H_
+#define FRC971_ACTIONS_ACTION_H_
+
+#include <stdio.h>
+
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/network/team_number.h"
+
+#include "frc971/constants.h"
+
+namespace frc971 {
+namespace actions {
+
+template <class T> class ActionBase {
+ public:
+ ActionBase(T* acq) : action_q_(acq) {}
+
+ virtual void RunAction() = 0;
+
+ // runs action while enabled
+ void Run() {
+ LOG(DEBUG, "Waiting for input to start\n");
+ action_q_->goal.FetchLatest();
+ while (!action_q_->goal.get()) {
+ action_q_->goal.FetchNextBlocking();
+ }
+
+ if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ while (true) {
+ while (!action_q_->goal->run) {
+ LOG(INFO, "Waiting for an action request.\n");
+ action_q_->goal.FetchNextBlocking();
+ if (!action_q_->goal->run) {
+ if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ }
+ }
+ LOG(INFO, "Starting action\n");
+ if (!action_q_->status.MakeWithBuilder().running(true).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ RunAction();
+ LOG(INFO, "Done with action\n");
+ if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ while (action_q_->goal->run) {
+ LOG(INFO, "Waiting for the action to be stopped.\n");
+ action_q_->goal.FetchNextBlocking();
+ }
+ }
+ }
+
+ // will run until the done condition is met.
+ // will return false if successful and true if the action was canceled or
+ // failed.
+ // done condition are defined as functions that return true when done and have
+ // some sort of blocking statement(FetchNextBlocking) to throttle spin rate.
+ bool WaitUntil(::std::function<bool(void)> done_condition) {
+ while (!done_condition()) {
+ if (ShouldCancel() || abort_) {
+ // Clear abort bit as we have just aborted.
+ abort_ = false;
+ return true;
+ }
+ }
+ if (ShouldCancel() || abort_) {
+ // Clear abort bit as we have just aborted.
+ abort_ = false;
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ // Returns true if the action should be canceled.
+ bool ShouldCancel() {
+ action_q_->goal.FetchLatest();
+ bool ans = !action_q_->goal->run;
+ if (ans) {
+ LOG(INFO, "Time to stop action\n");
+ }
+ return ans;
+ }
+
+ protected:
+
+ // boolean to stop on fail
+ bool abort_ = false;
+
+ // queue for this action
+ T* action_q_;
+};
+
+} // 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
new file mode 100644
index 0000000..7cfb7a5
--- /dev/null
+++ b/frc971/actions/actions.gyp
@@ -0,0 +1,232 @@
+{
+ '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'],
+ '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': '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',
+ 'sources': ['selfcatch_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': 'catch_action_queue',
+ 'type': 'static_library',
+ 'sources': ['catch_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': 'shoot_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'shoot_action.cc',
+ ],
+ 'dependencies': [
+ 'shoot_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(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',
+ ],
+ },
+ {
+ 'target_name': 'selfcatch_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'selfcatch_action.cc',
+ ],
+ 'dependencies': [
+ 'selfcatch_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(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',
+ ],
+ },
+ {
+ 'target_name': 'catch_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'catch_action.cc',
+ ],
+ 'dependencies': [
+ 'catch_action_queue',
+ 'action',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ ],
+ },
+ {
+ 'target_name': 'shoot_action',
+ 'type': 'executable',
+ 'sources': [
+ 'shoot_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'shoot_action_queue',
+ 'shoot_action_lib',
+ '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',
+ ],
+ },
+ {
+ 'target_name': 'selfcatch_action',
+ 'type': 'executable',
+ 'sources': [
+ 'selfcatch_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'selfcatch_action_queue',
+ 'selfcatch_action_lib',
+ 'action',
+ ],
+ },
+ {
+ 'target_name': 'catch_action',
+ 'type': 'executable',
+ 'sources': [
+ 'catch_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'catch_action_queue',
+ 'catch_action_lib',
+ 'action',
+ ],
+ },
+ ],
+}
diff --git a/frc971/actions/catch_action.cc b/frc971/actions/catch_action.cc
new file mode 100644
index 0000000..1fddd30
--- /dev/null
+++ b/frc971/actions/catch_action.cc
@@ -0,0 +1,154 @@
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/actions/catch_action.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/queues/other_sensors.q.h"
+
+namespace frc971 {
+namespace actions {
+
+CatchAction::CatchAction(actions::CatchActionGroup* s)
+ : actions::ActionBase<actions::CatchActionGroup>(s) {}
+
+void CatchAction::RunAction() {
+ control_loops::claw_queue_group.goal.FetchLatest();
+ if (control_loops::claw_queue_group.goal.get() == nullptr) {
+ LOG(WARNING, "no claw goal\n");
+ return;
+ }
+
+ // Set claw angle.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(action_q_->goal->catch_angle - kCatchSeparation / 2.0)
+ .separation_angle(kCatchSeparation)
+ .intake(kCatchIntake)
+ .centering(kCatchCentering)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ LOG(INFO, "Waiting for the claw to be ready\n");
+
+ // wait for claw to be ready
+ if (WaitUntil(::std::bind(&CatchAction::DoneSetupCatch, this))) return;
+ LOG(INFO, "Waiting for the sonar\n");
+
+ close_count_ = 0;
+
+ // 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(action_q_->goal->catch_angle)
+ .separation_angle(0.0)
+ .intake(kCatchIntake)
+ .centering(kCatchCentering)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ // claw now closed
+ if (WaitUntil(::std::bind(&CatchAction::DoneClawWithBall, this))) return;
+
+ for (int i = 0; i < 5; ++i) {
+ aos::time::SleepFor(aos::time::Time::InSeconds(0.05));
+ if (ShouldCancel()) return;
+ }
+}
+
+
+/*bool CatchAction::DoneBallIn() {
+ if (!sensors::othersensors.FetchLatest()) {
+ 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();
+ }
+
+ bool ans =
+ control_loops::claw_queue_group.status->zeroed &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom_velocity) <
+ 1.0) &&
+ (::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::other_sensors.FetchLatest()) {
+ sensors::other_sensors.FetchNextBlocking();
+ }
+ LOG(DEBUG, "Sonar at %.2f.\n", sensors::other_sensors->sonar_distance);
+ if (sensors::other_sensors->sonar_distance > 0.1 &&
+ sensors::other_sensors->sonar_distance < kSonarTriggerDist) {
+ ++close_count_;
+ } else {
+ close_count_ = 0;
+ }
+ if (close_count_ > 50) {
+ return true;
+ }
+ return false;
+}
+
+bool CatchAction::DoneSetupCatch() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+
+ // 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.15;
+ bool open_enough =
+ control_loops::claw_queue_group.status->separation > kCatchMinSeparation;
+
+ if (claw_angle_correct && open_enough) {
+ LOG(INFO, "Claw ready for catching.\n");
+ return true;
+ }
+
+ return false;
+}
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/catch_action.h b/frc971/actions/catch_action.h
new file mode 100644
index 0000000..56c5f42
--- /dev/null
+++ b/frc971/actions/catch_action.h
@@ -0,0 +1,42 @@
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/action.h"
+#include "aos/common/time.h"
+
+namespace frc971 {
+namespace actions {
+
+class CatchAction : public ActionBase<CatchActionGroup> {
+ public:
+
+ explicit CatchAction(CatchActionGroup* s);
+
+ // Actually executes the action of moving the claw into position and closing
+ // it.
+ virtual void RunAction();
+
+ static constexpr double kCatchSeparation = 0.8;
+ static constexpr double kCatchMinSeparation = 0.65;
+ static constexpr double kCatchIntake = 12.0;
+ static constexpr double kSonarTriggerDist = 0.725;
+ static constexpr double kCatchCentering = 12.0;
+ static constexpr double kFinishAngle = 0.2;
+
+ protected:
+ // ready for shot
+ bool DonePreShotOpen();
+ // in the right place
+ bool DoneSetupCatch();
+ // sonar is in valid range to close
+ bool DoneFoundSonar();
+ // Claw reports it is done
+ bool DoneClawWithBall();
+ // hall effect reports the ball is in
+ bool DoneBallIn();
+
+ private:
+ int close_count_;
+};
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/catch_action.q b/frc971/actions/catch_action.q
new file mode 100644
index 0000000..33b21f0
--- /dev/null
+++ b/frc971/actions/catch_action.q
@@ -0,0 +1,19 @@
+package frc971.actions;
+
+queue_group CatchActionGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run;
+ double catch_angle;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group CatchActionGroup catch_action;
diff --git a/frc971/actions/catch_action_main.cc b/frc971/actions/catch_action_main.cc
new file mode 100644
index 0000000..88fa48b
--- /dev/null
+++ b/frc971/actions/catch_action_main.cc
@@ -0,0 +1,20 @@
+#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/catch_action.q.h"
+#include "frc971/actions/catch_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init();
+
+ frc971::actions::CatchAction action_catch(
+ &::frc971::actions::catch_action);
+ action_catch.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
new file mode 100644
index 0000000..1ca6219
--- /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.2);
+ 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.cc b/frc971/actions/selfcatch_action.cc
new file mode 100644
index 0000000..eef0a05
--- /dev/null
+++ b/frc971/actions/selfcatch_action.cc
@@ -0,0 +1,244 @@
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/constants.h"
+
+#include "frc971/actions/selfcatch_action.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/queues/other_sensors.q.h"
+
+namespace frc971 {
+namespace actions {
+
+SelfCatchAction::SelfCatchAction(actions::SelfCatchActionGroup* s)
+ : actions::ActionBase<actions::SelfCatchActionGroup>(s) {}
+
+double SelfCatchAction::SpeedToAngleOffset(double speed) {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+ // scale speed to a [0.0-1.0] on something close to the max
+ return (speed / values.drivetrain_max_speed) *
+ SelfCatchAction::kSpeedOffsetRadians;
+}
+
+void SelfCatchAction::RunAction() {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+
+ // Set shot power to established constant
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
+ kShotPower)
+ .shot_requested(false).unload_requested(false).load_requested(false)
+ .Send()) {
+ LOG(ERROR, "Failed to send the shoot action\n");
+ return;
+ }
+
+ // Set claw angle to account for velocity
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ 0.0 +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for claw to be ready
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneSetupShot, this))) return;
+
+ // Open up the claw in preparation for shooting.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ 0.0 +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(values.shooter_action.claw_separation_goal)
+ .intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for the claw to open up a little before we shoot
+ if (WaitUntil(::std::bind(&SelfCatchAction::DonePreShotOpen, this))) return;
+
+ // Make sure that we have the latest shooter status.
+ control_loops::shooter_queue_group.status.FetchLatest();
+ // Get the number of shots fired up to this point. This should not be updated
+ // again for another few cycles.
+ previous_shots_ = control_loops::shooter_queue_group.status->shots;
+ // Shoot!
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
+ kShotPower)
+ .shot_requested(true).unload_requested(false).load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ return;
+ }
+
+ // wait for record of shot having been fired
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneShot, this))) return;
+
+ // Set claw angle to account for velocity note this is negative
+ // since we will be catching from behind
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ 0.0 -
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(kCatchSeperation).intake(kCatchIntake)
+ .centering(kCatchCentering).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for the sonar to trigger
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneFoundSonar, this))) return;
+
+ // close the claw
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ kFinishAngle)
+ .separation_angle(0.0).intake(kCatchIntake).centering(kCatchCentering)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // claw now closed
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneClawWithBall, this))) return;
+ // ball is fully in
+ if (WaitUntil(::std::bind(&SelfCatchAction::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(&SelfCatchAction::DoneClawWithBall, this))) return;
+
+ // done with action
+ return;
+}
+
+
+bool SelfCatchAction::DoneBallIn() {
+ if (!sensors::other_sensors.FetchLatest()) {
+ sensors::other_sensors.FetchNextBlocking();
+ }
+ if (sensors::other_sensors->plunger_hall_effect_distance > 0.005) {
+ LOG(INFO, "Ball in at %.2f.\n",
+ sensors::other_sensors->plunger_hall_effect_distance);
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::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 SelfCatchAction::DoneFoundSonar() {
+ if (!sensors::other_sensors.FetchLatest()) {
+ sensors::other_sensors.FetchNextBlocking();
+ }
+ if (sensors::other_sensors->sonar_distance > 0.3 &&
+ sensors::other_sensors->sonar_distance < kSonarTriggerDist) {
+ LOG(INFO, "Hit Sonar at %.2f.\n", sensors::other_sensors->sonar_distance);
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::DoneSetupShot() {
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ if (!control_loops::shooter_queue_group.goal.FetchLatest()) {
+ LOG(ERROR, "Failed to fetch shooter goal.\n");
+ }
+ if (!control_loops::claw_queue_group.goal.FetchLatest()) {
+ LOG(ERROR, "Failed to fetch claw goal.\n");
+ }
+ // Make sure that both the shooter and claw have reached the necessary
+ // states.
+ // 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 shooter_power_correct =
+ ::std::abs(control_loops::shooter_queue_group.status->hard_stop_power -
+ control_loops::shooter_queue_group.goal->shot_power) <
+ 0.005;
+
+ bool claw_angle_correct =
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) <
+ 0.005;
+
+ if (control_loops::shooter_queue_group.status->ready &&
+ control_loops::claw_queue_group.status->done_with_ball &&
+ shooter_power_correct && claw_angle_correct) {
+ LOG(INFO, "Claw and Shooter ready for shooting.\n");
+ // TODO(james): Get realer numbers for shooter_action.
+ return true;
+ }
+
+ // update the claw position to track velocity
+ // TODO(ben): the claw may never reach the goal if the velocity is
+ // continually changing, we will need testing to see
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ selfcatch_action.goal->shot_angle +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ abort_ = true;
+ return true;
+ } else {
+ LOG(INFO, "Updating claw angle for velocity offset(%.4f).\n",
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed));
+ }
+ return false;
+}
+
+bool SelfCatchAction::DonePreShotOpen() {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+
+ if (control_loops::claw_queue_group.status->separation >
+ values.shooter_action.claw_shooting_separation) {
+ LOG(INFO, "Opened up enough to shoot.\n");
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::DoneShot() {
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ if (control_loops::shooter_queue_group.status->shots > previous_shots_) {
+ LOG(INFO, "Shot succeeded!\n");
+ return true;
+ }
+ return false;
+}
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/selfcatch_action.h b/frc971/actions/selfcatch_action.h
new file mode 100644
index 0000000..c57d24a
--- /dev/null
+++ b/frc971/actions/selfcatch_action.h
@@ -0,0 +1,48 @@
+#include "frc971/actions/selfcatch_action.q.h"
+#include "frc971/actions/action.h"
+#include "aos/common/time.h"
+
+namespace frc971 {
+namespace actions {
+
+class SelfCatchAction : public ActionBase<SelfCatchActionGroup> {
+ public:
+
+ explicit SelfCatchAction(SelfCatchActionGroup* s);
+
+ // Actually execute the action of moving the claw and shooter into position
+ // and actually firing them.
+ virtual void RunAction();
+
+ // calc an offset to our requested shot based on robot speed
+ double SpeedToAngleOffset(double speed);
+
+ static constexpr double kSpeedOffsetRadians = 0.2;
+ static constexpr double kShotPower = 100.0;
+ static constexpr double kCatchSeperation = 1.0;
+ static constexpr double kCatchIntake = 12.0;
+ static constexpr double kSonarTriggerDist = 0.8;
+ static constexpr double kCatchCentering = 12.0;
+ static constexpr double kFinishAngle = 0.2;
+
+ protected:
+ // completed shot
+ bool DoneShot();
+ // ready for shot
+ bool DonePreShotOpen();
+ // in the right place
+ bool DoneSetupShot();
+ // sonar is in valid range to close
+ bool DoneFoundSonar();
+ // Claw reports it is done
+ bool DoneClawWithBall();
+ // hall effect reports the ball is in
+ bool DoneBallIn();
+
+ // to track when shot is complete
+ int previous_shots_;
+};
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/selfcatch_action.q b/frc971/actions/selfcatch_action.q
new file mode 100644
index 0000000..9ba21ad
--- /dev/null
+++ b/frc971/actions/selfcatch_action.q
@@ -0,0 +1,19 @@
+package frc971.actions;
+
+queue_group SelfCatchActionGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run;
+ double shot_angle;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group SelfCatchActionGroup selfcatch_action;
diff --git a/frc971/actions/selfcatch_action_main.cc b/frc971/actions/selfcatch_action_main.cc
new file mode 100644
index 0000000..53600ac
--- /dev/null
+++ b/frc971/actions/selfcatch_action_main.cc
@@ -0,0 +1,22 @@
+#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/selfcatch_action.q.h"
+#include "frc971/actions/selfcatch_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init();
+
+ frc971::actions::SelfCatchAction selfcatch(
+ &::frc971::actions::selfcatch_action);
+ selfcatch.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
new file mode 100644
index 0000000..b1e27b4
--- /dev/null
+++ b/frc971/actions/shoot_action.cc
@@ -0,0 +1,191 @@
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/actions/shoot_action.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+namespace frc971 {
+namespace actions {
+
+ShootAction::ShootAction(actions::ShootActionQueueGroup* s)
+ : actions::ActionBase<actions::ShootActionQueueGroup>(s) {}
+
+double ShootAction::SpeedToAngleOffset(double speed) {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+ // scale speed to a [0.0-1.0] on something close to the max
+ return (speed / values.drivetrain_max_speed) * ShootAction::kOffsetRadians;
+}
+void ShootAction::RunAction() {
+ InnerRunAction();
+
+ // Now do our 'finally' block and make sure that we aren't requesting shots
+ // continually.
+ control_loops::shooter_queue_group.goal.FetchLatest();
+ if (control_loops::shooter_queue_group.goal.get() == nullptr) {
+ return;
+ }
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(control_loops::shooter_queue_group.goal->shot_power)
+ .shot_requested(false)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ return;
+ }
+}
+
+void ShootAction::InnerRunAction() {
+ LOG(INFO, "Shooting at the original angle and power.\n");
+
+ // wait for claw to be ready
+ if (WaitUntil(::std::bind(&ShootAction::DoneSetupShot, this))) return;
+
+ // Turn the intake off.
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(control_loops::claw_queue_group.goal->bottom_angle)
+ .separation_angle(
+ control_loops::claw_queue_group.goal->separation_angle)
+ .intake(0.0)
+ .centering(0.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // Make sure that we have the latest shooter status.
+ control_loops::shooter_queue_group.status.FetchLatest();
+ // Get the number of shots fired up to this point. This should not be updated
+ // again for another few cycles.
+ previous_shots_ = control_loops::shooter_queue_group.status->shots;
+ // Shoot!
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(control_loops::shooter_queue_group.goal->shot_power)
+ .shot_requested(true)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ return;
+ }
+
+ // wait for record of shot having been fired
+ if (WaitUntil(::std::bind(&ShootAction::DoneShot, this))) return;
+
+ // Turn the intake off.
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(control_loops::claw_queue_group.goal->bottom_angle)
+ .separation_angle(
+ control_loops::claw_queue_group.goal->separation_angle)
+ .intake(0.0)
+ .centering(0.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+}
+
+bool ClawIsReady() {
+ 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) <
+ 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 ShooterIsReady() {
+ control_loops::shooter_queue_group.goal.FetchLatest();
+ control_loops::shooter_queue_group.status.FetchLatest();
+ if (control_loops::shooter_queue_group.status->ready) {
+ LOG(INFO, "Power error is %f - %f -> %f, ready %d\n",
+ control_loops::shooter_queue_group.status->hard_stop_power,
+ control_loops::shooter_queue_group.goal->shot_power,
+ ::std::abs(control_loops::shooter_queue_group.status->hard_stop_power -
+ control_loops::shooter_queue_group.goal->shot_power),
+ control_loops::shooter_queue_group.status->ready);
+ }
+ return (::std::abs(
+ control_loops::shooter_queue_group.status->hard_stop_power -
+ control_loops::shooter_queue_group.goal->shot_power) < 1.0) &&
+ control_loops::shooter_queue_group.status->ready;
+}
+
+bool ShootAction::DoneSetupShot() {
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ 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 (ShooterIsReady() && ClawIsReady()) {
+ LOG(INFO, "Claw and Shooter ready for shooting.\n");
+ return true;
+ }
+
+ return false;
+}
+
+bool ShootAction::DonePreShotOpen() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ if (control_loops::claw_queue_group.status->separation >
+ kClawShootingSeparation) {
+ LOG(INFO, "Opened up enough to shoot.\n");
+ return true;
+ }
+ return false;
+}
+
+bool ShootAction::DoneShot() {
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ if (control_loops::shooter_queue_group.status->shots > previous_shots_) {
+ LOG(INFO, "Shot succeeded!\n");
+ return true;
+ }
+ 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
new file mode 100644
index 0000000..6d29ca7
--- /dev/null
+++ b/frc971/actions/shoot_action.h
@@ -0,0 +1,44 @@
+#include <memory>
+
+#include "frc971/actions/shoot_action.q.h"
+#include "frc971/actions/action.h"
+#include "frc971/actions/action_client.h"
+
+namespace frc971 {
+namespace actions {
+
+class ShootAction : public ActionBase<actions::ShootActionQueueGroup> {
+ public:
+
+ explicit ShootAction(actions::ShootActionQueueGroup* s);
+
+ // Actually execute the action of moving the claw and shooter into position
+ // and actually firing them.
+ virtual void RunAction();
+ void InnerRunAction();
+
+ // calc an offset to our requested shot based on robot speed
+ double SpeedToAngleOffset(double speed);
+
+ static constexpr double kOffsetRadians = 0.4;
+ static constexpr double kClawShootingSeparation = 0.10;
+ static constexpr double kClawShootingSeparationGoal = 0.10;
+
+ protected:
+ // completed shot
+ bool DoneShot();
+ // ready for shot
+ bool DonePreShotOpen();
+ // in the right place
+ bool DoneSetupShot();
+
+ // to track when shot is complete
+ 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.q b/frc971/actions/shoot_action.q
new file mode 100644
index 0000000..d080071
--- /dev/null
+++ b/frc971/actions/shoot_action.q
@@ -0,0 +1,21 @@
+package frc971.actions;
+
+queue_group ShootActionQueueGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run; // Shot power in joules.
+ double shot_power;
+ // Claw angle when shooting.
+ double shot_angle;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group ShootActionQueueGroup shoot_action;
diff --git a/frc971/actions/shoot_action_main.cc b/frc971/actions/shoot_action_main.cc
new file mode 100644
index 0000000..18df9aa
--- /dev/null
+++ b/frc971/actions/shoot_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/shoot_action.q.h"
+#include "frc971/actions/shoot_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actions::ShootAction shoot(&::frc971::actions::shoot_action);
+ shoot.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index ea2a74b..107b380 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,85 @@
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.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_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.7) {
+ LOG(INFO, "Driving to %f\n", distance);
+ auto drivetrain_action = actions::MakeDrivetrainAction();
+ drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
+ drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
+ drivetrain_action->GetGoal()->y_offset = distance;
+ drivetrain_action->GetGoal()->maximum_velocity = maximum_velocity;
+ drivetrain_action->Start();
+ // Uncomment to make relative again.
+ 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 +185,107 @@
right_initial_position =
control_loops::drivetrain.position->right_encoder;
- StopDrivetrain();
+}
+
+void WaitUntilClawDone() {
+ while (true) {
+ // Poll the running bit and auto done bits.
+ ::aos::time::PhasedLoop10MS(5000);
+ control_loops::claw_queue_group.status.FetchLatest();
+ control_loops::claw_queue_group.goal.FetchLatest();
+ if (ShouldExitAuto()) {
+ return;
+ }
+ if (control_loops::claw_queue_group.status.get() == nullptr ||
+ control_loops::claw_queue_group.goal.get() == nullptr) {
+ continue;
+ }
+ bool ans =
+ control_loops::claw_queue_group.status->zeroed &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom_velocity) <
+ 1.0) &&
+ (::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) {
+ return;
+ }
+ }
+}
+
+void HandleAuto() {
+ // The front of the robot is 1.854 meters from the wall
+ const double kShootDistance = 3.15;
+ const double kPickupDistance = 0.5;
+ 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.
+ LOG(INFO, "Claw going up\n");
+ 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\n");
+
+ {
+ if (ShouldExitAuto()) return;
+ // Drive to the goal.
+ auto drivetrain_action = SetDriveGoal(-kShootDistance);
+ time::SleepFor(time::Time::InSeconds(0.75));
+ PositionClawForShot();
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+ LOG(INFO, "Shooting\n");
+
+ // Shoot.
+ Shoot();
+ time::SleepFor(time::Time::InSeconds(0.1));
+
+ {
+ if (ShouldExitAuto()) return;
+ // Intake the new ball.
+ LOG(INFO, "Claw ready for intake\n");
+ PositionClawBackIntake();
+ auto drivetrain_action = SetDriveGoal(kShootDistance + kPickupDistance);
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ LOG(INFO, "Wait for the claw.\n");
+ WaitUntilClawDone();
+ if (ShouldExitAuto()) return;
+ }
+
+ // Drive back.
+ {
+ LOG(INFO, "Driving back\n");
+ auto drivetrain_action = SetDriveGoal(-(kShootDistance + kPickupDistance));
+ time::SleepFor(time::Time::InSeconds(0.7));
+ if (ShouldExitAuto()) return;
+ PositionClawForShot();
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ WaitUntilClawDone();
+ if (ShouldExitAuto()) return;
+ }
+
+ LOG(INFO, "Shooting\n");
+ // 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/auto_main.cc b/frc971/autonomous/auto_main.cc
index e8914c6..36494bc 100644
--- a/frc971/autonomous/auto_main.cc
+++ b/frc971/autonomous/auto_main.cc
@@ -12,6 +12,7 @@
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init();
+ LOG(INFO, "Auto main started\n");
::frc971::autonomous::autonomous.FetchLatest();
while (!::frc971::autonomous::autonomous.get()) {
::frc971::autonomous::autonomous.FetchNextBlocking();
@@ -24,9 +25,13 @@
LOG(INFO, "Got another auto packet\n");
}
LOG(INFO, "Starting auto mode\n");
+ ::aos::time::Time start_time = ::aos::time::Time::Now();
::frc971::autonomous::HandleAuto();
- LOG(INFO, "Auto mode exited, waiting for it to finish.\n");
+
+ ::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");
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 dd47975..9db46fd 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -9,9 +9,7 @@
#include "aos/common/network/team_number.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -21,29 +19,28 @@
namespace constants {
namespace {
+const uint16_t kCompTeamNumber = 971;
+const uint16_t kPracticeTeamNumber = 9971;
+
const double kCompDrivetrainEncoderRatio =
- (15.0 / 50.0) /*output reduction*/ * (36.0 / 24.0) /*encoder gears*/;
-const double kCompLowGearRatio = 14.0 / 60.0 * 15.0 / 50.0;
-const double kCompHighGearRatio = 30.0 / 44.0 * 15.0 / 50.0;
+ (18.0 / 50.0) /*output reduction*/ * (56.0 / 30.0) /*encoder gears*/;
+const double kCompLowGearRatio = 18.0 / 60.0 * 18.0 / 50.0;
+const double kCompHighGearRatio = 28.0 / 50.0 * 18.0 / 50.0;
-const double kPracticeDrivetrainEncoderRatio =
- (17.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/;
-const double kPracticeLowGearRatio = 16.0 / 60.0 * 19.0 / 50.0;
-const double kPracticeHighGearRatio = 28.0 / 48.0 * 19.0 / 50.0;
+const double kPracticeDrivetrainEncoderRatio = kCompDrivetrainEncoderRatio;
+const double kPracticeLowGearRatio = kCompLowGearRatio;
+const double kPracticeHighGearRatio = kCompHighGearRatio;
-const ShifterHallEffect kCompLeftDriveShifter{0.83, 2.32, 1.2, 1.0};
-const ShifterHallEffect kCompRightDriveShifter{0.865, 2.375, 1.2, 1.0};
+const ShifterHallEffect kCompRightDriveShifter{555, 657, 660, 560, 0.2, 0.7};
+const ShifterHallEffect kCompLeftDriveShifter{555, 660, 644, 552, 0.2, 0.7};
-const ShifterHallEffect kPracticeLeftDriveShifter{5, 0, 0.60,
- 0.47};
-const ShifterHallEffect kPracticeRightDriveShifter{5, 0, 0.62,
- 0.55};
-const double shooter_zeroing_off_speed = 0.0;
+const ShifterHallEffect kPracticeRightDriveShifter{550, 640, 635, 550, 0.2, 0.7};
+const ShifterHallEffect kPracticeLeftDriveShifter{540, 620, 640, 550, 0.2, 0.7};
+
const double shooter_zeroing_speed = 0.05;
+const double shooter_unload_speed = 0.08;
-const Values *DoGetValues() {
- uint16_t team = ::aos::network::GetTeamNumber();
- LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+const Values *DoGetValuesForTeam(uint16_t team) {
switch (team) {
case 1: // for tests
return new Values{
@@ -52,15 +49,15 @@
kCompHighGearRatio,
kCompLeftDriveShifter,
kCompRightDriveShifter,
- true,
- control_loops::MakeVClutchDrivetrainLoop,
- control_loops::MakeClutchDrivetrainLoop,
+ false,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
// ShooterLimits
// TODO(ben): make these real numbers
{-0.00127, 0.298196, -0.001524, 0.305054, 0.0149098,
{-0.001778, 0.000762, 0, 0}, {-0.001778, 0.009906, 0, 0}, {0.006096, 0.026416, 0, 0},
- shooter_zeroing_off_speed,
- shooter_zeroing_speed
+ shooter_zeroing_speed,
+ shooter_unload_speed
},
{0.5,
0.1,
@@ -74,7 +71,10 @@
0.01, // claw_unimportant_epsilon
0.9, // start_fine_tune_pos
4.0,
- }
+ },
+ {0.07, 0.15}, // shooter_action
+ 0.02, // drivetrain done delta
+ 5.0 // drivetrain max speed
};
break;
case kCompTeamNumber:
@@ -85,28 +85,39 @@
kCompLeftDriveShifter,
kCompRightDriveShifter,
true,
- control_loops::MakeVClutchDrivetrainLoop,
- control_loops::MakeClutchDrivetrainLoop,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
// ShooterLimits
- // TODO(ben): make these real numbers
- {-0.00127, 0.298196, -0.001524, 0.305054, 0.0149098,
- {-0.001778, 0.000762, 0, 0}, {-0.001778, 0.009906, 0, 0}, {0.006096, 0.026416, 0, 0},
- shooter_zeroing_off_speed,
- shooter_zeroing_speed
+ {-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.5,
- 0.1,
- 0.1,
- 0.0,
- 1.57,
- 0,
- 0,
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, 0, 0}, {1.0, 1.1, 0, 0}, {2.0, 2.1, 0, 0}},
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, 0, 0}, {1.0, 1.1, 0, 0}, {2.0, 2.1, 0, 0}},
- 0.01, // claw_unimportant_epsilon
- 0.9, // start_fine_tune_pos
- 4.0,
- }
+ {0.800000,
+ 0.400000,
+ 0.000000,
+ -1.220821,
+ 1.822142,
+ -0.849484,
+ 1.42309,
+ {-3.328397, 2.091668, -3.166136, 1.95,
+ {-3.4, -3.092051, -3.4, -3.093414},
+ {-0.249073, -0.035452, -0.251800, -0.033179},
+ {1.889410, 2.2, 1.883956, 2.2}},
+ {-2.453460, 3.082960, -2.453460, 3.082960,
+ {-2.6, -2.185752, -2.6, -2.184843},
+ {-0.280434, -0.049087, -0.277707, -0.047724},
+ {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
+ 0.02, // drivetrain done delta
+ 5.0 // drivetrain max speed
};
break;
case kPracticeTeamNumber:
@@ -117,30 +128,39 @@
kPracticeLeftDriveShifter,
kPracticeRightDriveShifter,
false,
- control_loops::MakeVDogDrivetrainLoop,
- control_loops::MakeDogDrivetrainLoop,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
// ShooterLimits
- // TODO(ben): make these real numbers
{-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_off_speed,
- shooter_zeroing_speed
+ shooter_zeroing_speed,
+ shooter_unload_speed
},
- {0.400000,
- 0.200000,
- 0.000000,
- -0.762218,
- 0.912207,
- -0.362218,
- 0.512207,
- {-1.682379, 1.043334, -1.282379, 0.643334, {-1.7, -1.544662, -1.7, -1.547616}, {-0.130218, -0.019771, -0.132036, -0.018862}, {0.935842, 1.1, 0.932660, 1.1}},
- {-1.225821, 1.553752, -0.825821, 1.153752, {-1.3, -1.088331, -1.3, -1.088331}, {-0.134536, -0.018408, -0.136127, -0.019771}, {1.447396, 1.6, 1.443987, 1.6}},
- 0.020000, // claw_unimportant_epsilon
- -0.200000, // start_fine_tune_pos
+ {0.400000 * 2.0,
+ 0.200000 * 2.0,
+ 0.000000 * 2.0,
+ -0.762218 * 2.0,
+ 0.912207 * 2.0,
+ -0.849484,
+ 1.42308,
+ {-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,
- }
+ },
+ //TODO(james): Get realer numbers for shooter_action.
+ {0.07, 0.15}, // shooter_action
+ 0.02, // drivetrain done delta
+ 5.0 // drivetrain max speed
};
break;
default:
@@ -148,6 +168,12 @@
}
}
+const Values *DoGetValues() {
+ uint16_t team = ::aos::network::GetTeamNumber();
+ LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+ return DoGetValuesForTeam(team);
+}
+
} // namespace
const Values &GetValues() {
@@ -155,5 +181,9 @@
return *once.Get();
}
+const Values &GetValuesForTeam(uint16_t team_number) {
+ return *(DoGetValuesForTeam(team_number));
+}
+
} // namespace constants
} // namespace frc971
diff --git a/frc971/constants.h b/frc971/constants.h
index f696e8b..3a37aa7 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -11,16 +11,15 @@
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
-const uint16_t kCompTeamNumber = 8971;
-const uint16_t kPracticeTeamNumber = 971;
-
// Contains the voltages for an analog hall effect sensor on a shifter.
struct ShifterHallEffect {
// The numbers to use for scaling raw voltages to 0-1.
- double high, low;
+ // Low is near 0.0, high is near 1.0
+ double low_gear_middle, low_gear_low;
+ double high_gear_high, high_gear_middle;
// The numbers for when the dog is clear of each gear.
- double clear_high, clear_low;
+ double clear_low, clear_high;
};
// This structure contains current values for all of the things that change.
@@ -59,8 +58,8 @@
AnglePair plunger_back;
AnglePair pusher_distal;
AnglePair pusher_proximal;
- double zeroing_off_speed;
double zeroing_speed;
+ double unload_speed;
};
Shooter shooter;
@@ -99,10 +98,26 @@
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;
+ double drivetrain_done_distance;
+ double drivetrain_max_speed;
};
// Creates (once) a Values instance and returns a reference to it.
const Values &GetValues();
+const Values &GetValuesForTeam(uint16_t team_number);
} // namespace constants
} // namespace frc971
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index b804399..58b2ca0 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -4,6 +4,7 @@
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
@@ -34,7 +35,8 @@
// correct side of the zero and go zero.
// Valid region plan.
-// Difference between the arms has a range, and the values of each arm has a range.
+// Difference between the arms has a range, and the values of each arm has a
+// range.
// If a claw runs up against a static limit, don't let the goal change outside
// the limit.
// If a claw runs up against a movable limit, move both claws outwards to get
@@ -170,9 +172,9 @@
void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition &claw) {
set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- front_.Reset();
- calibration_.Reset();
- back_.Reset();
+ front_.Reset(claw.front);
+ calibration_.Reset(claw.calibration);
+ back_.Reset(claw.back);
// close up the min and max edge positions as they are no longer valid and
// will be expanded in future iterations
min_hall_effect_on_angle_ = claw.position;
@@ -211,7 +213,8 @@
}
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
- : aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
+ : aos::control_loops::ControlLoop<control_loops::ClawGroup, true, true,
+ false>(my_claw),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
@@ -226,17 +229,54 @@
const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
+bool ZeroedStateFeedbackLoop::SawFilteredPosedge(
+ const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB) {
+ if (posedge_filter_ == nullptr && this_sensor.posedge_count_changed() &&
+ !sensorA.posedge_count_changed() && !sensorB.posedge_count_changed() &&
+ this_sensor.value() && !this_sensor.last_value()) {
+ posedge_filter_ = &this_sensor;
+ } else if (posedge_filter_ == &this_sensor &&
+ !this_sensor.posedge_count_changed() &&
+ !sensorA.posedge_count_changed() &&
+ !sensorB.posedge_count_changed() && this_sensor.value()) {
+ posedge_filter_ = nullptr;
+ return true;
+ } else if (posedge_filter_ == &this_sensor) {
+ posedge_filter_ = nullptr;
+ }
+ return false;
+}
+
+bool ZeroedStateFeedbackLoop::SawFilteredNegedge(
+ const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB) {
+ if (negedge_filter_ == nullptr && this_sensor.negedge_count_changed() &&
+ !sensorA.negedge_count_changed() && !sensorB.negedge_count_changed() &&
+ !this_sensor.value() && this_sensor.last_value()) {
+ negedge_filter_ = &this_sensor;
+ } else if (negedge_filter_ == &this_sensor &&
+ !this_sensor.negedge_count_changed() &&
+ !sensorA.negedge_count_changed() &&
+ !sensorB.negedge_count_changed() && !this_sensor.value()) {
+ negedge_filter_ = nullptr;
+ return true;
+ } else if (negedge_filter_ == &this_sensor) {
+ negedge_filter_ = nullptr;
+ }
+ return false;
+}
+
bool ZeroedStateFeedbackLoop::DoGetPositionOfEdge(
const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
- double *edge_angle, const HallEffectTracker &sensor,
+ double *edge_angle, const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA, const HallEffectTracker &sensorB,
const char *hall_effect_name) {
bool found_edge = false;
- if (sensor.posedge_count_changed()) {
+ if (SawFilteredPosedge(this_sensor, sensorA, sensorB)) {
if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
- // we oddly got two of the same edge.
- *edge_angle = last_edge_value_;
- found_edge = true;
+ LOG(WARNING, "%s: Uncertain which side, rejecting posedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
@@ -251,14 +291,14 @@
name_, hall_effect_name, *edge_angle, posedge_value_,
average_last_encoder);
}
- }
- *edge_encoder = posedge_value_;
- found_edge = true;
- }
- if (sensor.negedge_count_changed()) {
- if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
- *edge_angle = last_edge_value_;
+ *edge_encoder = posedge_value_;
found_edge = true;
+ }
+ }
+
+ if (SawFilteredNegedge(this_sensor, sensorA, sensorB)) {
+ if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
+ LOG(WARNING, "%s: Uncertain which side, rejecting negedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
@@ -274,12 +314,8 @@
average_last_encoder);
}
*edge_encoder = negedge_value_;
+ found_edge = true;
}
- found_edge = true;
- }
-
- if (found_edge) {
- last_edge_value_ = *edge_angle;
}
return found_edge;
@@ -288,23 +324,16 @@
bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
const constants::Values::Claws::Claw &claw_values, double *edge_encoder,
double *edge_angle) {
- // TODO(austin): Validate that the hall effect edge makes sense.
- // We must now be on the side of the edge that we expect to be, and the
- // encoder must have been on either side of the edge before and after.
-
- // TODO(austin): Compute the last off range min and max and compare the edge
- // value to the middle of the range. This will be quite a bit more reliable.
-
- if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle,
- front_, "front")) {
+ if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle, front_,
+ calibration_, back_, "front")) {
return true;
}
if (DoGetPositionOfEdge(claw_values.calibration, edge_encoder, edge_angle,
- calibration_, "calibration")) {
+ calibration_, front_, back_, "calibration")) {
return true;
}
- if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle,
- back_, "back")) {
+ if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle, back_,
+ calibration_, front_, "back")) {
return true;
}
return false;
@@ -358,14 +387,12 @@
const double separation = *top_goal - *bottom_goal;
if (separation > values.claw.claw_max_separation) {
- LOG(DEBUG, "Greater than\n");
const double dsep = (separation - values.claw.claw_max_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
LOG(DEBUG, "Goals now bottom: %f, top: %f\n", *bottom_goal, *top_goal);
}
if (separation < values.claw.claw_min_separation) {
- LOG(DEBUG, "Less than\n");
const double dsep = (separation - values.claw.claw_min_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
@@ -396,7 +423,8 @@
return (
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (::aos::robot_state->autonomous &&
+ (((::aos::robot_state.get() == NULL) ? true
+ : ::aos::robot_state->autonomous) &&
((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -411,7 +439,7 @@
void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
control_loops::ClawGroup::Output *output,
- ::aos::control_loops::Status *status) {
+ control_loops::ClawGroup::Status *status) {
constexpr double dt = 0.01;
// Disable the motors now so that all early returns will return with the
@@ -420,6 +448,15 @@
output->top_claw_voltage = 0;
output->bottom_claw_voltage = 0;
output->intake_voltage = 0;
+ output->tusk_voltage = 0;
+ }
+
+ if (goal) {
+ if (::std::isnan(goal->bottom_angle) ||
+ ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
+ ::std::isnan(goal->centering)) {
+ return;
+ }
}
if (reset()) {
@@ -427,10 +464,6 @@
bottom_claw_.Reset(position->bottom);
}
- if (::aos::robot_state.get() == nullptr) {
- return;
- }
-
const frc971::constants::Values &values = constants::GetValues();
if (position) {
@@ -454,25 +487,33 @@
initial_separation_ =
top_claw_.absolute_position() - bottom_claw_.absolute_position();
}
- LOG(DEBUG, "Claw position is (top: %f bottom: %f\n",
- top_claw_.absolute_position(), bottom_claw_.absolute_position());
+ LOG_STRUCT(DEBUG, "absolute position",
+ ClawPositionToLog(top_claw_.absolute_position(),
+ bottom_claw_.absolute_position()));
}
- const bool autonomous = ::aos::robot_state->autonomous;
- const bool enabled = ::aos::robot_state->enabled;
+ bool autonomous, enabled;
+ if (::aos::robot_state.get() == nullptr) {
+ autonomous = true;
+ enabled = false;
+ } else {
+ autonomous = ::aos::robot_state->autonomous;
+ enabled = ::aos::robot_state->enabled;
+ }
double bottom_claw_velocity_ = 0.0;
double top_claw_velocity_ = 0.0;
- if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
- bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (autonomous &&
- ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
- top_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
- (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
- bottom_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::DISABLED_CALIBRATION)))) {
+ if (goal != NULL &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
+ bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
+ (autonomous &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))))) {
// Ready to use the claw.
// Limit the goals here.
bottom_claw_goal_ = goal->bottom_angle;
@@ -483,9 +524,9 @@
mode_ = READY;
} else if (top_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
bottom_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
// Time to fine tune the zero.
// Limit the goals here.
if (!enabled) {
@@ -527,23 +568,22 @@
mode_ = PREP_FINE_TUNE_BOTTOM;
}
- if (bottom_claw_.calibration().value()) {
- if (bottom_claw_.calibration().posedge_count_changed() &&
- position) {
- // do calibration
- bottom_claw_.SetCalibration(
- position->bottom.posedge_value,
- values.claw.lower_claw.calibration.lower_angle);
- bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
- // calibrated so we are done fine tuning bottom
- doing_calibration_fine_tune_ = false;
- LOG(DEBUG, "Calibrated the bottom correctly!\n");
- } else {
- doing_calibration_fine_tune_ = false;
- bottom_claw_goal_ = values.claw.start_fine_tune_pos;
- top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
- mode_ = PREP_FINE_TUNE_BOTTOM;
- }
+ if (position && bottom_claw_.SawFilteredPosedge(
+ bottom_claw_.calibration(), bottom_claw_.front(),
+ bottom_claw_.back())) {
+ // do calibration
+ bottom_claw_.SetCalibration(
+ position->bottom.posedge_value,
+ values.claw.lower_claw.calibration.lower_angle);
+ bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning bottom
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the bottom correctly!\n");
+ } else if (bottom_claw_.calibration().last_value()) {
+ doing_calibration_fine_tune_ = false;
+ bottom_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
+ mode_ = PREP_FINE_TUNE_BOTTOM;
} else {
LOG(DEBUG, "Fine tuning\n");
}
@@ -583,23 +623,23 @@
LOG(DEBUG, "Found a limit, starting over.\n");
mode_ = PREP_FINE_TUNE_TOP;
}
- if (top_claw_.calibration().value()) {
- if (top_claw_.calibration().posedge_count_changed() &&
- position) {
- // do calibration
- top_claw_.SetCalibration(
- position->top.posedge_value,
- values.claw.upper_claw.calibration.lower_angle);
- top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
- // calibrated so we are done fine tuning top
- doing_calibration_fine_tune_ = false;
- LOG(DEBUG, "Calibrated the top correctly!\n");
- } else {
- doing_calibration_fine_tune_ = false;
- top_claw_goal_ = values.claw.start_fine_tune_pos;
- top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
- mode_ = PREP_FINE_TUNE_TOP;
- }
+
+ if (position &&
+ top_claw_.SawFilteredPosedge(top_claw_.calibration(),
+ top_claw_.front(), top_claw_.back())) {
+ // do calibration
+ top_claw_.SetCalibration(
+ position->top.posedge_value,
+ values.claw.upper_claw.calibration.lower_angle);
+ top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning top
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the top correctly!\n");
+ } else if (top_claw_.calibration().last_value()) {
+ doing_calibration_fine_tune_ = false;
+ top_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
+ mode_ = PREP_FINE_TUNE_TOP;
}
}
// now set the bottom claw to track
@@ -609,8 +649,8 @@
doing_calibration_fine_tune_ = false;
if (!was_enabled_ && enabled) {
if (position) {
- top_claw_goal_ = position->top.position;
- bottom_claw_goal_ = position->bottom.position;
+ top_claw_goal_ = position->top.position + top_claw_.offset();
+ bottom_claw_goal_ = position->bottom.position + bottom_claw_.offset();
initial_separation_ =
position->top.position - position->bottom.position;
} else {
@@ -644,18 +684,26 @@
}
}
- if (enabled) {
- top_claw_.SetCalibrationOnEdge(
- values.claw.upper_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
- bottom_claw_.SetCalibrationOnEdge(
- values.claw.lower_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
- } else {
- // TODO(austin): Only calibrate on the predetermined edge.
- // We might be able to just ignore this since the backlash is soooo low. :)
- top_claw_.SetCalibrationOnEdge(
- values.claw.upper_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
- bottom_claw_.SetCalibrationOnEdge(
- values.claw.lower_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ if (position) {
+ if (enabled) {
+ top_claw_.SetCalibrationOnEdge(
+ values.claw.upper_claw,
+ ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.claw.lower_claw,
+ ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ } else {
+ // TODO(austin): Only calibrate on the predetermined edge.
+ // We might be able to just ignore this since the backlash is soooo
+ // low.
+ // :)
+ top_claw_.SetCalibrationOnEdge(
+ values.claw.upper_claw,
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.claw.lower_claw,
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ }
}
mode_ = UNKNOWN_LOCATION;
}
@@ -672,8 +720,8 @@
if (position != nullptr) {
separation = position->top.position - position->bottom.position;
}
- LOG(DEBUG, "Goal is %f (bottom) %f, separation is %f\n", claw_.R(0, 0),
- claw_.R(1, 0), separation);
+ LOG_STRUCT(DEBUG, "actual goal",
+ ClawGoalToLog(claw_.R(0, 0), claw_.R(1, 0), separation));
// Only cap power when one of the halves of the claw is moving slowly and
// could wind up.
@@ -709,8 +757,9 @@
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
- LOG(DEBUG, "Uncapped is %f, max is %f, difference is %f\n",
+ LOG(DEBUG, "Moving the goal by %f to prevent windup."
+ " Uncapped is %f, max is %f, difference is %f\n",
+ dx,
claw_.uncapped_average_voltage(), values.claw.max_zeroing_voltage,
(claw_.uncapped_average_voltage() -
values.claw.max_zeroing_voltage));
@@ -735,8 +784,19 @@
}
if (output) {
+ if (goal) {
+ //setup the intake
+ output->intake_voltage =
+ (goal->intake > 12.0) ? 12 : (goal->intake < -12.0) ? -12.0
+ : goal->intake;
+ output->tusk_voltage = goal->centering;
+ output->tusk_voltage =
+ (goal->centering > 12.0) ? 12 : (goal->centering < -12.0)
+ ? -12.0
+ : goal->centering;
+ }
output->top_claw_voltage = claw_.U(1, 0);
- output->bottom_claw_voltage = claw_.U(0, 0);
+ output->bottom_claw_voltage = claw_.U(0, 0);
if (output->top_claw_voltage > kMaxVoltage) {
output->top_claw_voltage = kMaxVoltage;
@@ -750,10 +810,41 @@
output->bottom_claw_voltage = -kMaxVoltage;
}
}
- status->done = false;
- was_enabled_ = ::aos::robot_state->enabled;
+ status->bottom = bottom_absolute_position();
+ status->separation = top_absolute_position() - bottom_absolute_position();
+ status->bottom_velocity = claw_.X_hat(2, 0);
+ status->separation_velocity = claw_.X_hat(3, 0);
+
+ if (goal) {
+ bool bottom_done =
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
+ bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
+ bool separation_done =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.020;
+ bool separation_done_with_ball =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.06;
+ status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
+ status->done_with_ball =
+ is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ } else {
+ status->done = status->done_with_ball = false;
+ }
+
+ status->zeroed = is_ready();
+ status->zeroed_for_auto =
+ (top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+
+ was_enabled_ = enabled;
}
} // namespace control_loops
} // namespace frc971
+
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index f71bf6b..bdce229 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -31,6 +31,7 @@
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/aos/build/externals.gyp:libcdd',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'claw_loop',
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 48650b4..dab1dca 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -75,10 +75,6 @@
void Reset(const HalfClawPosition &claw);
- bool ready() {
- return front_.ready() && calibration_.ready() && back_.ready();
- }
-
double absolute_position() const { return encoder() + offset(); }
const HallEffectTracker &front() const { return front_; }
@@ -107,6 +103,14 @@
bool GetPositionOfEdge(const constants::Values::Claws::Claw &claw,
double *edge_encoder, double *edge_angle);
+ bool SawFilteredPosedge(const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB);
+
+ bool SawFilteredNegedge(const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB);
+
#undef COUNT_SETTER_GETTER
protected:
@@ -121,7 +125,6 @@
JointZeroingState zeroing_state_;
double posedge_value_;
double negedge_value_;
- double last_edge_value_;
double min_hall_effect_on_angle_;
double max_hall_effect_on_angle_;
double min_hall_effect_off_angle_;
@@ -132,11 +135,16 @@
double last_off_encoder_;
bool any_triggered_last_;
+ const HallEffectTracker* posedge_filter_ = nullptr;
+ const HallEffectTracker* negedge_filter_ = nullptr;
+
private:
// Does the edges of 1 sensor for GetPositionOfEdge.
bool DoGetPositionOfEdge(const constants::Values::Claws::AnglePair &angles,
double *edge_encoder, double *edge_angle,
const HallEffectTracker &sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB,
const char *hall_effect_name);
};
@@ -164,8 +172,8 @@
JointZeroingState zeroing_state);
};
-class ClawMotor
- : public aos::control_loops::ControlLoop<control_loops::ClawGroup> {
+class ClawMotor : public aos::control_loops::ControlLoop<
+ control_loops::ClawGroup, true, true, false> {
public:
explicit ClawMotor(control_loops::ClawGroup *my_claw =
&control_loops::claw_queue_group);
@@ -201,7 +209,7 @@
virtual void RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
control_loops::ClawGroup::Output *output,
- ::aos::control_loops::Status *status);
+ control_loops::ClawGroup::Status *status);
double top_absolute_position() const {
return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 333f83c..116a182 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -31,7 +31,10 @@
double bottom_angle;
// How much higher the top claw is.
double separation_angle;
- bool intake;
+ // top claw intake roller
+ double intake;
+ // bottom claw tusk centering
+ double centering;
};
message Position {
@@ -48,10 +51,38 @@
double tusk_voltage;
};
+ message Status {
+ // True if zeroed enough for the current period (autonomous or teleop).
+ bool zeroed;
+ // True if zeroed as much as we will force during autonomous.
+ bool zeroed_for_auto;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ bool done;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ // seperation allowance much wider as a ball may be included
+ bool done_with_ball;
+ // Dump the values of the state matrix.
+ double bottom;
+ double bottom_velocity;
+ double separation;
+ double separation_velocity;
+ };
+
queue Goal goal;
queue Position position;
queue Output output;
- queue aos.control_loops.Status status;
+ queue Status status;
};
queue_group ClawGroup claw_queue_group;
+
+struct ClawPositionToLog {
+ double top;
+ double bottom;
+};
+
+struct ClawGoalToLog {
+ double bottom_pos;
+ double bottom_vel;
+ double separation;
+};
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index e35cd7f..8704ad0 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -231,6 +231,7 @@
v.claw.claw_max_separation);
EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
v.claw.claw_min_separation);
+ ::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
}
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
@@ -283,6 +284,7 @@
.reader_pid(254)
.cape_resets(5)
.Send();
+ ::aos::time::Time::EnableMockTime(::aos::time::Time::InSeconds(0.0));
}
void SendDSPacket(bool enabled) {
@@ -309,9 +311,23 @@
virtual ~ClawTest() {
::aos::robot_state.Clear();
::bbb::sensor_generation.Clear();
+ ::aos::time::Time::DisableMockTime();
}
};
+TEST_F(ClawTest, HandlesNAN) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(::std::nan(""))
+ .separation_angle(::std::nan(""))
+ .Send();
+ for (int i = 0; i < 500; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+}
+
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ClawTest, ZerosCorrectly) {
claw_queue_group.goal.MakeWithBuilder()
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index babbb04..106491d 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -25,9 +25,9 @@
StateFeedbackController<4, 2, 2> MakeClawController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.42518438347, 4.71027737605e-16, -1.42518438347, 1.04485564063, 30.6346010502, 1.00485917356e-14, -30.6346010502, 2.04727497147;
+ L << 1.48518438347, 2.30607329869e-16, -1.48518438347, 1.10485564063, 34.6171964667, 5.33831435952e-15, -34.6171964667, 3.52560374486;
Eigen::Matrix<double, 2, 4> K;
- K << 50.0, 0.0, 1.0, 0.0, 23.5668757858, 300.0, -0.88836718554, 1.1;
+ K << 104.272994613, 0.0, 1.72618753001, 0.0, 49.1477742369, 129.930293084, -0.546087759204, 0.551235956004;
return StateFeedbackController<4, 2, 2>(L, K, MakeClawPlantCoefficients());
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b54949d..c8b943c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -11,15 +11,16 @@
#include "aos/controls/polytope.h"
#include "aos/common/commonmath.h"
#include "aos/common/logging/queue_logging.h"
+#include "aos/common/logging/matrix_logging.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/constants.h"
-using frc971::sensors::gyro;
+using frc971::sensors::gyro_reading;
namespace frc971 {
namespace control_loops {
@@ -65,8 +66,36 @@
SetRawPosition(left, right);
}
+ void SetExternalMotors(double left_voltage, double right_voltage) {
+ loop_->U << left_voltage, right_voltage;
+ }
+
void Update(bool stop_motors) {
- loop_->Update(stop_motors);
+ if (_control_loop_driving) {
+ loop_->Update(stop_motors);
+ } else {
+ if (stop_motors) {
+ loop_->U.setZero();
+ loop_->U_uncapped.setZero();
+ }
+ loop_->UpdateObserver();
+ }
+ ::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
+ LOG_MATRIX(DEBUG, "E", E);
+ }
+
+ double GetEstimatedRobotSpeed() {
+ // lets just call the average of left and right velocities close enough
+ return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
+ }
+
+ double GetEstimatedLeftEncoder() {
+ // lets just call the average of left and right velocities close enough
+ return loop_->X_hat(0, 0);
+ }
+
+ double GetEstimatedRightEncoder() {
+ return loop_->X_hat(2, 0);
}
void SendMotors(Drivetrain::Output *output) {
@@ -75,10 +104,6 @@
output->right_voltage = loop_->U(1, 0);
}
}
- void PrintMotors() const {
- ::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
- LOG(DEBUG, "E[0, 0]: %f E[1, 0] %f E[2, 0] %f E[3, 0] %f\n", E(0, 0), E(1, 0), E(2, 0), E(3, 0));
- }
private:
::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> loop_;
@@ -137,8 +162,6 @@
/*[*/ 12 /*]]*/).finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(
constants::GetValues().make_v_drivetrain_loop())),
- left_cim_(new StateFeedbackLoop<1, 1, 1>(MakeCIMLoop())),
- right_cim_(new StateFeedbackLoop<1, 1, 1>(MakeCIMLoop())),
ttrust_(1.1),
wheel_(0.0),
throttle_(0.0),
@@ -154,18 +177,24 @@
}
static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
- static double MotorSpeed(double shifter_position, double velocity) {
+ static double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
+ double shifter_position, double velocity) {
// TODO(austin): G_high, G_low and kWheelRadius
- if (shifter_position > 0.57) {
+ const double avg_hall_effect =
+ (hall_effect.clear_high + hall_effect.clear_low) / 2.0;
+
+ if (shifter_position > avg_hall_effect) {
return velocity / constants::GetValues().high_gear_ratio / kWheelRadius;
} else {
return velocity / constants::GetValues().low_gear_ratio / kWheelRadius;
}
}
- Gear ComputeGear(double velocity, Gear current) {
- const double low_omega = MotorSpeed(0, ::std::abs(velocity));
- const double high_omega = MotorSpeed(1.0, ::std::abs(velocity));
+ Gear ComputeGear(const constants::ShifterHallEffect &hall_effect,
+ double velocity, Gear current) {
+ const double low_omega = MotorSpeed(hall_effect, 0.0, ::std::abs(velocity));
+ const double high_omega =
+ MotorSpeed(hall_effect, 1.0, ::std::abs(velocity));
double high_torque = ((12.0 - high_omega / Kv) * Kt / kR);
double low_torque = ((12.0 - low_omega / Kv) * Kt / kR);
@@ -200,6 +229,7 @@
// TODO(austin): Fix the upshift logic to include states.
Gear requested_gear;
if (false) {
+ const auto &values = constants::GetValues();
const double current_left_velocity =
(position_.left_encoder - last_position_.left_encoder) /
position_time_delta_;
@@ -207,8 +237,10 @@
(position_.right_encoder - last_position_.right_encoder) /
position_time_delta_;
- Gear left_requested = ComputeGear(current_left_velocity, left_gear_);
- Gear right_requested = ComputeGear(current_right_velocity, right_gear_);
+ Gear left_requested =
+ ComputeGear(values.left_drive, current_left_velocity, left_gear_);
+ Gear right_requested =
+ ComputeGear(values.right_drive, current_right_velocity, right_gear_);
requested_gear =
(left_requested == HIGH || right_requested == HIGH) ? HIGH : LOW;
} else {
@@ -252,6 +284,7 @@
}
}
void SetPosition(const Drivetrain::Position *position) {
+ const auto &values = constants::GetValues();
if (position == NULL) {
++stale_count_;
} else {
@@ -264,9 +297,14 @@
if (position) {
GearLogging gear_logging;
// Switch to the correct controller.
- // TODO(austin): Un-hard code 0.57
- if (position->left_shifter_position < 0.57) {
- if (position->right_shifter_position < 0.57 || right_gear_ == LOW) {
+ const double left_middle_shifter_position =
+ (values.left_drive.clear_high + values.left_drive.clear_low) / 2.0;
+ const double right_middle_shifter_position =
+ (values.right_drive.clear_high + values.right_drive.clear_low) / 2.0;
+
+ if (position->left_shifter_position < left_middle_shifter_position) {
+ if (position->right_shifter_position < right_middle_shifter_position ||
+ right_gear_ == LOW) {
gear_logging.left_loop_high = false;
gear_logging.right_loop_high = false;
loop_->set_controller_index(gear_logging.controller_index = 0);
@@ -276,7 +314,8 @@
loop_->set_controller_index(gear_logging.controller_index = 1);
}
} else {
- if (position->right_shifter_position < 0.57 || left_gear_ == LOW) {
+ if (position->right_shifter_position < right_middle_shifter_position ||
+ left_gear_ == LOW) {
gear_logging.left_loop_high = true;
gear_logging.right_loop_high = false;
loop_->set_controller_index(gear_logging.controller_index = 2);
@@ -288,16 +327,16 @@
}
// TODO(austin): Constants.
- if (position->left_shifter_position > 0.9 && left_gear_ == SHIFTING_UP) {
+ if (position->left_shifter_position > values.left_drive.clear_high && left_gear_ == SHIFTING_UP) {
left_gear_ = HIGH;
}
- if (position->left_shifter_position < 0.1 && left_gear_ == SHIFTING_DOWN) {
+ if (position->left_shifter_position < values.left_drive.clear_low && left_gear_ == SHIFTING_DOWN) {
left_gear_ = LOW;
}
- if (position->right_shifter_position > 0.9 && right_gear_ == SHIFTING_UP) {
+ if (position->right_shifter_position > values.right_drive.clear_high && right_gear_ == SHIFTING_UP) {
right_gear_ = HIGH;
}
- if (position->right_shifter_position < 0.1 && right_gear_ == SHIFTING_DOWN) {
+ if (position->right_shifter_position < values.right_drive.clear_low && right_gear_ == SHIFTING_DOWN) {
right_gear_ = LOW;
}
@@ -355,6 +394,7 @@
}
void Update() {
+ const auto &values = constants::GetValues();
// TODO(austin): Observer for the current velocity instead of difference
// calculations.
++counter_;
@@ -365,9 +405,11 @@
(position_.right_encoder - last_position_.right_encoder) /
position_time_delta_;
const double left_motor_speed =
- MotorSpeed(position_.left_shifter_position, current_left_velocity);
+ MotorSpeed(values.left_drive, position_.left_shifter_position,
+ current_left_velocity);
const double right_motor_speed =
- MotorSpeed(position_.right_shifter_position, current_right_velocity);
+ MotorSpeed(values.right_drive, position_.right_shifter_position,
+ current_right_velocity);
{
CIMLogging logging;
@@ -376,7 +418,6 @@
// shift.
if (IsInGear(left_gear_)) {
logging.left_in_gear = true;
- left_cim_->X_hat(0, 0) = left_motor_speed;
} else {
logging.left_in_gear = false;
}
@@ -384,7 +425,6 @@
logging.left_velocity = current_left_velocity;
if (IsInGear(right_gear_)) {
logging.right_in_gear = true;
- right_cim_->X_hat(0, 0) = right_motor_speed;
} else {
logging.right_in_gear = false;
}
@@ -451,23 +491,19 @@
} else {
// Any motor is not in gear. Speed match.
::Eigen::Matrix<double, 1, 1> R_left;
- R_left(0, 0) = left_motor_speed;
- const double wiggle =
- (static_cast<double>((counter_ % 4) / 2) - 0.5) * 3.5;
-
- loop_->U(0, 0) =
- ::aos::Clip((R_left / Kv)(0, 0) + wiggle, -position_.battery_voltage,
- position_.battery_voltage);
- right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat +
- right_cim_->B() * loop_->U(0, 0);
-
::Eigen::Matrix<double, 1, 1> R_right;
+ R_left(0, 0) = left_motor_speed;
R_right(0, 0) = right_motor_speed;
- loop_->U(1, 0) =
- ::aos::Clip((R_right / Kv)(0, 0) + wiggle, -position_.battery_voltage,
- position_.battery_voltage);
- right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat +
- right_cim_->B() * loop_->U(1, 0);
+
+ const double wiggle =
+ (static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
+
+ loop_->U(0, 0) = ::aos::Clip(
+ (R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
+ loop_->U(1, 0) = ::aos::Clip(
+ (R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
loop_->U *= 12.0 / position_.battery_voltage;
}
}
@@ -485,8 +521,6 @@
const ::aos::controls::HPolytope<2> U_Poly_;
::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
- ::std::unique_ptr<StateFeedbackLoop<1, 1, 1>> left_cim_;
- ::std::unique_ptr<StateFeedbackLoop<1, 1, 1>> right_cim_;
const double ttrust_;
double wheel_;
@@ -516,7 +550,7 @@
void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
const Drivetrain::Position *position,
Drivetrain::Output *output,
- Drivetrain::Status * /*status*/) {
+ Drivetrain::Status * status) {
// TODO(aschuh): These should be members of the class.
static DrivetrainMotorsSS dt_closedloop;
static PolyDrivetrain dt_openloop;
@@ -526,6 +560,7 @@
LOG_INTERVAL(no_position_);
bad_pos = true;
}
+ no_position_.Print();
double wheel = goal->steering;
double throttle = goal->throttle;
@@ -541,22 +576,43 @@
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
- if (gyro.FetchLatest()) {
- LOG_STRUCT(DEBUG, "using", *gyro);
- dt_closedloop.SetPosition(left_encoder, right_encoder, gyro->angle,
- control_loop_driving);
+ if (gyro_reading.FetchLatest()) {
+ LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+ dt_closedloop.SetPosition(left_encoder, right_encoder,
+ gyro_reading->angle, control_loop_driving);
} else {
dt_closedloop.SetRawPosition(left_encoder, right_encoder);
}
}
dt_openloop.SetPosition(position);
- dt_closedloop.Update(output == NULL);
dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
dt_openloop.Update();
+
if (control_loop_driving) {
+ dt_closedloop.Update(output == NULL);
dt_closedloop.SendMotors(output);
} else {
dt_openloop.SendMotors(output);
+ if (output) {
+ dt_closedloop.SetExternalMotors(output->left_voltage,
+ output->right_voltage);
+ }
+ dt_closedloop.Update(output == NULL);
+ }
+
+ // set the output status of the controll loop state
+ if (status) {
+ bool done = false;
+ if (goal) {
+ done = ((::std::abs(goal->left_goal -
+ dt_closedloop.GetEstimatedLeftEncoder()) <
+ constants::GetValues().drivetrain_done_distance) &&
+ (::std::abs(goal->right_goal -
+ dt_closedloop.GetEstimatedRightEncoder()) <
+ constants::GetValues().drivetrain_done_distance));
+ }
+ status->is_done = done;
+ status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index 5941f63..bf73d9f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -22,9 +22,7 @@
'type': 'static_library',
'sources': [
'polydrivetrain_dog_motor_plant.cc',
- 'polydrivetrain_clutch_motor_plant.cc',
'drivetrain_dog_motor_plant.cc',
- 'drivetrain_clutch_motor_plant.cc',
],
'dependencies': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
@@ -50,6 +48,7 @@
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/common/util/util.gyp:log_interval',
'<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/logging/logging.gyp:matrix_logging',
],
'export_dependent_settings': [
'<(DEPTH)/aos/build/externals.gyp:libcdd',
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 443282c..50d9dbf 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -45,12 +45,13 @@
message Output {
float left_voltage;
float right_voltage;
- bool left_high;
- bool right_high;
+ bool left_high;
+ bool right_high;
};
message Status {
bool is_done;
+ double robot_speed;
};
queue Goal goal;
diff --git a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc
deleted file mode 100644
index b3aa088..0000000
--- a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeClutchDrivetrainPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00876671940282, 0.0, 0.000204905465153, 0.0, 0.764245148008, 0.0, 0.0373841350548, 0.0, 0.000204905465153, 1.0, 0.00876671940282, 0.0, 0.0373841350548, 0.0, 0.764245148008;
- Eigen::Matrix<double, 4, 2> B;
- B << 0.000157874070659, -2.62302512161e-05, 0.0301793267864, -0.00478559834045, -2.62302512161e-05, 0.000157874070659, -0.00478559834045, 0.0301793267864;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<4, 2, 2> MakeClutchDrivetrainController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.60424514801, 0.0373841350548, 53.4463554671, 4.58647914599, 0.0373841350548, 1.60424514801, 4.58647914599, 53.4463554671;
- Eigen::Matrix<double, 2, 4> K;
- K << 292.330461448, 10.4890095334, -85.5980253252, -0.517234397951, -58.0206391358, -1.5636023242, 153.384904309, 5.5616531565;
- return StateFeedbackController<4, 2, 2>(L, K, MakeClutchDrivetrainPlantCoefficients());
-}
-
-StateFeedbackPlant<4, 2, 2> MakeClutchDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeClutchDrivetrainPlantCoefficients());
- return StateFeedbackPlant<4, 2, 2>(plants);
-}
-
-StateFeedbackLoop<4, 2, 2> MakeClutchDrivetrainLoop() {
- ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeClutchDrivetrainController());
- return StateFeedbackLoop<4, 2, 2>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h
deleted file mode 100644
index e9444e6..0000000
--- a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeClutchDrivetrainPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeClutchDrivetrainController();
-
-StateFeedbackPlant<4, 2, 2> MakeClutchDrivetrainPlant();
-
-StateFeedbackLoop<4, 2, 2> MakeClutchDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index 7822056..231eefb 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -7,11 +7,11 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients() {
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00923787174605, 0.0, 0.000131162317098, 0.0, 0.851672729447, 0.0, 0.0248457251406, 0.0, 0.000131162317098, 1.0, 0.00923787174605, 0.0, 0.0248457251406, 0.0, 0.851672729447;
+ A << 1.0, 0.00860955515291, 0.0, 0.000228184998733, 0.0, 0.735841675858, 0.0, 0.0410810558113, 0.0, 0.000228184998733, 1.0, 0.00860955515291, 0.0, 0.0410810558113, 0.0, 0.735841675858;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000126364935405, -2.17474127771e-05, 0.0245934537462, -0.00411955394149, -2.17474127771e-05, 0.000126364935405, -0.00411955394149, 0.0245934537462;
+ B << 0.000272244648044, -4.46778919705e-05, 0.0517213538779, -0.00804353916233, -4.46778919705e-05, 0.000272244648044, -0.00804353916233, 0.0517213538779;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -23,23 +23,23 @@
return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<4, 2, 2> MakeDogDrivetrainController() {
+StateFeedbackController<4, 2, 2> MakeDrivetrainController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.69167272945, 0.0248457251406, 64.4706646869, 3.2355304474, 0.0248457251406, 1.69167272945, 3.2355304474, 64.4706646869;
+ L << 1.57584167586, 0.0410810558113, 50.0130674801, 4.93325200717, 0.0410810558113, 1.57584167586, 4.93325200717, 50.0130674801;
Eigen::Matrix<double, 2, 4> K;
- K << 248.918529922, 14.4460993245, 41.6953764051, 3.43594323497, 41.6953764051, 3.43594323497, 248.918529922, 14.4460993245;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDogDrivetrainPlantCoefficients());
+ K << 128.210620632, 6.93828382074, 5.11036686771, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
}
-StateFeedbackPlant<4, 2, 2> MakeDogDrivetrainPlant() {
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDogDrivetrainPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainPlantCoefficients());
return StateFeedbackPlant<4, 2, 2>(plants);
}
-StateFeedbackLoop<4, 2, 2> MakeDogDrivetrainLoop() {
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDogDrivetrainController());
+ controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainController());
return StateFeedbackLoop<4, 2, 2>(controllers);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
index ba3d584..3829e9a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -6,13 +6,13 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients();
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients();
-StateFeedbackController<4, 2, 2> MakeDogDrivetrainController();
+StateFeedbackController<4, 2, 2> MakeDrivetrainController();
-StateFeedbackPlant<4, 2, 2> MakeDogDrivetrainPlant();
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
-StateFeedbackLoop<4, 2, 2> MakeDogDrivetrainLoop();
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 73f8525..0827fec 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -13,7 +13,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/other_sensors.q.h"
using ::aos::time::Time;
@@ -50,7 +50,7 @@
// TODO(aschuh) Do we want to test the clutch one too?
DrivetrainSimulation()
: drivetrain_plant_(
- new StateFeedbackPlant<4, 2, 2>(MakeDogDrivetrainPlant())),
+ new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
my_drivetrain_loop_(".frc971.control_loops.drivetrain",
0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
".frc971.control_loops.drivetrain.position",
@@ -134,7 +134,7 @@
.reader_pid(254)
.cape_resets(5)
.Send();
- ::frc971::sensors::gyro.Clear();
+ ::frc971::sensors::gyro_reading.Clear();
SendDSPacket(true);
}
@@ -158,7 +158,7 @@
virtual ~DrivetrainTest() {
::aos::robot_state.Clear();
- ::frc971::sensors::gyro.Clear();
+ ::frc971::sensors::gyro_reading.Clear();
::bbb::sensor_generation.Clear();
}
};
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc
deleted file mode 100644
index 82962f0..0000000
--- a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc
+++ /dev/null
@@ -1,125 +0,0 @@
-#include "frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.764245148008, 0.0373841350548, 0.0373841350548, 0.764245148008;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0301793267864, -0.00478559834045, -0.00478559834045, 0.0301793267864;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.763446428918, 0.00494258902788, 0.042202491067, 0.968991856576;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0302815719967, -0.00184882243178, -0.00540240320973, 0.011598890947;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.968991856576, 0.042202491067, 0.00494258902788, 0.763446428918;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.011598890947, -0.00540240320973, -0.00184882243178, 0.0302815719967;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.968881997557, 0.00555499847336, 0.00555499847336, 0.968881997557;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0116399847578, -0.0020779000091, -0.0020779000091, 0.0116399847578;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.744245148008, 0.0373841350548, 0.0373841350548, 0.744245148008;
- Eigen::Matrix<double, 2, 2> K;
- K << 5.78417881324, 2.15594244513, 2.15594244513, 5.78417881324;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainLowLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.742469928763, 0.0421768815418, 0.0421768815418, 0.949968356732;
- Eigen::Matrix<double, 2, 2> K;
- K << 5.78418649682, 2.16715237139, 6.33258809821, 32.8220766317;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainLowHighPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.954934950673, 0.00591596315544, 0.00591596315544, 0.737503334821;
- Eigen::Matrix<double, 2, 2> K;
- K << 32.8220766317, 6.33258809821, 2.16715237139, 5.78418649682;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainHighLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.948881997557, 0.00555499847336, 0.00555499847336, 0.948881997557;
- Eigen::Matrix<double, 2, 2> K;
- K << 32.8220767657, 6.33643373411, 6.33643373411, 32.8220767657;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainHighHighPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 2, 2> MakeVClutchDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<2, 2, 2>(plants);
-}
-
-StateFeedbackLoop<2, 2, 2> MakeVClutchDrivetrainLoop() {
- ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainHighHighController());
- return StateFeedbackLoop<2, 2, 2>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h
deleted file mode 100644
index 85c87c1..0000000
--- a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowHighController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighHighController();
-
-StateFeedbackPlant<2, 2, 2> MakeVClutchDrivetrainPlant();
-
-StateFeedbackLoop<2, 2, 2> MakeVClutchDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index b3d4277..b31cf85 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -7,11 +7,11 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowLowPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.851672729447, 0.0248457251406, 0.0248457251406, 0.851672729447;
+ A << 0.735841675858, 0.0410810558113, 0.0410810558113, 0.735841675858;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0245934537462, -0.00411955394149, -0.00411955394149, 0.0245934537462;
+ B << 0.0517213538779, -0.00804353916233, -0.00804353916233, 0.0517213538779;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -23,11 +23,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowHighPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.851389310398, 0.00553670185935, 0.0264939835067, 0.967000817219;
+ A << 0.735048848179, 0.0131811893199, 0.045929121897, 0.915703853642;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0246404461385, -0.00200815724925, -0.00439284398274, 0.0119687766843;
+ B << 0.0518765869984, -0.00481755802263, -0.00899277497558, 0.0308091755839;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -39,11 +39,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighLowPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.967000817219, 0.0264939835067, 0.00553670185935, 0.851389310398;
+ A << 0.915703853642, 0.045929121897, 0.0131811893199, 0.735048848179;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0119687766843, -0.00439284398274, -0.00200815724925, 0.0246404461385;
+ B << 0.0308091755839, -0.00899277497558, -0.00481755802263, 0.0518765869984;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -55,11 +55,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighHighPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.966936300149, 0.00589655754287, 0.00589655754287, 0.966936300149;
+ A << 0.915439806567, 0.0146814193986, 0.0146814193986, 0.915439806567;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0119921769728, -0.00213867661221, -0.00213867661221, 0.0119921769728;
+ B << 0.0309056814511, -0.00536587314624, -0.00536587314624, 0.0309056814511;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -71,53 +71,53 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowLowController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.831672729447, 0.0248457251406, 0.0248457251406, 0.831672729447;
+ L << 0.715841675858, 0.0410810558113, 0.0410810558113, 0.715841675858;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7028500331, 2.80305051463, 2.80305051463, 10.7028500331;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowLowPlantCoefficients());
+ K << 2.81809403994, 1.23253744933, 1.23253744933, 2.81809403994;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.831852326508, 0.0264837489415, 0.0264837489415, 0.946537801108;
+ L << 0.715885457343, 0.0459077351335, 0.0459077351335, 0.894867244478;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7028511964, 2.80768406175, 6.14180888507, 31.6936764099;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowHighPlantCoefficients());
+ K << 2.81810038978, 1.23928174475, 2.31332592354, 10.6088017388;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.951097545753, 0.0063707209266, 0.0063707209266, 0.827292581863;
+ L << 0.902328849033, 0.014581304798, 0.014581304798, 0.708423852788;
Eigen::Matrix<double, 2, 2> K;
- K << 31.6936764099, 6.14180888507, 2.80768406175, 10.7028511964;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighLowPlantCoefficients());
+ K << 10.6088017388, 2.31332592354, 1.23928174475, 2.81810038978;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.946936300149, 0.00589655754287, 0.00589655754287, 0.946936300149;
+ L << 0.895439806567, 0.0146814193986, 0.0146814193986, 0.895439806567;
Eigen::Matrix<double, 2, 2> K;
- K << 31.6936764663, 6.14392885659, 6.14392885659, 31.6936764663;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighHighPlantCoefficients());
+ K << 10.6088022944, 2.31694961514, 2.31694961514, 10.6088022944;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
-StateFeedbackPlant<2, 2, 2> MakeVDogDrivetrainPlant() {
+StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant() {
::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainHighHighPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
return StateFeedbackPlant<2, 2, 2>(plants);
}
-StateFeedbackLoop<2, 2, 2> MakeVDogDrivetrainLoop() {
+StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop() {
::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainHighHighController());
+ controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
+ controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
+ controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
+ controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
return StateFeedbackLoop<2, 2, 2>(controllers);
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
index 613bff4..27aa4dd 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
@@ -6,25 +6,25 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowLowPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowLowController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowHighPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighLowPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighHighPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController();
-StateFeedbackPlant<2, 2, 2> MakeVDogDrivetrainPlant();
+StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant();
-StateFeedbackLoop<2, 2, 2> MakeVDogDrivetrainLoop();
+StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop();
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index 7e6617c..b63a34b 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -20,47 +20,42 @@
bool negedge_count_changed() const { return negedges_.count_changed(); }
bool value() const { return value_; }
+ bool last_value() const { return last_value_; }
void Update(const HallEffectStruct &position) {
+ last_value_ = value_;
value_ = position.current;
posedges_.update(position.posedge_count);
negedges_.update(position.negedge_count);
}
- void Reset() {
- posedges_.Reset();
- negedges_.Reset();
+ void Reset(const HallEffectStruct &position) {
+ posedges_.Reset(position.posedge_count);
+ negedges_.Reset(position.negedge_count);
+ value_ = position.current;
+ last_value_ = position.current;
}
- bool ready() { return posedges_.ready() && negedges_.ready(); }
-
private:
class {
public:
void update(int32_t count) {
- if (first_) {
- count_ = count;
- LOG(DEBUG, "First time through the hall effect, resetting\n");
- }
previous_count_ = count_;
count_ = count;
- first_ = false;
}
- void Reset() { first_ = true; }
+ void Reset(int32_t count) { count_ = count; }
- bool count_changed() const { return !first_ && previous_count_ != count_; }
+ bool count_changed() const { return previous_count_ != count_; }
int32_t count() const { return count_; }
- bool ready() { return !first_; }
-
private:
int32_t count_ = 0;
int32_t previous_count_ = 0;
- bool first_ = true;
} posedges_, negedges_;
bool value_ = false;
+ bool last_value_ = false;
};
} // namespace frc971
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index aacf31e..ca69a2b 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -23,6 +23,7 @@
# measured from CAD
self.J_top = 0.3
self.J_bottom = 0.9
+
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
@@ -144,8 +145,8 @@
print "eigenvalues"
print numpy.linalg.eig(F)[0]
- self.rpl = .05
- self.ipl = 0.008
+ self.rpl = .02
+ self.ipl = 0.004
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl,
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 90faf9f..a103c79 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -1,8 +1,21 @@
import controls
import numpy
+class Constant(object):
+ def __init__ (self, name, formatt, value):
+ self.name = name
+ self.formatt = formatt
+ self.value = value
+ self.formatToType = {}
+ self.formatToType['%f'] = "double";
+ self.formatToType['%d'] = "int";
+ def __str__ (self):
+ return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
+ (self.formatToType[self.formatt], self.name, self.value)
+
+
class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None):
+ def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
"""Constructs a control loop writer.
Args:
@@ -24,6 +37,16 @@
self._namespace_end = '\n'.join(
['} // namespace %s' % name for name in reversed(self._namespaces)])
+
+ self._constant_list = []
+
+ def AddConstant(self, constant):
+ """Adds a constant to write.
+
+ Args:
+ constant: Constant, the constant to add to the header.
+ """
+ self._constant_list.append(constant)
def _TopDirectory(self):
return self._namespaces[0]
@@ -74,6 +97,10 @@
fd.write('\n')
fd.write(self._namespace_start)
+
+ for const in self._constant_list:
+ fd.write(str(const))
+
fd.write('\n\n')
for loop in self._loops:
fd.write(loop.DumpPlantHeader())
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 001fd1e..3d6e441 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -1,6 +1,7 @@
#!/usr/bin/python
import control_loop
+import controls
import numpy
import sys
from matplotlib import pylab
@@ -89,7 +90,7 @@
self.Gr = self.G_high
# Control loop time step
self.dt = 0.01
-
+
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
self.msp = 1.0 / self.m + self.rb * self.rb / self.J
@@ -118,13 +119,29 @@
self.D = numpy.matrix([[0, 0],
[0, 0]])
+ #print "THE NUMBER I WANT" + str(numpy.linalg.inv(self.A_continuous) * -self.B_continuous * numpy.matrix([[12.0], [12.0]]))
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
# Poles from last year.
self.hp = 0.65
self.lp = 0.83
- self.PlaceControllerPoles([self.hp, self.hp, self.lp, self.lp])
+ self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
+ print self.K
+ q_pos = 0.07
+ q_vel = 1.0
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ print self.A
+ print self.B
+ print self.K
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.hlp = 0.07
self.llp = 0.09
@@ -200,6 +217,7 @@
#pylab.show()
# Write the generated constants out to a file.
+ print "Output one"
drivetrain = Drivetrain()
if len(argv) != 5:
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 280db16..f2dfdbe 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -396,10 +396,10 @@
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
- "VDogDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high])
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 89f682a..69f2599 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -30,6 +30,9 @@
self.Kt = self.stall_torque / self.stall_current
# Spring constant for the springs, N/m
self.Ks = 2800.0
+ # Maximum extension distance (Distance from the 0 force point on the
+ # spring to the latch position.)
+ self.max_extension = 0.32385
# Gear ratio multiplied by radius of final sprocket.
self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
@@ -235,7 +238,13 @@
sprung_shooter = SprungShooterDeltaU()
shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter, shooter])
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter,
+ shooter])
+
+ loop_writer.AddConstant(control_loop.Constant("kMaxExtension", "%f",
+ sprung_shooter.max_extension))
+ loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
+ sprung_shooter.Ks))
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 4789e1d..4a3e6e4 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -5,8 +5,8 @@
#include <algorithm>
#include "aos/common/control_loop/control_loops.q.h"
-#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "frc971/constants.h"
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
@@ -27,17 +27,17 @@
// against last cycle's voltage.
if (X_hat(2, 0) > last_voltage_ + 4.0) {
voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
- LOG(INFO, "Capping due to runawway\n");
+ LOG(DEBUG, "Capping due to runaway\n");
} else if (X_hat(2, 0) < last_voltage_ - 4.0) {
voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
- LOG(INFO, "Capping due to runawway\n");
+ LOG(DEBUG, "Capping due to runaway\n");
}
voltage_ = std::min(max_voltage_, voltage_);
voltage_ = std::max(-max_voltage_, voltage_);
U(0, 0) = voltage_ - old_voltage;
- LOG(INFO, "X_hat is %f, applied is %f\n", X_hat(2, 0), voltage_);
+ LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2, 0), voltage_));
last_voltage_ = voltage_;
capped_goal_ = false;
@@ -56,7 +56,7 @@
R(0, 0) -= dx;
}
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
if (controller_index() == 0) {
@@ -69,7 +69,7 @@
R(0, 0) -= dx;
}
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
} else {
capped_goal_ = false;
}
@@ -85,13 +85,10 @@
void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
double known_position) {
- LOG(INFO, "Setting calibration such that %f -> %f\n", encoder_val,
- known_position);
- LOG(INFO, "Position was %f\n", absolute_position());
+ double old_position = absolute_position();
double previous_offset = offset_;
offset_ = known_position - encoder_val;
double doffset = offset_ - previous_offset;
- LOG(INFO, "Changing offset from %f to %f\n", previous_offset, offset_);
X_hat(0, 0) += doffset;
// Offset our measurements because the offset is baked into them.
Y_(0, 0) += doffset;
@@ -100,7 +97,10 @@
if (controller_index() == 0) {
R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
}
- LOG(INFO, "Validation: position is %f\n", absolute_position());
+ LOG_STRUCT(
+ DEBUG, "sensor edge (fake?)",
+ ShooterChangeCalibration(encoder_val, known_position, old_position,
+ absolute_position(), previous_offset, offset_));
}
ShooterMotor::ShooterMotor(control_loops::ShooterGroup *my_shooter)
@@ -111,18 +111,48 @@
load_timeout_(0, 0),
shooter_brake_set_time_(0, 0),
unload_timeout_(0, 0),
- prepare_fire_end_time_(0, 0),
shot_end_time_(0, 0),
- cycles_not_moved_(0) {}
+ cycles_not_moved_(0),
+ shot_count_(0),
+ zeroed_(false),
+ distal_posedge_validation_cycles_left_(0),
+ proximal_posedge_validation_cycles_left_(0),
+ last_distal_current_(true),
+ last_proximal_current_(true) {}
double ShooterMotor::PowerToPosition(double power) {
- // LOG(WARNING, "power to position not correctly implemented\n");
const frc971::constants::Values &values = constants::GetValues();
- double new_pos = ::std::min(::std::max(power, values.shooter.lower_limit),
+ double maxpower = 0.5 * kSpringConstant *
+ (kMaxExtension * kMaxExtension -
+ (kMaxExtension - values.shooter.upper_limit) *
+ (kMaxExtension - values.shooter.upper_limit));
+ if (power < 0) {
+ LOG_STRUCT(WARNING, "negative power", PowerAdjustment(power, 0));
+ power = 0;
+ } else if (power > maxpower) {
+ LOG_STRUCT(WARNING, "power too high", PowerAdjustment(power, maxpower));
+ power = maxpower;
+ }
+
+ double mp = kMaxExtension * kMaxExtension - (power + power) / kSpringConstant;
+ double new_pos = 0.10;
+ if (mp < 0) {
+ LOG(ERROR,
+ "Power calculation has negative number before square root (%f).\n", mp);
+ } else {
+ new_pos = kMaxExtension - ::std::sqrt(mp);
+ }
+
+ new_pos = ::std::min(::std::max(new_pos, values.shooter.lower_limit),
values.shooter.upper_limit);
return new_pos;
}
+double ShooterMotor::PositionToPower(double position) {
+ double power = kSpringConstant * position * (kMaxExtension - position / 2.0);
+ return power;
+}
+
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
const control_loops::ShooterGroup::Goal *goal,
@@ -131,15 +161,23 @@
control_loops::ShooterGroup::Status *status) {
constexpr double dt = 0.01;
+ if (::std::isnan(goal->shot_power)) {
+ state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because got a shot power of NAN.\n");
+ }
+
// we must always have these or we have issues.
if (goal == NULL || status == NULL) {
if (output) output->voltage = 0;
LOG(ERROR, "Thought I would just check for null and die.\n");
return;
}
+ status->ready = false;
if (reset()) {
state_ = STATE_INITIALIZE;
+ last_distal_current_ = position->pusher_distal.current;
+ last_proximal_current_ = position->pusher_proximal.current;
}
if (position) {
shooter_.CorrectPosition(position->position);
@@ -154,11 +192,8 @@
// Don't even let the control loops run.
bool shooter_loop_disable = false;
- // Adds voltage to take up slack in gears before shot.
- bool apply_some_voltage = false;
-
-
- const bool disabled = !::aos::robot_state->enabled;
+ const bool disabled =
+ !::aos::robot_state.get() || !::aos::robot_state->enabled;
// If true, move the goal if we saturate.
bool cap_goal = false;
@@ -174,7 +209,7 @@
shooter_.set_controller_index(1);
} else {
// Otherwise use the controller with the spring.
- shooter_.set_controller_index(1);
+ shooter_.set_controller_index(0);
}
if (shooter_.controller_index() != last_controller_index) {
shooter_.RecalculatePowerGoal();
@@ -200,13 +235,18 @@
values.shooter.upper_limit);
}
- state_ = STATE_REQUEST_LOAD;
-
// Go to the current position.
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
// If the plunger is all the way back, we want to be latched.
latch_piston_ = position->plunger;
brake_piston_ = false;
+ if (position->latch == latch_piston_) {
+ state_ = STATE_REQUEST_LOAD;
+ } else {
+ shooter_loop_disable = true;
+ LOG(DEBUG,
+ "Not moving on until the latch has moved to avoid a crash\n");
+ }
} else {
// If we can't start yet because we don't know where we are, set the
// latch and brake to their defaults.
@@ -216,7 +256,9 @@
break;
case STATE_REQUEST_LOAD:
if (position) {
- if (position->pusher_distal.current) {
+ zeroed_ = false;
+ if (position->pusher_distal.current ||
+ position->pusher_proximal.current) {
// We started on the sensor, back up until we are found.
// If the plunger is all the way back and not latched, it won't be
// there for long.
@@ -259,12 +301,13 @@
shooter_.set_max_voltage(4.0);
if (position) {
- if (!position->pusher_distal.current) {
+ if (!position->pusher_distal.current &&
+ !position->pusher_proximal.current) {
Load();
}
+ latch_piston_ = position->plunger;
}
- latch_piston_ = false;
brake_piston_ = false;
break;
case STATE_LOAD:
@@ -281,18 +324,48 @@
shooter_.SetGoalPosition(0.0, 0.0);
if (position) {
+ // TODO(austin): Validate that this is the right edge.
// If we see a posedge on any of the hall effects,
if (position->pusher_proximal.posedge_count !=
- last_proximal_posedge_count_) {
- LOG(DEBUG, "Setting calibration using proximal sensor\n");
- shooter_.SetCalibration(position->pusher_proximal.posedge_value,
- values.shooter.pusher_proximal.upper_angle);
+ last_proximal_posedge_count_ &&
+ !last_proximal_current_) {
+ proximal_posedge_validation_cycles_left_ = 2;
}
+ if (proximal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_proximal.current) {
+ --proximal_posedge_validation_cycles_left_;
+ if (proximal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_proximal.posedge_value,
+ values.shooter.pusher_proximal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using proximal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ proximal_posedge_validation_cycles_left_ = 0;
+ }
+ }
+
if (position->pusher_distal.posedge_count !=
- last_distal_posedge_count_) {
- LOG(DEBUG, "Setting calibration using distal sensor\n");
- shooter_.SetCalibration(position->pusher_distal.posedge_value,
- values.shooter.pusher_distal.upper_angle);
+ last_distal_posedge_count_ &&
+ !last_distal_current_) {
+ distal_posedge_validation_cycles_left_ = 2;
+ }
+ if (distal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_distal.current) {
+ --distal_posedge_validation_cycles_left_;
+ if (distal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_distal.posedge_value,
+ values.shooter.pusher_distal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using distal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ distal_posedge_validation_cycles_left_ = 0;
+ }
}
// Latch if the plunger is far enough back to trigger the hall effect.
@@ -303,7 +376,11 @@
// way back as well.
if (position->plunger && position->latch &&
position->pusher_distal.current) {
- state_ = STATE_PREPARE_SHOT;
+ if (!zeroed_) {
+ state_ = STATE_REQUEST_LOAD;
+ } else {
+ state_ = STATE_PREPARE_SHOT;
+ }
} else if (position->plunger &&
::std::abs(shooter_.absolute_position() -
shooter_.goal_position()) < 0.001) {
@@ -317,6 +394,7 @@
if (!position->pusher_distal.current ||
!position->pusher_proximal.current) {
state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because took too long to load.\n");
}
}
} else if (goal->unload_requested) {
@@ -326,7 +404,8 @@
break;
case STATE_LOADING_PROBLEM:
if (disabled) {
- Load();
+ state_ = STATE_REQUEST_LOAD;
+ break;
}
// We got to the goal, but the latch hasn't registered as down. It might
// be stuck, or on it's way but not there yet.
@@ -378,15 +457,8 @@
LOG(DEBUG, "In ready\n");
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
- if (::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) > 0.002) {
- // TODO(austin): Add a state to release the brake.
-
- // TODO(austin): Do we want to set the brake here or after shooting?
- // Depends on air usage.
- LOG(DEBUG, "Preparing shot again.\n");
- state_ = STATE_PREPARE_SHOT;
- } else if (Time::Now() > shooter_brake_set_time_) {
+ if (Time::Now() > shooter_brake_set_time_) {
+ status->ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
shooter_loop_disable = true;
@@ -394,13 +466,23 @@
if (goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
- apply_some_voltage = true;
- state_ = STATE_PREPARE_FIRE;
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
+ firing_starting_position_ = shooter_.absolute_position();
+ state_ = STATE_FIRE;
}
- } else {
- LOG(DEBUG, "Nothing %d %d\n", goal->shot_requested, !disabled);
}
+ if (state_ == STATE_READY &&
+ ::std::abs(shooter_.absolute_position() -
+ PowerToPosition(goal->shot_power)) > 0.002) {
+ // TODO(austin): Add a state to release the brake.
+
+ // TODO(austin): Do we want to set the brake here or after shooting?
+ // Depends on air usage.
+ status->ready = false;
+ LOG(DEBUG, "Preparing shot again.\n");
+ state_ = STATE_PREPARE_SHOT;
+ }
+
shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
latch_piston_ = true;
@@ -411,29 +493,6 @@
}
break;
- case STATE_PREPARE_FIRE:
- // Apply a bit of voltage to bias the gears for a little bit of time, and
- // then fire.
- shooter_loop_disable = true;
- if (disabled) {
- // If we are disabled, reset the backlash bias timer.
- prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
- break;
- }
- if (Time::Now() > prepare_fire_end_time_) {
- cycles_not_moved_ = 0;
- firing_starting_position_ = shooter_.absolute_position();
- shot_end_time_ = Time::Now() + kShotEndTimeout;
- state_ = STATE_FIRE;
- latch_piston_ = false;
- } else {
- apply_some_voltage = true;
- latch_piston_ = true;
- }
-
- brake_piston_ = true;
- break;
-
case STATE_FIRE:
if (disabled) {
if (position) {
@@ -461,6 +520,7 @@
cycles_not_moved_ > 3) ||
Time::Now() > shot_end_time_) {
state_ = STATE_REQUEST_LOAD;
+ ++shot_count_;
}
latch_piston_ = false;
brake_piston_ = true;
@@ -500,6 +560,7 @@
// We have been stuck trying to unload for way too long, give up and
// turn everything off.
state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because took too long to unload.\n");
}
brake_piston_ = false;
@@ -510,7 +571,7 @@
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
cap_goal = true;
- shooter_.set_max_voltage(5.0);
+ shooter_.set_max_voltage(6.0);
// Slowly move back until we hit the upper limit.
// If we were at the limit last cycle, we are done unloading.
@@ -526,8 +587,8 @@
shooter_.SetGoalPosition(
::std::min(
values.shooter.upper_limit,
- shooter_.goal_position() + values.shooter.zeroing_speed * dt),
- values.shooter.zeroing_speed);
+ shooter_.goal_position() + values.shooter.unload_speed * dt),
+ values.shooter.unload_speed);
}
latch_piston_ = false;
@@ -537,7 +598,7 @@
if (goal->load_requested) {
state_ = STATE_REQUEST_LOAD;
}
- // If we are ready to load again,
+ // If we are ready to load again,
shooter_loop_disable = true;
latch_piston_ = false;
@@ -545,6 +606,7 @@
break;
case STATE_ESTOP:
+ LOG(WARNING, "estopped\n");
// Totally lost, go to a safe state.
shooter_loop_disable = true;
latch_piston_ = true;
@@ -552,13 +614,10 @@
break;
}
- if (apply_some_voltage) {
- shooter_.Update(true);
- shooter_.ZeroPower();
- if (output) output->voltage = 2.0;
- } else if (!shooter_loop_disable) {
- LOG(DEBUG, "Running the loop, goal is %f, position is %f\n",
- shooter_.goal_position(), shooter_.absolute_position());
+ if (!shooter_loop_disable) {
+ LOG_STRUCT(DEBUG, "running the loop",
+ ShooterStatusToLog(shooter_.goal_position(),
+ shooter_.absolute_position()));
if (!cap_goal) {
shooter_.set_max_voltage(12.0);
}
@@ -573,25 +632,38 @@
if (output) output->voltage = 0.0;
}
+ status->hard_stop_power = PositionToPower(shooter_.absolute_position());
+
if (output) {
output->latch_piston = latch_piston_;
output->brake_piston = brake_piston_;
}
- status->done = ::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) < 0.004;
+ if (position) {
+ LOG_STRUCT(DEBUG, "internal state",
+ ShooterStateToLog(
+ shooter_.absolute_position(), shooter_.absolute_velocity(),
+ state_, position->latch, position->pusher_proximal.current,
+ position->pusher_distal.current, position->plunger,
+ brake_piston_, latch_piston_));
- if (position) {
last_position_ = *position;
- LOG(DEBUG,
- "pos > absolute: %f velocity: %f state= %d\n",
- shooter_.absolute_position(), shooter_.absolute_velocity(),
- state_);
- }
- if (position) {
+
last_distal_posedge_count_ = position->pusher_distal.posedge_count;
last_proximal_posedge_count_ = position->pusher_proximal.posedge_count;
+ last_distal_current_ = position->pusher_distal.current;
+ last_proximal_current_ = position->pusher_proximal.current;
}
+
+ status->shots = shot_count_;
+}
+
+void ShooterMotor::ZeroOutputs() {
+ queue_group()->output.MakeWithBuilder()
+ .voltage(0)
+ .latch_piston(latch_piston_)
+ .brake_piston(brake_piston_)
+ .Send();
}
} // namespace control_loops
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index efcc0e7..10e0f4e 100755
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -30,6 +30,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'shooter_loop',
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 195894b..1ab224b 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -66,7 +66,6 @@
void CorrectPosition(double position) {
Eigen::Matrix<double, 1, 1> Y;
Y << position + offset_ - kPositionOffset;
- LOG(DEBUG, "Setting position to %f\n", position);
Correct(Y);
}
@@ -97,7 +96,7 @@
private:
// The offset between what is '0' (0 rate on the spring) and the 0 (all the
// way cocked).
- constexpr static double kPositionOffset = 0.305054 + 0.0254;
+ constexpr static double kPositionOffset = kMaxExtension;
// The accumulated voltage to apply to the motor.
double voltage_;
double last_voltage_;
@@ -108,10 +107,11 @@
};
const Time kUnloadTimeout = Time::InSeconds(10);
-const Time kLoadTimeout = Time::InSeconds(10);
-const Time kLoadProblemEndTimeout = Time::InSeconds(0.5);
+const Time kLoadTimeout = Time::InSeconds(2);
+const Time kLoadProblemEndTimeout = Time::InSeconds(1.0);
const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
-const Time kShotEndTimeout = Time::InSeconds(1.0);
+// Time to wait after releasing the latch piston before winching back again.
+const Time kShotEndTimeout = Time::InSeconds(0.2);
const Time kPrepareFireEndTime = Time::InMS(40);
class ShooterMotor
@@ -124,6 +124,7 @@
bool capped_goal() const { return shooter_.capped_goal(); }
double PowerToPosition(double power);
+ double PositionToPower(double position);
typedef enum {
STATE_INITIALIZE = 0,
@@ -133,7 +134,6 @@
STATE_LOADING_PROBLEM = 4,
STATE_PREPARE_SHOT = 5,
STATE_READY = 6,
- STATE_PREPARE_FIRE = 7,
STATE_FIRE = 8,
STATE_UNLOAD = 9,
STATE_UNLOAD_MOVE = 10,
@@ -150,6 +150,9 @@
ShooterGroup::Output *output, ShooterGroup::Status *status);
private:
+ // We have to override this to keep the pistons in the correct positions.
+ virtual void ZeroOutputs();
+
// Friend the test classes for acces to the internal state.
friend class testing::ShooterTest_UnloadWindupPositive_Test;
friend class testing::ShooterTest_UnloadWindupNegative_Test;
@@ -180,14 +183,10 @@
// wait for brake to set
Time shooter_brake_set_time_;
-
+
// The timeout for unloading.
Time unload_timeout_;
- // we are attempting to take up some of the backlash
- // in the gears before the plunger hits
- Time prepare_fire_end_time_;
-
// time that shot must have completed
Time shot_end_time_;
@@ -201,6 +200,12 @@
bool brake_piston_;
int32_t last_distal_posedge_count_;
int32_t last_proximal_posedge_count_;
+ uint32_t shot_count_;
+ bool zeroed_;
+ int distal_posedge_validation_cycles_left_;
+ int proximal_posedge_validation_cycles_left_;
+ bool last_distal_current_;
+ bool last_proximal_current_;
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 6310320..2a42172 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -41,10 +41,14 @@
// Whether it's ready to shoot right now.
bool ready;
// Whether the plunger is in and out of the way of grabbing a ball.
+ // TODO(ben): Populate these!
bool cocked;
// How many times we've shot.
int32_t shots;
bool done;
+ // What we think the current position of the hard stop on the shooter is, in
+ // shot power (Joules).
+ double hard_stop_power;
};
queue Goal goal;
@@ -54,3 +58,43 @@
};
queue_group ShooterGroup shooter_queue_group;
+
+struct ShooterStateToLog {
+ double absolute_position;
+ double absolute_velocity;
+ uint32_t state;
+ bool latch_sensor;
+ bool proximal;
+ bool distal;
+ bool plunger;
+ bool brake;
+ bool latch_piston;
+};
+
+struct ShooterVoltageToLog {
+ double X_hat;
+ double applied;
+};
+
+struct ShooterMovingGoal {
+ double dx;
+};
+
+struct ShooterChangeCalibration {
+ double encoder;
+ double real_position;
+ double old_position;
+ double new_position;
+ double old_offset;
+ double new_offset;
+};
+
+struct ShooterStatusToLog {
+ double goal;
+ double position;
+};
+
+struct PowerAdjustment {
+ double requested_power;
+ double actual_power;
+};
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 8aa4c27..669e147 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -50,7 +50,7 @@
// The difference between the position with 0 at the back, and the position
// with 0 measured where the spring has 0 force.
- constexpr static double kPositionOffset = 0.305054 + 0.0254;
+ constexpr static double kPositionOffset = kMaxExtension;
void Reinitialize(double initial_position) {
LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
@@ -83,7 +83,8 @@
// (encoder, hall effect).
void SetPhysicalSensors(control_loops::ShooterGroup::Position *position) {
const frc971::constants::Values &values = constants::GetValues();
- position->position = GetPosition();
+
+ position->position = GetPosition();
LOG(DEBUG, "Physical shooter at {%f}\n", GetAbsolutePosition());
@@ -134,12 +135,27 @@
}
}
- // Sends out the position queue messages.
void SendPositionMessage() {
+ // the first bool is false
+ SendPositionMessage(false, false, false, false);
+ }
+
+ // Sends out the position queue messages.
+ // if the first bool is false then this is
+ // just the default state, otherwise will force
+ // it into a state using the passed values
+ void SendPositionMessage(bool use_passed, bool plunger_in,
+ bool latch_in, bool brake_in) {
const frc971::constants::Values &values = constants::GetValues();
::aos::ScopedMessagePtr<control_loops::ShooterGroup::Position> position =
shooter_queue_group_.position.MakeMessage();
+ if (use_passed) {
+ plunger_latched_ = latch_in && plunger_in;
+ latch_piston_state_ = plunger_latched_;
+ brake_piston_state_ = brake_in;
+ }
+
SetPhysicalSensors(position.get());
position->latch = latch_piston_state_;
@@ -170,10 +186,6 @@
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << ": The test doesn't support that.";
latch_delay_count_ = -6;
- if (GetAbsolutePosition() > 0.01) {
- EXPECT_GE(last_voltage_, 1)
- << ": Must preload the gearbox when firing.";
- }
}
if (shooter_queue_group_.output->brake_piston && !brake_piston_state_ &&
@@ -206,6 +218,7 @@
shooter_plant_->D() * shooter_plant_->U;
} else {
shooter_plant_->U << last_voltage_;
+ //shooter_plant_->U << shooter_queue_group_.output->voltage;
shooter_plant_->Update();
}
LOG(DEBUG, "Plant index is %d\n", shooter_plant_->plant_index());
@@ -272,6 +285,7 @@
};
class ShooterTest : public ::testing::Test {
+
protected:
::aos::common::testing::GlobalCoreInstance my_core;
@@ -333,21 +347,39 @@
};
TEST_F(ShooterTest, PowerConversion) {
- // test a couple of values return the right thing
- EXPECT_EQ(0.021, shooter_motor_.PowerToPosition(0.021));
- EXPECT_EQ(0.175, shooter_motor_.PowerToPosition(0.175));
-
const frc971::constants::Values &values = constants::GetValues();
+ // test a couple of values return the right thing
+ EXPECT_NEAR(0.254001, shooter_motor_.PowerToPosition(140.0), 0.00001);
+ EXPECT_NEAR(0.00058, shooter_motor_.PowerToPosition(0.53), 0.00001);
+ EXPECT_NEAR(0.095251238129837101, shooter_motor_.PowerToPosition(73.67),
+ 0.00001);
+
// value too large should get max
- EXPECT_EQ(values.shooter.upper_limit,
- shooter_motor_.PowerToPosition(505050.99));
+ EXPECT_NEAR(values.shooter.upper_limit,
+ shooter_motor_.PowerToPosition(505050.99), 0.00001);
// negative values should zero
- EXPECT_EQ(values.shooter.lower_limit, shooter_motor_.PowerToPosition(-123.4));
+ EXPECT_NEAR(0, shooter_motor_.PowerToPosition(-123.4), 0.00001);
+}
+
+// Test that PowerToPosition and PositionToPower are inverses of each other.
+// Note that PowerToPosition will cap position whereas PositionToPower will not
+// cap power.
+TEST_F(ShooterTest, InversePowerConversion) {
+ // Test a few values.
+ double power = 140.0;
+ double position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+ power = .53;
+ position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+ power = 71.971;
+ position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 200; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -356,13 +388,15 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Fire) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.021).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 120; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -370,21 +404,21 @@
SendDSPacket(true);
}
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
+ shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_power(35.0)
+ .shot_requested(true)
+ .Send();
- bool hit_preparefire = false;
bool hit_fire = false;
for (int i = 0; i < 400; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
- hit_preparefire = true;
- }
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_power(17.0)
.shot_requested(false)
.Send();
}
@@ -393,15 +427,16 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- EXPECT_TRUE(hit_preparefire);
EXPECT_TRUE(hit_fire);
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, FireLong) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -411,16 +446,12 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
- bool hit_preparefire = false;
bool hit_fire = false;
for (int i = 0; i < 400; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
- hit_preparefire = true;
- }
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
shooter_queue_group_.goal.MakeWithBuilder()
@@ -432,16 +463,30 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power), pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- EXPECT_TRUE(hit_preparefire);
EXPECT_TRUE(hit_fire);
}
+// Verifies that it doesn't try to go out too far if you give it a ridicilous
+// power.
+TEST_F(ShooterTest, LoadTooFar) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(500.0).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_LT(
+ shooter_motor_plant_.GetAbsolutePosition(),
+ constants::GetValuesForTeam(971).shooter.upper_limit);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, MoveGoal) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -449,7 +494,7 @@
SendDSPacket(true);
}
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.11).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(14.0).Send();
for (int i = 0; i < 100; ++i) {
shooter_motor_plant_.SendPositionMessage();
@@ -459,14 +504,16 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Unload) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -476,7 +523,9 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
- for (int i = 0; i < 400; ++i) {
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -484,13 +533,13 @@
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
- shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ shooter_motor_plant_.GetAbsolutePosition(), 0.015);
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupNegative) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.20).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -502,7 +551,9 @@
int kicked_delay = 20;
int capped_goal_count = 0;
- for (int i = 0; i < 400; ++i) {
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
@@ -512,15 +563,15 @@
shooter_motor_.shooter_.R(0, 0) -= 100;
}
}
- if (shooter_motor_.capped_goal()) {
- ++capped_goal_count;
- }
+ if (shooter_motor_.capped_goal() && kicked_delay < 0) {
+ ++capped_goal_count;
+ }
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
- shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ shooter_motor_plant_.GetAbsolutePosition(), 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
EXPECT_LE(1, capped_goal_count);
EXPECT_GE(3, capped_goal_count);
@@ -528,7 +579,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupPositive) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.20).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -540,7 +591,9 @@
int kicked_delay = 20;
int capped_goal_count = 0;
- for (int i = 0; i < 400; ++i) {
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
@@ -550,15 +603,15 @@
shooter_motor_.shooter_.R(0, 0) += 0.1;
}
}
- if (shooter_motor_.capped_goal()) {
- ++capped_goal_count;
- }
+ if (shooter_motor_.capped_goal() && kicked_delay < 0) {
+ ++capped_goal_count;
+ }
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
- shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ shooter_motor_plant_.GetAbsolutePosition(), 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
EXPECT_LE(1, capped_goal_count);
EXPECT_GE(3, capped_goal_count);
@@ -571,7 +624,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, StartsOnDistal) {
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 200; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -580,7 +633,9 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -589,7 +644,7 @@
TEST_F(ShooterTest, StartsOnProximal) {
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 300; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -598,17 +653,59 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
+class ShooterZeroingTest : public ShooterTest,
+ public ::testing::WithParamInterface<
+ ::std::tr1::tuple<bool, bool, bool, double>> {};
+
+TEST_P(ShooterZeroingTest, AllDisparateStartingZero) {
+ bool latch = ::std::tr1::get<0>(GetParam());
+ bool brake = ::std::tr1::get<1>(GetParam());
+ bool plunger_back = ::std::tr1::get<2>(GetParam());
+ double start_pos = ::std::tr1::get<3>(GetParam());
+ // flag to initialize test
+ //printf("@@@@ l= %d b= %d p= %d s= %.3f\n",
+ // latch, brake, plunger_back, start_pos);
+ bool initialized = false;
+ Reinitialize(start_pos);
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(120.0).Send();
+ for (int i = 0; i < 200; ++i) {
+ shooter_motor_plant_.SendPositionMessage(!initialized, plunger_back, latch, brake);
+ initialized = true;
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ // EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
+ ASSERT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
+
+INSTANTIATE_TEST_CASE_P(
+ ShooterZeroingTest, ShooterZeroingTest,
+ ::testing::Combine(
+ ::testing::Bool(), ::testing::Bool(), ::testing::Bool(),
+ ::testing::Values(
+ 0.05, constants::GetValuesForTeam(971).shooter.upper_limit - 0.05,
+ HallEffectMiddle(constants::GetValuesForTeam(971)
+ .shooter.pusher_proximal),
+ HallEffectMiddle(constants::GetValuesForTeam(971)
+ .shooter.pusher_distal),
+ constants::GetValuesForTeam(971).shooter.latch_max_safe_position -
+ 0.001)));
+
// TODO(austin): Slip the encoder somewhere.
// TODO(austin): Test all the timeouts...
-// TODO(austin): Test starting latched and with the plunger back.
-// TODO(austin): Verify that we zeroed if we started latched and all the way back.
-
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
old mode 100755
new mode 100644
index 968fd04..606395a
--- a/frc971/control_loops/shooter/shooter_motor_plant.h
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -5,6 +5,10 @@
namespace frc971 {
namespace control_loops {
+static constexpr double kMaxExtension = 0.323850;
+
+static constexpr double kSpringConstant = 2800.000000;
+
StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients();
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
old mode 100755
new mode 100644
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 0ef0df5..f289e81 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -323,14 +323,17 @@
//::std::cout << "Predict xhat before " << X_hat;
//::std::cout << "Measurement error is " << Y_ - C() * X_hat;
//X_hat = A() * X_hat + B() * U;
+ //::std::cout << "Predict xhat after " << X_hat;
+ UpdateObserver();
+ }
+
+ void UpdateObserver() {
if (new_y_) {
- LOG(INFO, "Got Y. R is (%f, %f, %f)\n", R(0, 0), R(1, 0), R(2, 0));
X_hat = (A() - L() * C()) * X_hat + L() * Y_ + B() * U;
new_y_ = false;
} else {
X_hat = A() * X_hat + B() * U;
}
- //::std::cout << "Predict xhat after " << X_hat;
}
// Sets the current controller to be index and verifies that it isn't out of
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 195195e..c91a8ac 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -10,12 +10,19 @@
'<(AOS)/prime/input/input.gyp:joystick_input',
'<(AOS)/linux_code/linux_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:log_interval',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
'<(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 d751a43..7b97bbd 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -7,15 +7,22 @@
#include "aos/prime/input/joystick_input.h"
#include "aos/common/input/driver_station_data.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/time.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/constants.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/autonomous/auto.q.h"
#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::gyro;
+using ::frc971::sensors::gyro_reading;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
@@ -30,114 +37,440 @@
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kClawClosed(3, 7);
-const ButtonLocation kClawOpen(3, 9);
-const ButtonLocation kFire(3, 11);
-const ButtonLocation kUnload(3, 12);
+
+const ButtonLocation kCatch(3, 10);
+
+const ButtonLocation kFire(3, 9);
+const ButtonLocation kUnload(2, 11);
+const ButtonLocation kReload(2, 6);
+
+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 JoystickAxis kFlipRobot(3, 3);
+
+const ButtonLocation kLongShot(3, 7);
+const ButtonLocation kMediumShot(3, 6);
+const ButtonLocation kShortShot(3, 2);
+const ButtonLocation kTrussShot(3, 1);
+
+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;
+// TODO(brians): This wants to be -0.04 on the comp bot. Make them both the
+// same.
+const double kGrabSeparation = 0;
+const double kShootSeparation = 0.11 + kGrabSeparation;
+
+const ClawGoal kTuckGoal = {-2.273474, -0.749484};
+const ClawGoal kIntakeGoal = {-2.273474, kGrabSeparation};
+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, kGrabSeparation};
+const ClawGoal kFlippedIntakeOpenGoal = {0.95, 1.0};
+
+//const ShotGoal kLongShotGoal = {
+ //{-M_PI / 2.0 + 0.46, kShootSeparation}, 120, false, kIntakePower};
+const ShotGoal kLongShotGoal = {
+ {-1.04, kShootSeparation}, 140, 0.04, kIntakePower};
+const ShotGoal kMediumShotGoal = {
+ {-0.90, kShootSeparation}, 105, 0.2, kIntakePower};
+const ShotGoal kShortShotGoal = {
+ {-0.670, kShootSeparation}, 71.0, 0, kIntakePower};
+const ShotGoal kTrussShotGoal = {
+ {-0.05, kShootSeparation}, 61.0, 0, kIntakePower};
+
+const ShotGoal kFlippedLongShotGoal = {
+ {0.97, kShootSeparation}, 140, 0.08, kIntakePower};
+const ShotGoal kFlippedMediumShotGoal = {
+ {0.80, kShootSeparation}, 105, 0.2, kIntakePower};
+const ShotGoal kFlippedShortShotGoal = {
+ {0.57, kShootSeparation}, 80.0, 0, kIntakePower};
+
+// Makes a new ShootAction 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.
+class ActionQueue {
+ public:
+ // Queues up an action for sending.
+ void QueueAction(::std::unique_ptr<Action> action) {
+ if (current_action_) {
+ LOG(INFO, "Queueing action, canceling prior\n");
+ current_action_->Cancel();
+ next_action_ = ::std::move(action);
+ } else {
+ LOG(INFO, "Queueing action\n");
+ current_action_ = ::std::move(action);
+ current_action_->Start();
+ }
+ }
+
+ // Cancels the current action, and runs the next one when the current one has
+ // finished.
+ void CancelCurrentAction() {
+ LOG(INFO, "Canceling current action\n");
+ if (current_action_) {
+ current_action_->Cancel();
+ }
+ }
+
+ // Cancels all running actions.
+ void CancelAllActions() {
+ LOG(DEBUG, "Cancelling all actions\n");
+ if (current_action_) {
+ current_action_->Cancel();
+ }
+ next_action_.reset();
+ }
+
+ // Runs the next action when the current one is finished running.
+ void Tick() {
+ if (current_action_) {
+ if (!current_action_->Running()) {
+ LOG(INFO, "Action is done.\n");
+ current_action_ = ::std::move(next_action_);
+ if (current_action_) {
+ LOG(INFO, "Running next action\n");
+ current_action_->Start();
+ }
+ }
+ }
+ }
+
+ // Returns true if any action is running or could be running.
+ // For a one cycle faster response, call Tick before running this.
+ bool Running() { return (bool)current_action_; }
+
+ private:
+ ::std::unique_ptr<Action> current_action_;
+ ::std::unique_ptr<Action> next_action_;
+};
+
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() : closed_(true) {}
+ 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) {}
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
- static bool is_high_gear = false;
-
if (data.GetControlBit(ControlBit::kAutonomous)) {
if (data.PosEdge(ControlBit::kEnabled)){
LOG(INFO, "Starting auto mode\n");
::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(true).Send();
+ .run_auto(true)
+ .Send();
} else if (data.NegEdge(ControlBit::kEnabled)) {
LOG(INFO, "Stopping auto mode\n");
::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(false).Send();
+ .run_auto(false)
+ .Send();
}
- } else { // teleop
- bool is_control_loop_driving = false;
- double left_goal = 0.0;
- 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))) {
- 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.position.FetchLatest() && gyro.FetchLatest()) {
- distance = (drivetrain.position->left_encoder +
- drivetrain.position->right_encoder) / 2.0
- - throttle * kThrottleGain / 2.0;
- angle = gyro->angle;
- filtered_goal_distance = distance;
- }
+ } else {
+ HandleTeleop(data);
+ }
+ }
+
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
+ bool is_control_loop_driving = false;
+ double left_goal = 0.0;
+ 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.position.FetchLatest() && gyro_reading.FetchLatest()) {
+ distance = (drivetrain.position->left_encoder +
+ drivetrain.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.goal.MakeWithBuilder()
- .steering(wheel)
- .throttle(throttle)
- .highgear(is_high_gear).quickturn(data.IsPressed(kQuickTurn))
- .control_loop_driving(is_control_loop_driving)
- .left_goal(left_goal).right_goal(right_goal).Send()) {
- LOG(WARNING, "sending stick values failed\n");
+ 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.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .highgear(is_high_gear_)
+ .quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal)
+ .right_goal(right_goal)
+ .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;
+ velocity_compensation_ = 0.0;
+ intake_power_ = 0.0;
+ }
+
+ void SetGoal(ShotGoal goal) {
+ goal_angle_ = goal.claw.angle;
+ separation_angle_ = goal.claw.separation;
+ shot_power_ = goal.shot_power;
+ velocity_compensation_ = goal.velocity_compensation;
+ intake_power_ = goal.intake_power;
+ }
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ HandleDrivetrain(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;
+ }
+
+ static const double kAdjustClawGoalDeadband = 0.08;
+ double claw_goal_adjust = data.GetAxis(kAdjustClawGoal);
+ if (::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 (::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 (data.GetAxis(kFlipRobot) > 0.5) {
+ 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(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(kMediumShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedMediumShotGoal);
+ } else if (data.PosEdge(kShortShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedShortShotGoal);
+ } else if (data.PosEdge(kTrussShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kTrussShotGoal);
+ }
+ } 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(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(kMediumShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kMediumShotGoal);
+ } else if (data.PosEdge(kShortShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kShortShotGoal);
+ } else if (data.PosEdge(kTrussShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kTrussShotGoal);
+ }
+ }
+
+ /*
+ 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(actions::MakeShootAction());
+ }
+
+ 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;
}
- if (data.PosEdge(kShiftHigh)) {
- is_high_gear = false;
+ control_loops::drivetrain.status.FetchLatest();
+ double goal_angle = goal_angle_;
+ if (control_loops::drivetrain.status.get()) {
+ goal_angle +=
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed);
+ } else {
+ LOG_INTERVAL(no_drivetrain_status_);
}
- if (data.PosEdge(kShiftLow)) {
- is_high_gear = true;
- }
- if (data.PosEdge(kClawClosed)) {
- closed_ = true;
- }
- if (data.PosEdge(kClawOpen)) {
- closed_ = false;
+ double separation_angle = separation_angle_;
+
+ if (data.IsPressed(kCatch)) {
+ const double kCatchSeparation = 1.0;
+ goal_angle -= kCatchSeparation / 2.0;
+ separation_angle = kCatchSeparation;
}
- double separation_angle = closed_ ? 0.0 : 0.5;
- double goal_angle = closed_ ? 0.0 : -0.2;
+ bool intaking =
+ data.IsPressed(kRollersIn) || data.IsPressed(kIntakePosition) ||
+ data.IsPressed(kIntakeOpenPosition) || data.IsPressed(kCatch);
if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
.bottom_angle(goal_angle)
.separation_angle(separation_angle)
- .intake(false)
+ .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_group.goal.MakeWithBuilder()
- .shot_power(0.25)
+ .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 frc971::constants::Values &values = frc971::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:
- bool closed_;
+ bool is_high_gear_;
+ double shot_power_;
+ double goal_angle_;
+ double separation_angle_;
+ double velocity_compensation_;
+ double intake_power_;
+ bool was_running_;
+
+ ActionQueue action_queue_;
+
+ ::aos::util::SimpleLogInterval no_drivetrain_status_ =
+ ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ "no drivetrain status");
};
} // namespace joysticks
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 8c4172d..7648db8 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -9,7 +9,7 @@
#include "bbb/sensor_reader.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/constants.h"
#include "frc971/queues/to_log.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
@@ -20,7 +20,8 @@
#endif
using ::frc971::control_loops::drivetrain;
-using ::frc971::sensors::gyro;
+using ::frc971::sensors::other_sensors;
+using ::frc971::sensors::gyro_reading;
using ::aos::util::WrappingCounter;
namespace frc971 {
@@ -47,29 +48,47 @@
}
// Translates values from the ADC into voltage.
-// TODO(brian): Fix this.
+// TODO(brian): Tune this to the actual hardware.
double adc_translate(uint16_t in) {
static const double kVcc = 5;
static const double kR1 = 5, kR2 = 6.65;
static const uint16_t kMaximumValue = 0x3FF;
- return (kVcc *
- (static_cast<double>(in) / static_cast<double>(kMaximumValue)) -
- kVcc * kR1) /
- kR2;
+ const double raw =
+ (kVcc * static_cast<double>(in) / static_cast<double>(kMaximumValue));
+ return (raw * (kR1 + kR2) - (kVcc / 2) * kR2) / kR1;
}
double gyro_translate(int64_t in) {
return in / 16.0 / 1000.0 / (180.0 / M_PI);
}
-double battery_translate(uint16_t in) {
- static const double kDividerBig = 98.9, kDividerSmall = 17.8;
- return adc_translate(in) * kDividerBig / kDividerSmall;
+double battery_translate(uint16_t in_high, uint16_t in_low) {
+ const double high = adc_translate(in_high), low = adc_translate(in_low);
+ static const double kDividerBig = 5.55, kDividerSmall = 2.66;
+ static const double kSensorVcc = 5.0;
+ return (high - low) * (kDividerBig + kDividerSmall) / kDividerSmall +
+ kDividerBig / kDividerSmall * kSensorVcc;
}
-double hall_translate(const constants::ShifterHallEffect &k, uint16_t in) {
- const double voltage = adc_translate(in);
- return (voltage - k.low) / (k.high - k.low);
+double sonar_translate(uint32_t in) {
+ return static_cast<double>(in) / 1000.0 * 2.0;
+}
+
+double hall_translate(const constants::ShifterHallEffect &k, uint16_t in_low,
+ uint16_t in_high) {
+ const double low_ratio =
+ 0.5 * (in_low - static_cast<double>(k.low_gear_low)) /
+ static_cast<double>(k.low_gear_middle - k.low_gear_low);
+ const double high_ratio =
+ 0.5 + 0.5 * (in_high - static_cast<double>(k.high_gear_middle)) /
+ static_cast<double>(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) {
+ return low_ratio;
+ } else {
+ return high_ratio;
+ }
}
double claw_translate(int32_t in) {
@@ -77,7 +96,8 @@
/ (256.0 /*cpr*/ * 4.0 /*quad*/)
/ (18.0 / 48.0 /*encoder gears*/)
/ (12.0 / 60.0 /*chain reduction*/)
- * (M_PI / 180.0);
+ * (M_PI / 180.0)
+ * 2.0 /*TODO(austin): Debug this, encoders read too little*/;
}
double shooter_translate(int32_t in) {
@@ -119,9 +139,12 @@
State *state) {
::frc971::logging_structs::CapeReading reading_to_log(
cape_timestamp.sec(), cape_timestamp.nsec(),
- data->main.ultrasonic_pulse_length);
+ sizeof(*data), sonar_translate(data->main.ultrasonic_pulse_length),
+ data->main.low_left_drive_hall, data->main.high_left_drive_hall,
+ data->main.low_right_drive_hall, data->main.high_right_drive_hall);
LOG_STRUCT(DEBUG, "cape reading", reading_to_log);
bool bad_gyro;
+ // TODO(brians): Switch to LogInterval for these things.
if (data->uninitialized_gyro) {
LOG(DEBUG, "uninitialized gyro\n");
bad_gyro = true;
@@ -131,7 +154,6 @@
} else if (data->bad_gyro) {
LOG(ERROR, "bad gyro\n");
bad_gyro = true;
- gyro.MakeWithBuilder().angle(0).Send();
} else if (data->old_gyro_reading) {
LOG(WARNING, "old/bad gyro reading\n");
bad_gyro = true;
@@ -140,19 +162,30 @@
}
if (!bad_gyro) {
- gyro.MakeWithBuilder()
+ gyro_reading.MakeWithBuilder()
.angle(gyro_translate(data->gyro_angle))
.Send();
}
+ if (data->analog_errors != 0) {
+ LOG(WARNING, "%" PRIu8 " analog errors\n", data->analog_errors);
+ }
+
+ other_sensors.MakeWithBuilder()
+ .sonar_distance(sonar_translate(data->main.ultrasonic_pulse_length))
+ .Send();
+
drivetrain.position.MakeWithBuilder()
.right_encoder(drivetrain_translate(data->main.right_drive))
.left_encoder(-drivetrain_translate(data->main.left_drive))
.left_shifter_position(hall_translate(constants::GetValues().left_drive,
- data->main.left_drive_hall))
- .right_shifter_position(hall_translate(
- constants::GetValues().right_drive, data->main.right_drive_hall))
- .battery_voltage(battery_translate(data->main.battery_voltage))
+ data->main.low_left_drive_hall,
+ data->main.high_left_drive_hall))
+ .right_shifter_position(hall_translate(constants::GetValues().right_drive,
+ data->main.low_right_drive_hall,
+ data->main.high_right_drive_hall))
+ .battery_voltage(battery_translate(data->main.battery_voltage_high,
+ data->main.battery_voltage_low))
.Send();
{
diff --git a/frc971/output/led_setter.cc b/frc971/output/led_setter.cc
new file mode 100644
index 0000000..a69183d
--- /dev/null
+++ b/frc971/output/led_setter.cc
@@ -0,0 +1,19 @@
+#include "aos/common/logging/logging.h"
+#include "aos/linux_code/init.h"
+
+#include "bbb/led.h"
+
+#include "frc971/control_loops/claw/claw.q.h"
+
+using ::frc971::control_loops::claw_queue_group;
+
+int main() {
+ ::aos::InitNRT();
+
+ ::bbb::LED claw_zeroed(3);
+
+ while (true) {
+ CHECK(claw_queue_group.status.FetchNextBlocking());
+ claw_zeroed.Set(claw_queue_group.status->zeroed_for_auto);
+ }
+}
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index e17d6bc..dca221b 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -26,6 +26,12 @@
static constexpr ::aos::time::Time kOldLogInterval =
::aos::time::Time::InSeconds(0.5);
+ double Cap(double value, double max) {
+ if (value > max) return max;
+ if (value < -max) return -max;
+ return value;
+ }
+
virtual void RunIteration() {
values_.digital_module = 0;
values_.pressure_switch_channel = 1;
@@ -45,6 +51,7 @@
DisablePWMOutput(8);
LOG_INTERVAL(drivetrain_old_);
}
+ drivetrain_old_.Print();
}
{
@@ -61,6 +68,7 @@
SetSolenoid(5, false); // engage the brake
LOG_INTERVAL(shooter_old_);
}
+ shooter_old_.Print();
}
{
@@ -83,6 +91,7 @@
DisablePWMOutput(5);
LOG_INTERVAL(claw_old_);
}
+ claw_old_.Print();
}
}
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 6faca12..bc2d604 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -1,6 +1,19 @@
{
'targets': [
{
+ 'target_name': 'led_setter',
+ 'type': 'executable',
+ 'sources': [
+ 'led_setter.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:led',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ ],
+ },
+ {
'target_name': 'CameraServer',
'type': 'executable',
'sources': [
@@ -24,7 +37,7 @@
},
{
'target_name': 'motor_writer',
- 'type': '<(aos_target)',
+ 'type': 'executable',
'sources': [
'motor_writer.cc'
],
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 2b5d600..c4db795 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -13,6 +13,10 @@
'../control_loops/shooter/shooter.gyp:shooter',
'../control_loops/shooter/shooter.gyp:shooter_lib_test',
'../autonomous/autonomous.gyp:auto',
+ '../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',
@@ -21,12 +25,13 @@
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:packet_finder_test',
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:cows_test',
'<(DEPTH)/bbb_cape/src/flasher/flasher.gyp:stm32_flasher',
+ '../output/output.gyp:led_setter',
],
'copies': [
{
'destination': '<(rsync_dir)',
'files': [
- 'scripts/start_list.txt',
+ 'start_list.txt',
],
},
],
diff --git a/frc971/prime/scripts/start_list.txt b/frc971/prime/start_list.txt
similarity index 67%
rename from frc971/prime/scripts/start_list.txt
rename to frc971/prime/start_list.txt
index 179c122..94bf6bb 100644
--- a/frc971/prime/scripts/start_list.txt
+++ b/frc971/prime/start_list.txt
@@ -6,3 +6,6 @@
sensor_receiver
claw
shooter
+shoot_action
+drivetrain_action
+catch_action
diff --git a/frc971/queues/gyro_angle.q b/frc971/queues/gyro_angle.q
deleted file mode 100644
index bcf3ac4..0000000
--- a/frc971/queues/gyro_angle.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package frc971.sensors;
-
-message Gyro {
- double angle;
-};
-
-queue Gyro gyro;
diff --git a/frc971/queues/other_sensors.q b/frc971/queues/other_sensors.q
new file mode 100644
index 0000000..7181ce9
--- /dev/null
+++ b/frc971/queues/other_sensors.q
@@ -0,0 +1,12 @@
+package frc971.sensors;
+
+message OtherSensors {
+ double sonar_distance;
+ double plunger_hall_effect_distance;
+};
+queue OtherSensors other_sensors;
+
+message GyroReading {
+ double angle;
+};
+queue GyroReading gyro_reading;
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index ac53a70..8434437 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -1,7 +1,7 @@
{
'variables': {
'queue_files': [
- 'gyro_angle.q',
+ 'other_sensors.q',
'to_log.q',
]
},
diff --git a/frc971/queues/to_log.q b/frc971/queues/to_log.q
index 24e2402..ded9b64 100644
--- a/frc971/queues/to_log.q
+++ b/frc971/queues/to_log.q
@@ -3,5 +3,11 @@
struct CapeReading {
uint32_t sec;
uint32_t nsec;
- uint32_t sonar;
+ uint64_t struct_size;
+ double sonar;
+
+ uint16_t left_low;
+ uint16_t left_high;
+ uint16_t right_low;
+ uint16_t right_high;
};