Merge branch 'shooter_loop' into merged_loops
Conflicts:
frc971/atom_code/atom_code.gyp
diff --git a/aos/common/time.cc b/aos/common/time.cc
index 00c936d..995011a 100644
--- a/aos/common/time.cc
+++ b/aos/common/time.cc
@@ -8,21 +8,66 @@
#include "aos/common/logging/logging.h"
#include "aos/common/inttypes.h"
+#include "aos/common/mutex.h"
namespace aos {
namespace time {
-Time Time::Now(clockid_t clock) {
- timespec temp;
- if (clock_gettime(clock, &temp) != 0) {
- // TODO(aschuh): There needs to be a pluggable low level logging interface
- // so we can break this dependency loop. This also would help during
- // startup.
- LOG(FATAL, "clock_gettime(%jd, %p) failed with %d: %s\n",
- static_cast<uintmax_t>(clock), &temp, errno, strerror(errno));
- }
- return Time(temp);
+// State required to enable and use mock time.
+namespace {
+// True if mock time is enabled.
+// This does not need to be checked with the mutex held because setting time to
+// be enabled or disabled is atomic, and all future operations are atomic
+// anyways. If there is a race condition setting or clearing whether time is
+// enabled or not, it will still be a race condition if current_mock_time is
+// also set atomically with enabled.
+bool mock_time_enabled = false;
+// Mutex to make time reads and writes thread safe.
+Mutex time_mutex;
+// Current time when time is mocked.
+Time current_mock_time(0, 0);
+
+// TODO(aschuh): This doesn't include SleepFor and SleepUntil.
+// TODO(aschuh): Create a clock source object and change the default?
+// That would let me create a MockTime clock source.
}
+
+void Time::EnableMockTime(const Time now) {
+ mock_time_enabled = true;
+ MutexLocker time_mutex_locker(&time_mutex);
+ current_mock_time = now;
+}
+
+void Time::DisableMockTime() {
+ MutexLocker time_mutex_locker(&time_mutex);
+ mock_time_enabled = false;
+}
+
+void Time::SetMockTime(const Time now) {
+ MutexLocker time_mutex_locker(&time_mutex);
+ if (!mock_time_enabled) {
+ LOG(FATAL, "Tried to set mock time and mock time is not enabled\n");
+ }
+ current_mock_time = now;
+}
+
+Time Time::Now(clockid_t clock) {
+ if (mock_time_enabled) {
+ MutexLocker time_mutex_locker(&time_mutex);
+ return current_mock_time;
+ } else {
+ timespec temp;
+ if (clock_gettime(clock, &temp) != 0) {
+ // TODO(aschuh): There needs to be a pluggable low level logging interface
+ // so we can break this dependency loop. This also would help during
+ // startup.
+ LOG(FATAL, "clock_gettime(%jd, %p) failed with %d: %s\n",
+ static_cast<uintmax_t>(clock), &temp, errno, strerror(errno));
+ }
+ return Time(temp);
+ }
+}
+
void Time::Check() {
if (nsec_ >= kNSecInSec || nsec_ < 0) {
LOG(FATAL, "0 <= nsec_(%"PRId32") < %"PRId32" isn't true.\n",
diff --git a/aos/common/time.h b/aos/common/time.h
index 1d7ccae..38238f2 100644
--- a/aos/common/time.h
+++ b/aos/common/time.h
@@ -60,6 +60,7 @@
return ans;
}
#endif // SWIG
+
// CLOCK_MONOTONIC on the fitpc and CLOCK_REALTIME on the cRIO because the
// cRIO doesn't have any others.
// CLOCK_REALTIME is the default realtime clock and CLOCK_MONOTONIC doesn't
@@ -159,6 +160,15 @@
Check();
}
+ // Enables returning the mock time value for Now instead of checking the
+ // system clock. This should only be used when testing things depending on
+ // time, or many things may/will break.
+ static void EnableMockTime(const Time now);
+ // Sets now when time is being mocked.
+ static void SetMockTime(const Time now);
+ // Disables mocking time.
+ static void DisableMockTime();
+
private:
int32_t sec_, nsec_;
// LOG(FATAL)s if nsec_ is >= kNSecInSec.
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index ea1f8d2..e9c787e 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -8,6 +8,11 @@
'../control_loops/control_loops.gyp:DriveTrain',
'../control_loops/wrist/wrist.gyp:wrist',
'../control_loops/wrist/wrist.gyp:wrist_lib_test',
+ '../control_loops/index/index.gyp:index',
+ '../control_loops/index/index.gyp:index_lib_test',
+ '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust',
+ '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_lib_test',
+ '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_csv',
'../control_loops/shooter/shooter.gyp:shooter_lib_test',
'../control_loops/shooter/shooter.gyp:shooter',
'../input/input.gyp:JoystickReader',
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index a7bbfa5..e4c804a 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -11,6 +11,9 @@
#define M_PI 3.14159265358979323846
#endif
+// Note: So far, none of the Angle Adjust numbers have been measured.
+// Do not rely on them for real life.
+
namespace frc971 {
namespace constants {
@@ -36,6 +39,28 @@
const double kWristZeroingSpeed = 1.0;
+const int kAngleAdjustHallEffect = 2;
+
+const double kCompAngleAdjustHallEffectStartAngle[2] = {0.5, 1.5};
+const double kPracticeAngleAdjustHallEffectStartAngle[2] = {0.5, 1.5};
+
+const double kCompAngleAdjustHallEffectStopAngle[2] = {-0.1, 1.0};
+const double kPracticeAngleAdjustHallEffectStopAngle[2] = {-0.1, 1.0};
+
+const double kPracticeAngleAdjustUpperPhysicalLimit = 3.0;
+const double kCompAngleAdjustUpperPhysicalLimit = 3.0;
+
+const double kPracticeAngleAdjustLowerPhysicalLimit = 0.0;
+const double kCompAngleAdjustLowerPhysicalLimit = 0.0;
+
+const double kPracticeAngleAdjustUpperLimit = 3.0;
+const double kCompAngleAdjustUpperLimit = 3.0;
+
+const double kPracticeAngleAdjustLowerLimit = 0.0;
+const double kCompAngleAdjustLowerLimit = 0.0;
+
+const double kAngleAdjustZeroingSpeed = -1.0;
+
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
@@ -55,6 +80,20 @@
// Zeroing speed.
double wrist_zeroing_speed;
+ // AngleAdjust hall effect positive and negative edges.
+ const double *angle_adjust_hall_effect_start_angle;
+ const double *angle_adjust_hall_effect_stop_angle;
+
+ // Upper and lower extreme limits of travel for the angle adjust.
+ double angle_adjust_upper_limit;
+ double angle_adjust_lower_limit;
+ // Physical limits. These are here for testing.
+ double angle_adjust_upper_physical_limit;
+ double angle_adjust_lower_physical_limit;
+
+ // Zeroing speed.
+ double angle_adjust_zeroing_speed;
+
// what camera_center returns
int camera_center;
};
@@ -77,6 +116,13 @@
kCompWristUpperPhysicalLimit,
kCompWristLowerPhysicalLimit,
kWristZeroingSpeed,
+ kCompAngleAdjustHallEffectStartAngle,
+ kCompAngleAdjustHallEffectStopAngle,
+ kCompAngleAdjustUpperLimit,
+ kCompAngleAdjustLowerLimit,
+ kCompAngleAdjustUpperPhysicalLimit,
+ kCompAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
kCompCameraCenter};
break;
case kPracticeTeamNumber:
@@ -87,6 +133,13 @@
kPracticeWristUpperPhysicalLimit,
kPracticeWristLowerPhysicalLimit,
kWristZeroingSpeed,
+ kPracticeAngleAdjustHallEffectStartAngle,
+ kPracticeAngleAdjustHallEffectStopAngle,
+ kPracticeAngleAdjustUpperLimit,
+ kPracticeAngleAdjustLowerLimit,
+ kPracticeAngleAdjustUpperPhysicalLimit,
+ kPracticeAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
kPracticeCameraCenter};
break;
default:
@@ -147,6 +200,55 @@
return true;
}
+bool angle_adjust_hall_effect_start_angle(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ angle[0] = values->angle_adjust_hall_effect_start_angle[0];
+ angle[1] = values->angle_adjust_hall_effect_start_angle[1];
+ return true;
+}
+bool angle_adjust_hall_effect_stop_angle(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ angle[0] = values->angle_adjust_hall_effect_stop_angle[0];
+ angle[1] = values->angle_adjust_hall_effect_stop_angle[1];
+ return true;
+}
+bool angle_adjust_upper_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_upper_limit;
+ return true;
+}
+
+bool angle_adjust_lower_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_lower_limit;
+ return true;
+}
+
+bool angle_adjust_upper_physical_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_upper_physical_limit;
+ return true;
+}
+
+bool angle_adjust_lower_physical_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_lower_physical_limit;
+ return true;
+}
+
+bool angle_adjust_zeroing_speed(double *speed) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *speed = values->angle_adjust_zeroing_speed;
+ return true;
+}
+
bool camera_center(int *center) {
const Values *const values = GetValues();
if (values == NULL) return false;
diff --git a/frc971/constants.h b/frc971/constants.h
index 187ab6a..98baca4 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -25,6 +25,17 @@
// Returns the speed to move the wrist at when zeroing in rad/sec
bool wrist_zeroing_speed(double *speed);
+bool angle_adjust_hall_effect_start_angle(double *angle);
+bool angle_adjust_hall_effect_stop_angle(double *angle);
+// These are the soft stops for up and down.
+bool angle_adjust_lower_limit(double *angle);
+bool angle_adjust_upper_limit(double *angle);
+// These are the hard stops. Don't use these for anything but testing.
+bool angle_adjust_lower_physical_limit(double *angle);
+bool angle_adjust_upper_physical_limit(double *angle);
+
+// Returns speed to move the angle adjust when zeroing, in rad/sec
+bool angle_adjust_zeroing_speed(double *speed);
// Sets *center to how many pixels off center the vertical line
// on the camera view is.
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.cc b/frc971/control_loops/angle_adjust/angle_adjust.cc
new file mode 100644
index 0000000..8482a4b
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust.cc
@@ -0,0 +1,105 @@
+#include "frc971/control_loops/angle_adjust/angle_adjust.h"
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+AngleAdjustMotor::AngleAdjustMotor(
+ control_loops::AngleAdjustLoop *my_angle_adjust)
+ : aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop>(
+ my_angle_adjust),
+ zeroed_joint_(MakeAngleAdjustLoop()) {
+}
+
+bool AngleAdjustMotor::FetchConstants(
+ ZeroedJoint<2>::ConfigurationData *config_data) {
+ if (!constants::angle_adjust_lower_limit(
+ &config_data->lower_limit)) {
+ LOG(ERROR, "Failed to fetch the angle adjust lower limit constant.\n");
+ return false;
+ }
+ if (!constants::angle_adjust_upper_limit(
+ &config_data->upper_limit)) {
+ LOG(ERROR, "Failed to fetch the angle adjust upper limit constant.\n");
+ return false;
+ }
+ if (!constants::angle_adjust_hall_effect_start_angle(
+ config_data->hall_effect_start_angle)) {
+ LOG(ERROR, "Failed to fetch the hall effect start angle constants.\n");
+ return false;
+ }
+ if (!constants::angle_adjust_zeroing_speed(
+ &config_data->zeroing_speed)) {
+ LOG(ERROR, "Failed to fetch the angle adjust zeroing speed constant.\n");
+ return false;
+ }
+
+ config_data->max_zeroing_voltage = 4.0;
+ return true;
+}
+
+// Positive angle is up, and positive power is up.
+void AngleAdjustMotor::RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::AngleAdjustLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status * /*status*/) {
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ if (output) {
+ output->voltage = 0;
+ }
+
+ // Cache the constants to avoid error handling down below.
+ ZeroedJoint<2>::ConfigurationData config_data;
+ if (!FetchConstants(&config_data)) {
+ LOG(WARNING, "Failed to fetch constants.\n");
+ return;
+ } else {
+ zeroed_joint_.set_config_data(config_data);
+ }
+
+ ZeroedJoint<2>::PositionData transformed_position;
+ ZeroedJoint<2>::PositionData *transformed_position_ptr =
+ &transformed_position;
+ if (!position) {
+ transformed_position_ptr = NULL;
+ } else {
+ transformed_position.position = position->angle;
+ transformed_position.hall_effects[0] = position->bottom_hall_effect;
+ transformed_position.hall_effect_positions[0] =
+ position->bottom_calibration;
+ transformed_position.hall_effects[1] = position->middle_hall_effect;
+ transformed_position.hall_effect_positions[1] =
+ position->middle_calibration;
+ }
+
+ const double voltage = zeroed_joint_.Update(transformed_position_ptr,
+ output != NULL,
+ goal->goal, 0.0);
+
+ if (position) {
+ LOG(DEBUG, "pos=%f bottom_hall: %s middle_hall: %s\n",
+ position->angle,
+ position->bottom_hall_effect ? "true" : "false",
+ position->middle_hall_effect ? "true" : "false");
+ }
+
+ if (output) {
+ output->voltage = voltage;
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.gyp b/frc971/control_loops/angle_adjust/angle_adjust.gyp
new file mode 100644
index 0000000..cb3ba1b
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust.gyp
@@ -0,0 +1,84 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'angle_adjust_loop',
+ 'type': 'static_library',
+ 'sources': ['angle_adjust_motor.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/angle_adjust',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'angle_adjust_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'angle_adjust.cc',
+ 'angle_adjust_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ 'angle_adjust_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ 'angle_adjust_loop',
+ ],
+ },
+ {
+ 'target_name': 'angle_adjust_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'angle_adjust_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ 'angle_adjust_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ 'angle_adjust_loop',
+ ],
+ },
+ {
+ 'target_name': 'angle_adjust_csv',
+ 'type': 'executable',
+ 'sources': [
+ 'angle_adjust_csv.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ 'angle_adjust_loop',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ ],
+ },
+ {
+ 'target_name': 'angle_adjust',
+ 'type': 'executable',
+ 'sources': [
+ 'angle_adjust_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'angle_adjust_lib',
+ 'angle_adjust_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.h b/frc971/control_loops/angle_adjust/angle_adjust.h
new file mode 100644
index 0000000..dfa8ad2
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust.h
@@ -0,0 +1,68 @@
+#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
+#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
+
+#include <array>
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
+#include "frc971/control_loops/zeroed_joint.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// Allows the control loop to add the tests to access private members.
+namespace testing {
+class AngleAdjustTest_RezeroWithMissingPos_Test;
+class AngleAdjustTest_DisableGoesUninitialized_Test;
+}
+
+class AngleAdjustMotor
+ : public aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop> {
+ public:
+ explicit AngleAdjustMotor(
+ control_loops::AngleAdjustLoop *my_angle_adjust =
+ &control_loops::angle_adjust);
+ protected:
+ virtual void RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::AngleAdjustLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status *status);
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+
+ // True if the wrist is zeroing.
+ bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+
+ // True if the wrist is zeroing.
+ bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return zeroed_joint_.is_ready(); }
+
+ private:
+ // Allows the testing code to access some of private members.
+ friend class testing::AngleAdjustTest_RezeroWithMissingPos_Test;
+ friend class testing::AngleAdjustTest_DisableGoesUninitialized_Test;
+
+ // Fetches and locally caches the latest set of constants.
+ // Returns whether it succeeded or not.
+ bool FetchConstants(ZeroedJoint<2>::ConfigurationData *config_data);
+
+ // The zeroed joint to use.
+ ZeroedJoint<2> zeroed_joint_;
+
+ DISALLOW_COPY_AND_ASSIGN(AngleAdjustMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_csv.cc b/frc971/control_loops/angle_adjust/angle_adjust_csv.cc
new file mode 100644
index 0000000..6d484d5
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_csv.cc
@@ -0,0 +1,52 @@
+#include "stdio.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
+
+using ::frc971::control_loops::angle_adjust;
+using ::aos::time::Time;
+
+// Records data from the queue and stores it in a .csv file which can then
+// be plotted/processed with relative ease.
+int main(int argc, char * argv[]) {
+ FILE *data_file = NULL;
+ FILE *output_file = NULL;
+
+ if (argc == 2) {
+ data_file = fopen(argv[1], "w");
+ output_file = data_file;
+ } else {
+ printf("Not saving to a CSV file.\n");
+ output_file = stdout;
+ }
+
+ fprintf(data_file, "time, power, position");
+
+ ::aos::Init();
+
+ Time start_time = Time::Now();
+
+ while (true) {
+ ::aos::time::PhasedLoop10MS(2000);
+ angle_adjust.goal.FetchLatest();
+ angle_adjust.status.FetchLatest();
+ angle_adjust.position.FetchLatest();
+ angle_adjust.output.FetchLatest();
+ if (angle_adjust.output.get() &&
+ angle_adjust.position.get()) {
+ fprintf(output_file, "\n%f, %f, %f",
+ (angle_adjust.position->sent_time - start_time).ToSeconds(),
+ angle_adjust.output->voltage,
+ angle_adjust.position->angle);
+ }
+ }
+
+ if (data_file) {
+ fclose(data_file);
+ }
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
new file mode 100644
index 0000000..f8a7c63
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -0,0 +1,308 @@
+#include <unistd.h>
+
+#include <memory>
+#include <array>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+
+// Class which simulates the angle_adjust and
+// sends out queue messages containing the position.
+class AngleAdjustMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // angle_adjust, which will be treated as 0 by the encoder.
+ explicit AngleAdjustMotorSimulation(double initial_position)
+ : angle_adjust_plant_(
+ new StateFeedbackPlant<2, 1, 1>(MakeAngleAdjustPlant())),
+ my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
+ 0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
+ ".frc971.control_loops.angle_adjust.position",
+ ".frc971.control_loops.angle_adjust.output",
+ ".frc971.control_loops.angle_adjust.status") {
+ Reinitialize(initial_position);
+ }
+
+ // Resets the plant so that it starts at initial_position.
+ void Reinitialize(double initial_position) {
+ initial_position_ = initial_position;
+ angle_adjust_plant_->X(0, 0) = initial_position_;
+ angle_adjust_plant_->X(1, 0) = 0.0;
+ angle_adjust_plant_->Y = angle_adjust_plant_->C * angle_adjust_plant_->X;
+ last_position_ = angle_adjust_plant_->Y(0, 0);
+ calibration_value_[0] = 0.0;
+ calibration_value_[1] = 0.0;
+ }
+
+ // Returns the absolute angle of the angle_adjust.
+ double GetAbsolutePosition() const {
+ return angle_adjust_plant_->Y(0, 0);
+ }
+
+ // Returns the adjusted angle of the angle_adjust.
+ double GetPosition() const {
+ return GetAbsolutePosition() - initial_position_;
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const double angle = GetPosition();
+
+ double hall_effect_start_angle[2];
+ ASSERT_TRUE(constants::angle_adjust_hall_effect_start_angle(
+ hall_effect_start_angle));
+ double hall_effect_stop_angle[2];
+ ASSERT_TRUE(constants::angle_adjust_hall_effect_stop_angle(
+ hall_effect_stop_angle));
+
+ ::aos::ScopedMessagePtr<control_loops::AngleAdjustLoop::Position> position =
+ my_angle_adjust_loop_.position.MakeMessage();
+ position->angle = angle;
+
+ // Signal that the hall effect sensor has been triggered if it is within
+ // the correct range.
+ double abs_position = GetAbsolutePosition();
+ if (abs_position <= hall_effect_start_angle[0] &&
+ abs_position >= hall_effect_stop_angle[0]) {
+ position->bottom_hall_effect = true;
+ } else {
+ position->bottom_hall_effect = false;
+ }
+ if (abs_position <= hall_effect_start_angle[1] &&
+ abs_position >= hall_effect_stop_angle[1]) {
+ position->middle_hall_effect = true;
+ } else {
+ position->middle_hall_effect = false;
+ }
+
+ // Only set calibration if it changed last cycle. Calibration starts out
+ // with a value of 0.
+ // TODO(aschuh): This won't deal with both edges correctly.
+ if ((last_position_ < hall_effect_start_angle[0] ||
+ last_position_ > hall_effect_stop_angle[0]) &&
+ (position->bottom_hall_effect)) {
+ calibration_value_[0] = hall_effect_start_angle[0] - initial_position_;
+ }
+ if ((last_position_ < hall_effect_start_angle[1] ||
+ last_position_ > hall_effect_stop_angle[1]) &&
+ (position->middle_hall_effect)) {
+ calibration_value_[1] = hall_effect_start_angle[1] - initial_position_;
+ }
+
+ position->bottom_calibration = calibration_value_[0];
+ position->middle_calibration = calibration_value_[1];
+ position.Send();
+ }
+
+ // Simulates the angle_adjust moving for one timestep.
+ void Simulate() {
+ last_position_ = angle_adjust_plant_->Y(0, 0);
+ EXPECT_TRUE(my_angle_adjust_loop_.output.FetchLatest());
+ angle_adjust_plant_->U << my_angle_adjust_loop_.output->voltage;
+ angle_adjust_plant_->Update();
+
+ // Assert that we are in the right physical range.
+ double upper_physical_limit;
+ ASSERT_TRUE(constants::angle_adjust_upper_physical_limit(
+ &upper_physical_limit));
+ double lower_physical_limit;
+ ASSERT_TRUE(constants::angle_adjust_lower_physical_limit(
+ &lower_physical_limit));
+
+ EXPECT_GE(upper_physical_limit, angle_adjust_plant_->Y(0, 0));
+ EXPECT_LE(lower_physical_limit, angle_adjust_plant_->Y(0, 0));
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> angle_adjust_plant_;
+
+ private:
+ AngleAdjustLoop my_angle_adjust_loop_;
+ double initial_position_;
+ double last_position_;
+ double calibration_value_[2];
+};
+
+class AngleAdjustTest : public ::testing::Test {
+ protected:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ AngleAdjustLoop my_angle_adjust_loop_;
+
+ // Create a loop and simulation plant.
+ AngleAdjustMotor angle_adjust_motor_;
+ AngleAdjustMotorSimulation angle_adjust_motor_plant_;
+
+ AngleAdjustTest() :
+ my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
+ 0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
+ ".frc971.control_loops.angle_adjust.position",
+ ".frc971.control_loops.angle_adjust.output",
+ ".frc971.control_loops.angle_adjust.status"),
+ angle_adjust_motor_(&my_angle_adjust_loop_),
+ angle_adjust_motor_plant_(0.75) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_angle_adjust_loop_.goal.FetchLatest();
+ my_angle_adjust_loop_.position.FetchLatest();
+ EXPECT_NEAR(my_angle_adjust_loop_.goal->goal,
+ angle_adjust_motor_plant_.GetAbsolutePosition(),
+ 1e-4);
+ }
+
+ virtual ~AngleAdjustTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the angle_adjust zeros correctly and goes to a position.
+TEST_F(AngleAdjustTest, ZerosCorrectly) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the angle_adjust zeros correctly starting on the sensor.
+TEST_F(AngleAdjustTest, ZerosStartingOn) {
+ angle_adjust_motor_plant_.Reinitialize(0.25);
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that missing positions are correctly handled.
+TEST_F(AngleAdjustTest, HandleMissingPosition) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ if (i % 23) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ }
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that loosing the encoder for a second triggers a re-zero.
+TEST_F(AngleAdjustTest, RezeroWithMissingPos) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 800; ++i) {
+ // After 3 seconds, simulate the encoder going missing.
+ // This should trigger a re-zero. To make sure it works, change the goal as
+ // well.
+ if (i < 300 || i > 400) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ } else {
+ if (i > 310) {
+ // Should be re-zeroing now.
+ EXPECT_TRUE(angle_adjust_motor_.is_uninitialized());
+ }
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.2).Send();
+ }
+ if (i == 430) {
+ EXPECT_TRUE(angle_adjust_motor_.is_zeroing() ||
+ angle_adjust_motor_.is_moving_off());
+ }
+
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that disabling while zeroing sends the state machine into the
+// uninitialized state.
+TEST_F(AngleAdjustTest, DisableGoesUninitialized) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 800; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ // After 0.5 seconds, disable the robot.
+ if (i > 50 && i < 200) {
+ SendDSPacket(false);
+ if (i > 100) {
+ // Give the loop a couple cycled to get the message and then verify that
+ // it is in the correct state.
+ EXPECT_TRUE(angle_adjust_motor_.is_uninitialized());
+ }
+ } else {
+ SendDSPacket(true);
+ }
+ if (i == 202) {
+ // Verify that we are zeroing after the bot gets enabled again.
+ EXPECT_TRUE(angle_adjust_motor_.is_zeroing());
+ }
+
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the angle_adjust zeros correctly from above the second sensor.
+TEST_F(AngleAdjustTest, ZerosCorrectlyAboveSecond) {
+ angle_adjust_motor_plant_.Reinitialize(1.75);
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(2.0).Send();
+ for (int i = 0; i < 400; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the angle_adjust zeros correctly starting on
+// the second hall effect sensor.
+TEST_F(AngleAdjustTest, ZerosStartingOnSecond) {
+ angle_adjust_motor_plant_.Reinitialize(1.25);
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(2.0).Send();
+ for (int i = 0; i < 500; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_main.cc b/frc971/control_loops/angle_adjust/angle_adjust_main.cc
new file mode 100644
index 0000000..b9f8189
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/angle_adjust/angle_adjust.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ ::frc971::control_loops::AngleAdjustMotor angle_adjust;
+ angle_adjust.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor.q b/frc971/control_loops/angle_adjust/angle_adjust_motor.q
new file mode 100644
index 0000000..a98419a
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor.q
@@ -0,0 +1,24 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group AngleAdjustLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Position {
+ // Angle of the height adjust.
+ double angle;
+ bool bottom_hall_effect;
+ bool middle_hall_effect;
+ // The exact position when the corresponding hall_effect changed.
+ double bottom_calibration;
+ double middle_calibration;
+ };
+
+ queue aos.control_loops.Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue aos.control_loops.Status status;
+};
+
+queue_group AngleAdjustLoop angle_adjust;
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
new file mode 100644
index 0000000..a3c89a7
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00135041324202, 0.0, 0.000610875713246;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.00111752476609, 0.129120861909;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 0.900610875713, 1.85371819524;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 12.7787251077, -5.83693180893;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeAngleAdjustPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
new file mode 100644
index 0000000..0bcd582
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_ANGLEADJUST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_ANGLEADJUST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_ANGLEADJUST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
new file mode 100644
index 0000000..c8f5cc8
--- /dev/null
+++ b/frc971/control_loops/index/index.cc
@@ -0,0 +1,908 @@
+#include "frc971/control_loops/index/index.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/inttypes.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/index/index_motor_plant.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+
+double IndexMotor::Frisbee::ObserveNoTopDiscSensor(
+ double index_position, double index_velocity) {
+ // The absolute disc position in meters.
+ double disc_position = absolute_position(index_position);
+ if (IndexMotor::kTopDiscDetectStart <= disc_position &&
+ disc_position <= IndexMotor::kTopDiscDetectStop) {
+ // Whoops, this shouldn't be happening.
+ // Move the disc off the way that makes most sense.
+ double distance_to_above = IndexMotor::ConvertDiscPositionToIndex(
+ ::std::abs(disc_position - IndexMotor::kTopDiscDetectStop));
+ double distance_to_below = IndexMotor::ConvertDiscPositionToIndex(
+ ::std::abs(disc_position - IndexMotor::kTopDiscDetectStart));
+ if (::std::abs(index_velocity) < 100) {
+ if (distance_to_above < distance_to_below) {
+ printf("Moving disc to top slow.\n");
+ // Move it up.
+ index_start_position_ -= distance_to_above;
+ return -distance_to_above;
+ } else {
+ printf("Moving disc to bottom slow.\n");
+ index_start_position_ += distance_to_below;
+ return distance_to_below;
+ }
+ } else {
+ if (index_velocity > 0) {
+ // Now going up. If we didn't see it before, and we don't see it
+ // now but it should be in view, it must still be below. If it were
+ // above, it would be going further away from us.
+ printf("Moving fast up, shifting disc down. Disc was at %f\n",
+ absolute_position(index_position));
+ index_start_position_ += distance_to_below;
+ printf("Moving fast up, shifting disc down. Disc now at %f\n",
+ absolute_position(index_position));
+ return distance_to_below;
+ } else {
+ printf("Moving fast down, shifting disc up. Disc was at %f\n",
+ absolute_position(index_position));
+ index_start_position_ -= distance_to_above;
+ printf("Moving fast down, shifting disc up. Disc now at %f\n",
+ absolute_position(index_position));
+ return -distance_to_above;
+ }
+ }
+ }
+ return 0.0;
+}
+
+IndexMotor::IndexMotor(control_loops::IndexLoop *my_index)
+ : aos::control_loops::ControlLoop<control_loops::IndexLoop>(my_index),
+ wrist_loop_(new IndexStateFeedbackLoop(MakeIndexLoop())),
+ hopper_disc_count_(0),
+ total_disc_count_(0),
+ shot_disc_count_(0),
+ safe_goal_(Goal::HOLD),
+ loader_goal_(LoaderGoal::READY),
+ loader_state_(LoaderState::READY),
+ loader_up_(false),
+ disc_clamped_(false),
+ disc_ejected_(false),
+ last_bottom_disc_detect_(false),
+ last_top_disc_detect_(false),
+ no_prior_position_(true),
+ missing_position_count_(0) {
+}
+
+/*static*/ const double IndexMotor::kTransferStartPosition = 0.0;
+/*static*/ const double IndexMotor::kIndexStartPosition = 0.2159;
+/*static*/ const double IndexMotor::kIndexFreeLength =
+ IndexMotor::ConvertDiscAngleToDiscPosition((360 * 2 + 14) * M_PI / 180);
+/*static*/ const double IndexMotor::kLoaderFreeStopPosition =
+ kIndexStartPosition + kIndexFreeLength;
+/*static*/ const double IndexMotor::kReadyToPreload =
+ kLoaderFreeStopPosition - ConvertDiscAngleToDiscPosition(M_PI / 6.0);
+/*static*/ const double IndexMotor::kReadyToLiftPosition =
+ kLoaderFreeStopPosition + 0.2921;
+/*static*/ const double IndexMotor::kGrabberLength = 0.03175;
+/*static*/ const double IndexMotor::kGrabberStartPosition =
+ kReadyToLiftPosition - kGrabberLength;
+/*static*/ const double IndexMotor::kGrabberMovementVelocity = 0.7;
+/*static*/ const double IndexMotor::kLifterStopPosition =
+ kReadyToLiftPosition + 0.161925;
+/*static*/ const double IndexMotor::kLifterMovementVelocity = 1.0;
+/*static*/ const double IndexMotor::kEjectorStopPosition =
+ kLifterStopPosition + 0.01;
+/*static*/ const double IndexMotor::kEjectorMovementVelocity = 1.0;
+/*static*/ const double IndexMotor::kBottomDiscDetectStart = 0.00;
+/*static*/ const double IndexMotor::kBottomDiscDetectStop = 0.13;
+/*static*/ const double IndexMotor::kBottomDiscIndexDelay = 0.032;
+
+// TODO(aschuh): Verify these with the sensor actually on.
+/*static*/ const double IndexMotor::kTopDiscDetectStart =
+ (IndexMotor::kLoaderFreeStopPosition -
+ IndexMotor::ConvertDiscAngleToDiscPosition(49 * M_PI / 180));
+/*static*/ const double IndexMotor::kTopDiscDetectStop =
+ (IndexMotor::kLoaderFreeStopPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(19 * M_PI / 180));
+
+// I measured the angle between 2 discs. That then gives me the distance
+// between 2 posedges (or negedges). Then subtract off the width of the
+// positive pulse, and that gives the width of the negative pulse.
+/*static*/ const double IndexMotor::kTopDiscDetectMinSeperation =
+ (IndexMotor::ConvertDiscAngleToDiscPosition(120 * M_PI / 180) -
+ (IndexMotor::kTopDiscDetectStop - IndexMotor::kTopDiscDetectStart));
+
+const /*static*/ double IndexMotor::kDiscRadius = 10.875 * 0.0254 / 2;
+const /*static*/ double IndexMotor::kRollerRadius = 2.0 * 0.0254 / 2;
+const /*static*/ double IndexMotor::kTransferRollerRadius = 1.25 * 0.0254 / 2;
+
+/*static*/ const int IndexMotor::kGrabbingDelay = 5;
+/*static*/ const int IndexMotor::kLiftingDelay = 20;
+/*static*/ const int IndexMotor::kShootingDelay = 5;
+/*static*/ const int IndexMotor::kLoweringDelay = 20;
+
+// TODO(aschuh): Tune these.
+/*static*/ const double
+ IndexMotor::IndexStateFeedbackLoop::kMinMotionVoltage = 5.0;
+/*static*/ const double
+ IndexMotor::IndexStateFeedbackLoop::kNoMotionCuttoffCount = 30;
+
+// Distance to move the indexer when grabbing a disc.
+const double kNextPosition = 10.0;
+
+/*static*/ double IndexMotor::ConvertDiscAngleToIndex(const double angle) {
+ return (angle * (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
+}
+
+/*static*/ double IndexMotor::ConvertDiscAngleToDiscPosition(
+ const double angle) {
+ return angle * (kDiscRadius + kRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertDiscPositionToDiscAngle(
+ const double position) {
+ return position / (kDiscRadius + kRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertIndexToDiscAngle(const double angle) {
+ return (angle / (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
+}
+
+/*static*/ double IndexMotor::ConvertIndexToDiscPosition(const double angle) {
+ return IndexMotor::ConvertDiscAngleToDiscPosition(
+ ConvertIndexToDiscAngle(angle));
+}
+
+/*static*/ double IndexMotor::ConvertTransferToDiscPosition(
+ const double angle) {
+ const double gear_ratio = (1 + (kDiscRadius * 2 + kTransferRollerRadius) /
+ kTransferRollerRadius);
+ return angle / gear_ratio * (kDiscRadius + kTransferRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertDiscPositionToIndex(
+ const double position) {
+ return IndexMotor::ConvertDiscAngleToIndex(
+ ConvertDiscPositionToDiscAngle(position));
+}
+
+bool IndexMotor::MinDiscPosition(double *disc_position, Frisbee **found_disc) {
+ bool found_start = false;
+ for (unsigned int i = 0; i < frisbees_.size(); ++i) {
+ Frisbee &frisbee = frisbees_[i];
+ if (!found_start) {
+ if (frisbee.has_position()) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ found_start = true;
+ }
+ } else {
+ if (frisbee.position() <= *disc_position) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ }
+ }
+ }
+ return found_start;
+}
+
+bool IndexMotor::MaxDiscPosition(double *disc_position, Frisbee **found_disc) {
+ bool found_start = false;
+ for (unsigned int i = 0; i < frisbees_.size(); ++i) {
+ Frisbee &frisbee = frisbees_[i];
+ if (!found_start) {
+ if (frisbee.has_position()) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ found_start = true;
+ }
+ } else {
+ if (frisbee.position() > *disc_position) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ }
+ }
+ }
+ return found_start;
+}
+
+void IndexMotor::IndexStateFeedbackLoop::CapU() {
+ // If the voltage has been low for a large number of cycles, cut the motor
+ // power. This is generally very bad controls practice since this isn't LTI,
+ // but we don't really care about tracking anything other than large step
+ // inputs, and the loader doesn't need to be that accurate.
+ if (::std::abs(U(0, 0)) < kMinMotionVoltage) {
+ ++low_voltage_count_;
+ if (low_voltage_count_ > kNoMotionCuttoffCount) {
+ printf("Limiting power from %f to 0\n", U(0, 0));
+ U(0, 0) = 0.0;
+ }
+ } else {
+ low_voltage_count_ = 0;
+ }
+
+ for (int i = 0; i < kNumOutputs; ++i) {
+ if (U[i] > plant.U_max[i]) {
+ U[i] = plant.U_max[i];
+ } else if (U[i] < plant.U_min[i]) {
+ U[i] = plant.U_min[i];
+ }
+ }
+}
+
+
+// Positive angle is towards the shooter, and positive power is towards the
+// shooter.
+void IndexMotor::RunIteration(
+ const control_loops::IndexLoop::Goal *goal,
+ const control_loops::IndexLoop::Position *position,
+ control_loops::IndexLoop::Output *output,
+ control_loops::IndexLoop::Status *status) {
+ // Make goal easy to work with and sanity check it.
+ Goal goal_enum = static_cast<Goal>(goal->goal_state);
+ if (goal->goal_state < 0 || goal->goal_state > 4) {
+ LOG(ERROR, "Goal state is %"PRId32" which is out of range. Going to HOLD.\n",
+ goal->goal_state);
+ goal_enum = Goal::HOLD;
+ }
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ double intake_voltage = 0.0;
+ double transfer_voltage = 0.0;
+ if (output) {
+ output->intake_voltage = 0.0;
+ output->transfer_voltage = 0.0;
+ output->index_voltage = 0.0;
+ }
+
+ status->ready_to_intake = false;
+
+ // Compute a safe index position that we can use.
+ if (position) {
+ wrist_loop_->Y << position->index_position;
+ // Set the goal to be the current position if this is the first time through
+ // so we don't always spin the indexer to the 0 position before starting.
+ if (no_prior_position_) {
+ wrist_loop_->R << wrist_loop_->Y(0, 0), 0.0;
+ wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
+ no_prior_position_ = false;
+ last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
+ last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
+ last_bottom_disc_negedge_wait_count_ =
+ position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ // The open positions for the upper is right here and isn't a hard edge.
+ upper_open_region_.Restart(wrist_loop_->Y(0, 0));
+ lower_open_region_.Restart(wrist_loop_->Y(0, 0));
+ }
+
+ // If the cRIO is gone for over 1/2 of a second, assume that it rebooted.
+ if (missing_position_count_ > 50) {
+ last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
+ last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
+ last_bottom_disc_negedge_wait_count_ =
+ position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ // We can't really trust the open range any more if the crio rebooted.
+ upper_open_region_.Restart(wrist_loop_->Y(0, 0));
+ lower_open_region_.Restart(wrist_loop_->Y(0, 0));
+ // Adjust the disc positions so that they don't have to move.
+ const double disc_offset =
+ position->index_position - wrist_loop_->X_hat(0, 0);
+ for (auto frisbee = frisbees_.begin();
+ frisbee != frisbees_.end(); ++frisbee) {
+ frisbee->OffsetDisc(disc_offset);
+ }
+ wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
+ }
+ missing_position_count_ = 0;
+ } else {
+ ++missing_position_count_;
+ }
+ const double index_position = wrist_loop_->X_hat(0, 0);
+
+ if (position) {
+ // Reset the open region if we saw a negedge.
+ if (position->bottom_disc_negedge_wait_count !=
+ last_bottom_disc_negedge_wait_count_) {
+ // Saw a negedge, must be a new region.
+ lower_open_region_.Restart(position->bottom_disc_negedge_wait_position);
+ }
+ // Reset the open region if we saw a negedge.
+ if (position->top_disc_negedge_count != last_top_disc_negedge_count_) {
+ // Saw a negedge, must be a new region.
+ upper_open_region_.Restart(position->top_disc_negedge_position);
+ }
+
+ // No disc. Expand the open region.
+ if (!position->bottom_disc_detect) {
+ lower_open_region_.Expand(index_position);
+ }
+
+ // No disc. Expand the open region.
+ if (!position->top_disc_detect) {
+ upper_open_region_.Expand(index_position);
+ }
+
+ if (!position->top_disc_detect) {
+ // We don't see a disc. Verify that there are no discs that we should be
+ // seeing.
+ // Assume that discs will move slow enough that we won't miss one as it
+ // goes by. They will either pile up above or below the sensor.
+
+ double cumulative_offset = 0.0;
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ frisbee->OffsetDisc(cumulative_offset);
+ double amount_moved = frisbee->ObserveNoTopDiscSensor(
+ wrist_loop_->X_hat(0, 0), wrist_loop_->X_hat(1, 0));
+ cumulative_offset += amount_moved;
+ }
+ }
+
+ if (position->top_disc_posedge_count != last_top_disc_posedge_count_) {
+ const double index_position = wrist_loop_->X_hat(0, 0) -
+ position->index_position + position->top_disc_posedge_position;
+ // TODO(aschuh): Sanity check this number...
+ // Requires storing when the disc was last seen with the sensor off, and
+ // figuring out what to do if things go south.
+
+ // 1 if discs are going up, 0 if we have no clue, and -1 if they are going
+ // down.
+ int disc_direction = 0;
+ if (wrist_loop_->X_hat(1, 0) > 50.0) {
+ disc_direction = 1;
+ } else if (wrist_loop_->X_hat(1, 0) < -50.0) {
+ disc_direction = -1;
+ } else {
+ // Save the upper and lower positions that we last saw a disc at.
+ // If there is a big buffer above, must be a disc from below.
+ // If there is a big buffer below, must be a disc from above.
+ // This should work to replace the velocity threshold above.
+
+ const double open_width = upper_open_region_.width();
+ const double relative_upper_open_precentage =
+ (upper_open_region_.upper_bound() - index_position) / open_width;
+ const double relative_lower_open_precentage =
+ (index_position - upper_open_region_.lower_bound()) / open_width;
+ printf("Width %f upper %f lower %f\n",
+ open_width, relative_upper_open_precentage,
+ relative_lower_open_precentage);
+
+ if (ConvertIndexToDiscPosition(open_width) <
+ kTopDiscDetectMinSeperation * 0.9) {
+ LOG(ERROR, "Discs are way too close to each other. Doing nothing\n");
+ } else if (relative_upper_open_precentage > 0.75) {
+ // Looks like it is a disc going down from above since we are near
+ // the upper edge.
+ disc_direction = -1;
+ printf("Disc edge going down\n");
+ } else if (relative_lower_open_precentage > 0.75) {
+ // Looks like it is a disc going up from below since we are near
+ // the lower edge.
+ disc_direction = 1;
+ printf("Disc edge going up\n");
+ } else {
+ LOG(ERROR,
+ "Got an edge in the middle of what should be an open region.\n");
+ LOG(ERROR, "Open width: %f upper precentage %f %%\n",
+ open_width, relative_upper_open_precentage);
+ }
+ }
+
+ if (disc_direction > 0) {
+ // Moving up at a reasonable clip.
+ // Find the highest disc that is below the top disc sensor.
+ // While we are at it, count the number above and log an error if there
+ // are too many.
+ if (frisbees_.size() == 0) {
+ Frisbee new_frisbee;
+ new_frisbee.has_been_indexed_ = true;
+ new_frisbee.index_start_position_ = index_position -
+ ConvertDiscPositionToIndex(kTopDiscDetectStart -
+ kIndexStartPosition);
+ ++hopper_disc_count_;
+ ++total_disc_count_;
+ frisbees_.push_front(new_frisbee);
+ LOG(WARNING, "Added a disc to the hopper at the top sensor\n");
+ }
+
+ int above_disc_count = 0;
+ double highest_position = 0;
+ Frisbee *highest_frisbee_below_sensor = NULL;
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ const double disc_position = frisbee->absolute_position(
+ index_position);
+ // It is save to use the top position for the cuttoff, since the
+ // sensor being low will result in discs being pushed off of it.
+ if (disc_position >= kTopDiscDetectStop) {
+ ++above_disc_count;
+ } else if (!highest_frisbee_below_sensor ||
+ disc_position > highest_position) {
+ highest_frisbee_below_sensor = &*frisbee;
+ highest_position = disc_position;
+ }
+ }
+ if (above_disc_count > 1) {
+ LOG(ERROR, "We have 2 discs above the top sensor.\n");
+ }
+
+ // We now have the disc. Shift all the ones below the sensor up by the
+ // computed delta.
+ const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
+ highest_position - kTopDiscDetectStart);
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ const double disc_position = frisbee->absolute_position(
+ index_position);
+ if (disc_position < kTopDiscDetectStop) {
+ frisbee->OffsetDisc(disc_delta);
+ }
+ }
+ printf("Currently have %d discs, saw posedge moving up. "
+ "Moving down by %f to %f\n", frisbees_.size(),
+ ConvertIndexToDiscPosition(disc_delta),
+ highest_frisbee_below_sensor->absolute_position(
+ wrist_loop_->X_hat(0, 0)));
+ } else if (disc_direction < 0) {
+ // Moving down at a reasonable clip.
+ // There can only be 1 disc up top that would give us a posedge.
+ // Find it and place it at the one spot that it can be.
+ double min_disc_position = 0;
+ Frisbee *min_frisbee = NULL;
+ MinDiscPosition(&min_disc_position, &min_frisbee);
+ if (!min_frisbee) {
+ // Uh, oh, we see a disc but there isn't one...
+ LOG(ERROR, "Saw a disc up top but there isn't one in the hopper\n");
+ } else {
+ const double disc_position = min_frisbee->absolute_position(
+ index_position);
+
+ const double disc_delta_meters = disc_position - kTopDiscDetectStop;
+ const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
+ disc_delta_meters);
+ printf("Posedge going down. Moving top disc down by %f\n",
+ disc_delta_meters);
+ for (auto frisbee = frisbees_.begin(), end = frisbees_.end();
+ frisbee != end; ++frisbee) {
+ frisbee->OffsetDisc(disc_delta);
+ }
+ }
+ } else {
+ LOG(ERROR, "Not sure how to handle the upper posedge, doing nothing\n");
+ }
+ }
+ }
+
+ // Bool to track if it is safe for the goal to change yet.
+ bool safe_to_change_state_ = true;
+ switch (safe_goal_) {
+ case Goal::HOLD:
+ // The goal should already be good, so sit tight with everything the same
+ // as it was.
+ break;
+ case Goal::READY_LOWER:
+ case Goal::INTAKE:
+ {
+ Time now = Time::Now();
+ if (position) {
+ // Posedge of the disc entering the beam break.
+ if (position->bottom_disc_posedge_count !=
+ last_bottom_disc_posedge_count_) {
+ transfer_frisbee_.Reset();
+ transfer_frisbee_.bottom_posedge_time_ = now;
+ printf("Posedge of bottom disc %f\n",
+ transfer_frisbee_.bottom_posedge_time_.ToSeconds());
+ ++hopper_disc_count_;
+ ++total_disc_count_;
+ }
+
+ // Disc exited the beam break now.
+ if (position->bottom_disc_negedge_count !=
+ last_bottom_disc_negedge_count_) {
+ transfer_frisbee_.bottom_negedge_time_ = now;
+ printf("Negedge of bottom disc %f\n",
+ transfer_frisbee_.bottom_negedge_time_.ToSeconds());
+ frisbees_.push_front(transfer_frisbee_);
+ }
+
+ if (position->bottom_disc_detect) {
+ intake_voltage = transfer_voltage = 12.0;
+ // Must wait until the disc gets out before we can change state.
+ safe_to_change_state_ = false;
+
+ // TODO(aschuh): A disc on the way through needs to start moving
+ // the indexer if it isn't already moving. Maybe?
+
+ Time elapsed_posedge_time = now -
+ transfer_frisbee_.bottom_posedge_time_;
+ if (elapsed_posedge_time >= Time::InSeconds(0.3)) {
+ // It has been too long. The disc must be jammed.
+ LOG(ERROR, "Been way too long. Jammed disc?\n");
+ printf("Been way too long. Jammed disc?\n");
+ }
+ }
+
+ // Check all non-indexed discs and see if they should be indexed.
+ for (auto frisbee = frisbees_.begin();
+ frisbee != frisbees_.end(); ++frisbee) {
+ if (!frisbee->has_been_indexed_) {
+ intake_voltage = transfer_voltage = 12.0;
+
+ if (last_bottom_disc_negedge_wait_count_ !=
+ position->bottom_disc_negedge_wait_count) {
+ // We have an index difference.
+ // Save the indexer position, and the time.
+ if (last_bottom_disc_negedge_wait_count_ + 1 !=
+ position->bottom_disc_negedge_wait_count) {
+ LOG(ERROR, "Funny, we got 2 edges since we last checked.\n");
+ }
+
+ // Save the captured position as the position at which the disc
+ // touched the indexer.
+ LOG(INFO, "Grabbed on the index now at %f\n", index_position);
+ printf("Grabbed on the index now at %f\n", index_position);
+ frisbee->has_been_indexed_ = true;
+ frisbee->index_start_position_ =
+ position->bottom_disc_negedge_wait_position;
+ }
+ }
+ if (!frisbee->has_been_indexed_) {
+ // All discs must be indexed before it is safe to stop indexing.
+ safe_to_change_state_ = false;
+ }
+ }
+
+ // Figure out where the indexer should be to move the discs down to
+ // the right position.
+ double max_disc_position = 0;
+ if (MaxDiscPosition(&max_disc_position, NULL)) {
+ printf("There is a disc down here!\n");
+ // TODO(aschuh): Figure out what to do if grabbing the next one
+ // would cause things to jam into the loader.
+ // Say we aren't ready any more. Undefined behavior will result if
+ // that isn't observed.
+ double bottom_disc_position =
+ max_disc_position + ConvertDiscAngleToIndex(M_PI);
+ wrist_loop_->R << bottom_disc_position, 0.0;
+
+ // Verify that we are close enough to the goal so that we should be
+ // fine accepting the next disc.
+ double disc_error_meters = ConvertIndexToDiscPosition(
+ wrist_loop_->X_hat(0, 0) - bottom_disc_position);
+ // We are ready for the next disc if the first one is in the first
+ // half circle of the indexer. It will take time for the disc to
+ // come into the indexer, so we will be able to move it out of the
+ // way in time.
+ // This choice also makes sure that we don't claim that we aren't
+ // ready between full speed intaking.
+ if (-ConvertDiscAngleToIndex(M_PI) < disc_error_meters &&
+ disc_error_meters < 0.04) {
+ // We are only ready if we aren't being asked to change state or
+ // are full.
+ status->ready_to_intake =
+ (safe_goal_ == goal_enum) && hopper_disc_count_ < 4;
+ } else {
+ status->ready_to_intake = false;
+ }
+ } else {
+ // No discs! We are always ready for more if we aren't being
+ // asked to change state.
+ status->ready_to_intake = (safe_goal_ == goal_enum);
+ printf("Ready to intake, zero discs. %d %d %d\n",
+ status->ready_to_intake, hopper_disc_count_, safe_goal_);
+ }
+
+ // Turn on the transfer roller if we are ready.
+ if (status->ready_to_intake && hopper_disc_count_ < 4 &&
+ safe_goal_ == Goal::INTAKE) {
+ intake_voltage = transfer_voltage = 12.0;
+ }
+ }
+ printf("INTAKE\n");
+ }
+ break;
+ case Goal::READY_SHOOTER:
+ case Goal::SHOOT:
+ // Check if we have any discs to shoot or load and handle them.
+ double min_disc_position = 0;
+ if (MinDiscPosition(&min_disc_position, NULL)) {
+ const double ready_disc_position = min_disc_position +
+ ConvertDiscPositionToIndex(kReadyToPreload - kIndexStartPosition);
+
+ const double grabbed_disc_position =
+ min_disc_position +
+ ConvertDiscPositionToIndex(kReadyToLiftPosition -
+ kIndexStartPosition + 0.03);
+
+ // Check the state of the loader FSM.
+ // If it is ready to load discs, position the disc so that it is ready
+ // to be grabbed.
+ // If it isn't ready, there is a disc in there. It needs to finish it's
+ // cycle first.
+ if (loader_state_ != LoaderState::READY) {
+ // We already have a disc in the loader.
+ // Stage the discs back a bit.
+ wrist_loop_->R << ready_disc_position, 0.0;
+ printf("Loader not ready but asked to shoot\n");
+
+ // Shoot if we are grabbed and being asked to shoot.
+ if (loader_state_ == LoaderState::GRABBED &&
+ safe_goal_ == Goal::SHOOT) {
+ loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ }
+
+ // Must wait until it has been grabbed to continue.
+ if (loader_state_ == LoaderState::GRABBING) {
+ safe_to_change_state_ = false;
+ }
+ } else {
+ // No disc up top right now.
+ wrist_loop_->R << grabbed_disc_position, 0.0;
+
+ // See if the disc has gotten pretty far up yet.
+ if (wrist_loop_->X_hat(0, 0) > ready_disc_position) {
+ // Point of no return. We are committing to grabbing it now.
+ safe_to_change_state_ = false;
+ const double robust_grabbed_disc_position =
+ (grabbed_disc_position -
+ ConvertDiscPositionToIndex(kGrabberLength));
+
+ // If close, start grabbing and/or shooting.
+ if (wrist_loop_->X_hat(0, 0) > robust_grabbed_disc_position) {
+ // Start the state machine.
+ if (safe_goal_ == Goal::SHOOT) {
+ loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ } else {
+ loader_goal_ = LoaderGoal::GRAB;
+ }
+ // This frisbee is now gone. Take it out of the queue.
+ frisbees_.pop_back();
+ }
+ }
+ }
+ } else {
+ if (loader_state_ != LoaderState::READY) {
+ // Shoot if we are grabbed and being asked to shoot.
+ if (loader_state_ == LoaderState::GRABBED &&
+ safe_goal_ == Goal::SHOOT) {
+ loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ }
+ } else {
+ // Ok, no discs in sight. Spin the hopper up by 150% of it's full
+ // range and verify that we don't see anything.
+ printf("Moving the indexer to verify that it is clear\n");
+ const double hopper_clear_verification_position =
+ ::std::min(upper_open_region_.lower_bound(),
+ lower_open_region_.lower_bound()) +
+ ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
+
+ wrist_loop_->R << hopper_clear_verification_position, 0.0;
+ if (::std::abs(wrist_loop_->X_hat(0, 0) -
+ hopper_clear_verification_position) <
+ ConvertDiscPositionToIndex(0.05)) {
+ printf("Should be empty\n");
+ // We are at the end of the range. There are no more discs here.
+ while (frisbees_.size() > 0) {
+ LOG(ERROR, "Dropping an extra disc since it can't exist\n");
+ frisbees_.pop_back();
+ --hopper_disc_count_;
+ --total_disc_count_;
+ }
+ if (hopper_disc_count_ != 0) {
+ LOG(ERROR,
+ "Emptied the hopper out but there are still discs there\n");
+ }
+ }
+ }
+ }
+
+ {
+ const double hopper_clear_verification_position =
+ ::std::min(upper_open_region_.lower_bound(),
+ lower_open_region_.lower_bound()) +
+ ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
+
+ if (wrist_loop_->X_hat(0, 0) >
+ hopper_clear_verification_position +
+ ConvertDiscPositionToIndex(0.05)) {
+ // We are at the end of the range. There are no more discs here.
+ while (frisbees_.size() > 0) {
+ LOG(ERROR, "Dropping an extra disc since it can't exist\n");
+ frisbees_.pop_back();
+ --hopper_disc_count_;
+ --total_disc_count_;
+ }
+ if (hopper_disc_count_ != 0) {
+ LOG(ERROR,
+ "Emptied the hopper out but there are still discs there\n");
+ }
+ }
+ }
+
+ printf("READY_SHOOTER or SHOOT\n");
+ break;
+ }
+
+ // The only way out of the loader is to shoot the disc. The FSM can only go
+ // forwards.
+ switch (loader_state_) {
+ case LoaderState::READY:
+ printf("Loader READY\n");
+ // Open and down, ready to accept a disc.
+ loader_up_ = false;
+ disc_clamped_ = false;
+ disc_ejected_ = false;
+ if (loader_goal_ == LoaderGoal::GRAB ||
+ loader_goal_ == LoaderGoal::SHOOT_AND_RESET) {
+ if (loader_goal_ == LoaderGoal::GRAB) {
+ printf("Told to GRAB, moving on\n");
+ } else {
+ printf("Told to SHOOT_AND_RESET, moving on\n");
+ }
+ loader_state_ = LoaderState::GRABBING;
+ loader_countdown_ = kGrabbingDelay;
+ } else {
+ break;
+ }
+ case LoaderState::GRABBING:
+ printf("Loader GRABBING %d\n", loader_countdown_);
+ // Closing the grabber.
+ loader_up_ = false;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::GRABBED;
+ }
+ case LoaderState::GRABBED:
+ printf("Loader GRABBED\n");
+ // Grabber closed.
+ loader_up_ = false;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ if (loader_goal_ == LoaderGoal::SHOOT_AND_RESET) {
+ // TODO(aschuh): Only shoot if the shooter is up to speed.
+ // Seems like that would have us shooting a bit later than we could be,
+ // but it also probably spins back up real fast.
+ loader_state_ = LoaderState::LIFTING;
+ loader_countdown_ = kLiftingDelay;
+ printf("Told to SHOOT_AND_RESET, moving on\n");
+ } else if (loader_goal_ == LoaderGoal::READY) {
+ LOG(ERROR, "Can't go to ready when we have something grabbed.\n");
+ printf("Can't go to ready when we have something grabbed.\n");
+ break;
+ } else {
+ break;
+ }
+ case LoaderState::LIFTING:
+ printf("Loader LIFTING %d\n", loader_countdown_);
+ // Lifting the disc.
+ loader_up_ = true;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::LIFTED;
+ }
+ case LoaderState::LIFTED:
+ printf("Loader LIFTED\n");
+ // Disc lifted. Time to eject it out.
+ loader_up_ = true;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ loader_state_ = LoaderState::SHOOTING;
+ loader_countdown_ = kShootingDelay;
+ case LoaderState::SHOOTING:
+ printf("Loader SHOOTING %d\n", loader_countdown_);
+ // Ejecting the disc into the shooter.
+ loader_up_ = true;
+ disc_clamped_ = false;
+ disc_ejected_ = true;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::SHOOT;
+ }
+ case LoaderState::SHOOT:
+ printf("Loader SHOOT\n");
+ // The disc has been shot.
+ loader_up_ = true;
+ disc_clamped_ = false;
+ disc_ejected_ = true;
+ loader_state_ = LoaderState::LOWERING;
+ loader_countdown_ = kLoweringDelay;
+ --hopper_disc_count_;
+ ++shot_disc_count_;
+ case LoaderState::LOWERING:
+ printf("Loader LOWERING %d\n", loader_countdown_);
+ // Lowering the loader back down.
+ loader_up_ = false;
+ disc_clamped_ = false;
+ disc_ejected_ = true;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::LOWERED;
+ }
+ case LoaderState::LOWERED:
+ printf("Loader LOWERED\n");
+ // The indexer is lowered.
+ loader_up_ = false;
+ disc_clamped_ = false;
+ disc_ejected_ = false;
+ loader_state_ = LoaderState::READY;
+ // Once we have shot, we need to hang out in READY until otherwise
+ // notified.
+ loader_goal_ = LoaderGoal::READY;
+ break;
+ }
+
+ // Update the observer.
+ wrist_loop_->Update(position != NULL, output == NULL);
+
+ if (position) {
+ LOG(DEBUG, "pos=%f\n", position->index_position);
+ last_bottom_disc_detect_ = position->bottom_disc_detect;
+ last_top_disc_detect_ = position->top_disc_detect;
+ last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
+ last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
+ last_bottom_disc_negedge_wait_count_ =
+ position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ }
+
+ status->hopper_disc_count = hopper_disc_count_;
+ status->total_disc_count = total_disc_count_;
+ status->shot_disc_count = shot_disc_count_;
+ status->preloaded = (loader_state_ != LoaderState::READY);
+
+ if (output) {
+ output->intake_voltage = intake_voltage;
+ output->transfer_voltage = transfer_voltage;
+ output->index_voltage = wrist_loop_->U(0, 0);
+ output->loader_up = loader_up_;
+ output->disc_clamped = disc_clamped_;
+ output->disc_ejected = disc_ejected_;
+ }
+
+ if (safe_to_change_state_) {
+ safe_goal_ = goal_enum;
+ }
+ if (hopper_disc_count_ < 0) {
+ LOG(ERROR, "NEGATIVE DISCS. VERY VERY BAD\n");
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/index.gyp b/frc971/control_loops/index/index.gyp
new file mode 100644
index 0000000..3e92dcc
--- /dev/null
+++ b/frc971/control_loops/index/index.gyp
@@ -0,0 +1,71 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'index_loop',
+ 'type': 'static_library',
+ 'sources': ['index_motor.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/index',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'index_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'index.cc',
+ 'index_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'index_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'index_loop',
+ ],
+ },
+ {
+ 'target_name': 'index_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'index_lib_test.cc',
+ 'transfer_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'index_loop',
+ 'index_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'index',
+ 'type': 'executable',
+ 'sources': [
+ 'index_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'index_lib',
+ 'index_loop',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
new file mode 100644
index 0000000..c9dc86f
--- /dev/null
+++ b/frc971/control_loops/index/index.h
@@ -0,0 +1,342 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_H_
+#define FRC971_CONTROL_LOOPS_WRIST_H_
+
+#include <deque>
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/index/index_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class IndexTest_InvalidStateTest_Test;
+class IndexTest_LostDisc_Test;
+}
+
+// This class represents a region of space.
+class Region {
+ public:
+ Region () : upper_bound_(0.0), lower_bound_(0.0) {}
+
+ // Restarts the region tracking by starting over with a 0 width region with
+ // the bounds at [edge, edge].
+ void Restart(double edge) {
+ upper_bound_ = edge;
+ lower_bound_ = edge;
+ }
+
+ // Expands the region to include the new point.
+ void Expand(double new_point) {
+ if (new_point > upper_bound_) {
+ upper_bound_ = new_point;
+ } else if (new_point < lower_bound_) {
+ lower_bound_ = new_point;
+ }
+ }
+
+ // Returns the width of the region.
+ double width() const { return upper_bound_ - lower_bound_; }
+ // Returns the upper and lower bounds.
+ double upper_bound() const { return upper_bound_; }
+ double lower_bound() const { return lower_bound_; }
+
+ private:
+ // Upper bound of the region.
+ double upper_bound_;
+ // Lower bound of the region.
+ double lower_bound_;
+};
+
+class IndexMotor
+ : public aos::control_loops::ControlLoop<control_loops::IndexLoop> {
+ public:
+ explicit IndexMotor(
+ control_loops::IndexLoop *my_index = &control_loops::index_loop);
+
+ static const double kTransferStartPosition;
+ static const double kIndexStartPosition;
+ // The distance from where the disc first grabs on the indexer to where it
+ // just bairly clears the loader.
+ static const double kIndexFreeLength;
+ // The distance to where the disc just starts to enter the loader.
+ static const double kLoaderFreeStopPosition;
+ // The distance to where the next disc gets positioned while the current disc
+ // is shooting.
+ static const double kReadyToPreload;
+
+ // Distance that the grabber pulls the disc in by.
+ static const double kGrabberLength;
+ // Distance to where the grabber takes over.
+ static const double kGrabberStartPosition;
+
+ // The distance to where the disc hits the back of the loader and is ready to
+ // lift.
+ static const double kReadyToLiftPosition;
+
+ static const double kGrabberMovementVelocity;
+ // TODO(aschuh): This depends on the shooter angle...
+ // Distance to where the shooter is up and ready to shoot.
+ static const double kLifterStopPosition;
+ static const double kLifterMovementVelocity;
+
+ // Distance to where the disc has been launched.
+ // TODO(aschuh): This depends on the shooter angle...
+ static const double kEjectorStopPosition;
+ static const double kEjectorMovementVelocity;
+
+ // Start and stop position of the bottom disc detect sensor in meters.
+ static const double kBottomDiscDetectStart;
+ static const double kBottomDiscDetectStop;
+ // Delay between the negedge of the disc detect and when it engages on the
+ // indexer.
+ static const double kBottomDiscIndexDelay;
+
+ static const double kTopDiscDetectStart;
+ static const double kTopDiscDetectStop;
+
+ // Minimum distance between 2 frisbees as seen by the top disc detect sensor.
+ static const double kTopDiscDetectMinSeperation;
+
+ // Converts the angle of the indexer to the angle of the disc.
+ static double ConvertIndexToDiscAngle(const double angle);
+ // Converts the angle of the indexer to the position that the center of the
+ // disc has traveled.
+ static double ConvertIndexToDiscPosition(const double angle);
+
+ // Converts the angle of the transfer roller to the position that the center
+ // of the disc has traveled.
+ static double ConvertTransferToDiscPosition(const double angle);
+
+ // Converts the distance around the indexer to the position of
+ // the index roller.
+ static double ConvertDiscPositionToIndex(const double position);
+ // Converts the angle around the indexer to the position of the index roller.
+ static double ConvertDiscAngleToIndex(const double angle);
+ // Converts the angle around the indexer to the position of the disc in the
+ // indexer.
+ static double ConvertDiscAngleToDiscPosition(const double angle);
+ // Converts the distance around the indexer to the angle of the disc around
+ // the indexer.
+ static double ConvertDiscPositionToDiscAngle(const double position);
+
+ // Disc radius in meters.
+ static const double kDiscRadius;
+ // Roller radius in meters.
+ static const double kRollerRadius;
+ // Transfer roller radius in meters.
+ static const double kTransferRollerRadius;
+
+ // Time that it takes to grab the disc in cycles.
+ static const int kGrabbingDelay;
+ // Time that it takes to lift the loader in cycles.
+ static const int kLiftingDelay;
+ // Time that it takes to shoot the disc in cycles.
+ static const int kShootingDelay;
+ // Time that it takes to lower the loader in cycles.
+ static const int kLoweringDelay;
+
+ // Object representing a Frisbee tracked by the indexer.
+ class Frisbee {
+ public:
+ Frisbee()
+ : bottom_posedge_time_(0, 0),
+ bottom_negedge_time_(0, 0) {
+ Reset();
+ }
+
+ // Resets a Frisbee so it can be reused.
+ void Reset() {
+ bottom_posedge_time_ = ::aos::time::Time(0, 0);
+ bottom_negedge_time_ = ::aos::time::Time(0, 0);
+ has_been_indexed_ = false;
+ index_start_position_ = 0.0;
+ }
+
+ // Returns true if the position is valid.
+ bool has_position() const {
+ return has_been_indexed_;
+ }
+
+ // Returns the most up to date and accurate position that we have for the
+ // disc. This is the indexer position that the disc grabbed at.
+ double position() const {
+ return index_start_position_;
+ }
+
+ // Returns the absolute position of the disc in meters in the hopper given
+ // that the indexer is at the provided position.
+ double absolute_position(const double index_position) const {
+ return IndexMotor::ConvertIndexToDiscPosition(
+ index_position - index_start_position_) +
+ IndexMotor::kIndexStartPosition;
+ }
+
+ // Shifts the disc down the indexer by the provided offset. This is to
+ // handle when the cRIO reboots.
+ void OffsetDisc(double offset) {
+ index_start_position_ += offset;
+ }
+
+ // Potentially offsets the position with the knowledge that no discs are
+ // currently blocking the top sensor. This knowledge can be used to move
+ // this disc if it is believed to be blocking the top sensor.
+ // Returns the amount that the disc moved due to this observation.
+ double ObserveNoTopDiscSensor(double index_position, double index_velocity);
+
+ // Posedge and negedge disc times.
+ ::aos::time::Time bottom_posedge_time_;
+ ::aos::time::Time bottom_negedge_time_;
+
+ // True if the disc has a valid index position.
+ bool has_been_indexed_;
+ // Location of the index when the disc first contacted it.
+ double index_start_position_;
+ };
+
+ // Returns where the indexer thinks the frisbees are.
+ const ::std::deque<Frisbee> &frisbees() const { return frisbees_; }
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::IndexLoop::Goal *goal,
+ const control_loops::IndexLoop::Position *position,
+ control_loops::IndexLoop::Output *output,
+ control_loops::IndexLoop::Status *status);
+
+ private:
+ friend class testing::IndexTest_InvalidStateTest_Test;
+ friend class testing::IndexTest_LostDisc_Test;
+
+ // This class implements the CapU function correctly given all the extra
+ // information that we know about from the wrist motor.
+ class IndexStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
+ public:
+ IndexStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop)
+ : StateFeedbackLoop<2, 1, 1>(loop),
+ low_voltage_count_(0) {
+ }
+
+ // Voltage below which the indexer won't move with a disc in it.
+ static const double kMinMotionVoltage;
+ // Maximum number of cycles to apply a low voltage to the motor.
+ static const double kNoMotionCuttoffCount;
+
+ // Caps U, but disables the motor after a number of cycles of inactivity.
+ virtual void CapU();
+ private:
+ // Number of cycles that we have seen a small voltage being applied.
+ uint32_t low_voltage_count_;
+ };
+
+ // Sets disc_position to the minimum or maximum disc position.
+ // Sets found_disc to point to the frisbee that was found, and ignores it if
+ // found_disc is NULL.
+ // Returns true if there were discs, and false if there weren't.
+ // On false, disc_position is left unmodified.
+ bool MinDiscPosition(double *disc_position, Frisbee **found_disc);
+ bool MaxDiscPosition(double *disc_position, Frisbee **found_disc);
+
+ // The state feedback control loop to talk to for the index.
+ ::std::unique_ptr<IndexStateFeedbackLoop> wrist_loop_;
+
+ // Count of the number of discs that we have collected.
+ int32_t hopper_disc_count_;
+ int32_t total_disc_count_;
+ int32_t shot_disc_count_;
+
+ enum class Goal {
+ // Hold position, in a low power state.
+ HOLD = 0,
+ // Get ready to load discs by shifting the discs down.
+ READY_LOWER = 1,
+ // Ready the discs, spin up the transfer roller, and accept discs.
+ INTAKE = 2,
+ // Get ready to shoot, and place a disc in the loader.
+ READY_SHOOTER = 3,
+ // Shoot at will.
+ SHOOT = 4
+ };
+
+ // These two enums command and track the loader loading discs into the
+ // shooter.
+ enum class LoaderState {
+ // Open and down, ready to accept a disc.
+ READY,
+ // Closing the grabber.
+ GRABBING,
+ // Grabber closed.
+ GRABBED,
+ // Lifting the disc.
+ LIFTING,
+ // Disc lifted.
+ LIFTED,
+ // Ejecting the disc into the shooter.
+ SHOOTING,
+ // The disc has been shot.
+ SHOOT,
+ // Lowering the loader back down.
+ LOWERING,
+ // The indexer is lowered.
+ LOWERED
+ };
+
+ // TODO(aschuh): If we are grabbed and asked to be ready, now what?
+ // LOG ?
+ enum class LoaderGoal {
+ // Get the loader ready to accept another disc.
+ READY,
+ // Grab a disc now.
+ GRAB,
+ // Lift it up, shoot, and reset.
+ // Waits to shoot until the shooter is stable.
+ // Resets the goal to READY once one disc has been shot.
+ SHOOT_AND_RESET
+ };
+
+ // The current goal
+ Goal safe_goal_;
+
+ // Loader goal, state, and counter.
+ LoaderGoal loader_goal_;
+ LoaderState loader_state_;
+ int loader_countdown_;
+
+ // Current state of the pistons.
+ bool loader_up_;
+ bool disc_clamped_;
+ bool disc_ejected_;
+
+ // The frisbee that is flying through the transfer rollers.
+ Frisbee transfer_frisbee_;
+
+ // Bottom disc detect from the last valid packet for detecting edges.
+ bool last_bottom_disc_detect_;
+ bool last_top_disc_detect_;
+ int32_t last_bottom_disc_posedge_count_;
+ int32_t last_bottom_disc_negedge_count_;
+ int32_t last_bottom_disc_negedge_wait_count_;
+ int32_t last_top_disc_posedge_count_;
+ int32_t last_top_disc_negedge_count_;
+
+ // Frisbees are in order such that the newest frisbee is on the front.
+ ::std::deque<Frisbee> frisbees_;
+
+ // True if we haven't seen a position before.
+ bool no_prior_position_;
+ // Number of position messages that we have missed in a row.
+ uint32_t missing_position_count_;
+
+ // The no-disc regions for both the bottom and top beam break sensors.
+ Region upper_open_region_;
+ Region lower_open_region_;
+
+ DISALLOW_COPY_AND_ASSIGN(IndexMotor);
+};
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_H_
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
new file mode 100644
index 0000000..9b9c939
--- /dev/null
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -0,0 +1,1299 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/index/index.h"
+#include "frc971/control_loops/index/index_motor_plant.h"
+#include "frc971/control_loops/index/transfer_motor_plant.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+class Frisbee {
+ public:
+ // Creates a frisbee starting at the specified position in the frisbee path,
+ // and with the transfer and index rollers at the specified positions.
+ Frisbee(double transfer_roller_position,
+ double index_roller_position,
+ double position = IndexMotor::kBottomDiscDetectStart - 0.001)
+ : transfer_roller_position_(transfer_roller_position),
+ index_roller_position_(index_roller_position),
+ position_(position),
+ has_been_shot_(false),
+ has_bottom_disc_negedge_wait_position_(false),
+ bottom_disc_negedge_wait_position_(0.0),
+ after_negedge_time_left_(IndexMotor::kBottomDiscIndexDelay),
+ counted_negedge_wait_(false),
+ has_top_disc_posedge_position_(false),
+ top_disc_posedge_position_(0.0),
+ has_top_disc_negedge_position_(false),
+ top_disc_negedge_position_(0.0) {
+ }
+
+ // Returns true if the frisbee is controlled by the transfer roller.
+ bool IsTouchingTransfer(double position) const {
+ return (position >= IndexMotor::kBottomDiscDetectStart &&
+ position <= IndexMotor::kIndexStartPosition);
+ }
+ bool IsTouchingTransfer() const { return IsTouchingTransfer(position_); }
+
+ // Returns true if the frisbee is in a place where it is unsafe to grab.
+ bool IsUnsafeToGrab() const {
+ return (position_ > (IndexMotor::kLoaderFreeStopPosition) &&
+ position_ < IndexMotor::kGrabberStartPosition);
+ }
+
+ // Returns true if the frisbee is controlled by the indexing roller.
+ bool IsTouchingIndex(double position) const {
+ return (position >= IndexMotor::kIndexStartPosition &&
+ position < IndexMotor::kGrabberStartPosition);
+ }
+ bool IsTouchingIndex() const { return IsTouchingIndex(position_); }
+
+ // Returns true if the frisbee is in a position such that the disc can be
+ // lifted.
+ bool IsUnsafeToLift() const {
+ return (position_ >= IndexMotor::kLoaderFreeStopPosition &&
+ position_ <= IndexMotor::kReadyToLiftPosition);
+ }
+
+ // Returns true if the frisbee is in a position such that the grabber will
+ // pull it into the loader.
+ bool IsTouchingGrabber() const {
+ return (position_ >= IndexMotor::kGrabberStartPosition &&
+ position_ < IndexMotor::kReadyToLiftPosition);
+ }
+
+ // Returns true if the frisbee is in a position such that the disc can be
+ // lifted.
+ bool IsTouchingLoader() const {
+ return (position_ >= IndexMotor::kReadyToLiftPosition &&
+ position_ < IndexMotor::kLifterStopPosition);
+ }
+
+ // Returns true if the frisbee is touching the ejector.
+ bool IsTouchingEjector() const {
+ return (position_ >= IndexMotor::kLifterStopPosition &&
+ position_ < IndexMotor::kEjectorStopPosition);
+ }
+
+ // Returns true if the disc is triggering the bottom disc detect sensor.
+ bool bottom_disc_detect(double position) const {
+ return (position >= IndexMotor::kBottomDiscDetectStart &&
+ position <= IndexMotor::kBottomDiscDetectStop);
+ }
+ bool bottom_disc_detect() const { return bottom_disc_detect(position_); }
+
+ // Returns true if the disc is triggering the top disc detect sensor.
+ bool top_disc_detect(double position) const {
+ return (position >= IndexMotor::kTopDiscDetectStart &&
+ position <= IndexMotor::kTopDiscDetectStop);
+ }
+ bool top_disc_detect() const { return top_disc_detect(position_); }
+
+ // Returns true if the bottom disc sensor will negedge after the disc moves
+ // by dx.
+ bool will_negedge_bottom_disc_detect(double disc_dx) {
+ if (bottom_disc_detect()) {
+ return !bottom_disc_detect(position_ + disc_dx);
+ }
+ return false;
+ }
+
+ // Returns true if the bottom disc sensor will posedge after the disc moves
+ // by dx.
+ bool will_posedge_top_disc_detect(double disc_dx) {
+ if (!top_disc_detect()) {
+ return top_disc_detect(position_ + disc_dx);
+ }
+ return false;
+ }
+
+ // Returns true if the bottom disc sensor will negedge after the disc moves
+ // by dx.
+ bool will_negedge_top_disc_detect(double disc_dx) {
+ if (top_disc_detect()) {
+ return !top_disc_detect(position_ + disc_dx);
+ }
+ return false;
+ }
+
+ // Handles potentially dealing with the delayed negedge.
+ // Computes the index position when time expires using the cached old indexer
+ // position, the elapsed time, and the average velocity.
+ void HandleAfterNegedge(
+ double index_velocity, double elapsed_time, double time_left) {
+ if (!has_bottom_disc_negedge_wait_position_) {
+ if (after_negedge_time_left_ < time_left) {
+ // Assume constant velocity and compute the position.
+ bottom_disc_negedge_wait_position_ =
+ index_roller_position_ +
+ index_velocity * (elapsed_time + after_negedge_time_left_);
+ after_negedge_time_left_ = 0.0;
+ has_bottom_disc_negedge_wait_position_ = true;
+ } else {
+ after_negedge_time_left_ -= time_left;
+ }
+ }
+ }
+
+ // Updates the position of the disc assuming that it has started on the
+ // transfer. The elapsed time is the simulated amount of time that has
+ // elapsed since the simulation timestep started and this method was called.
+ // time_left is the amount of time left to spend during this timestep.
+ double UpdateTransferPositionForTime(double transfer_roller_velocity,
+ double index_roller_velocity,
+ double elapsed_time,
+ double time_left) {
+ double disc_dx = IndexMotor::ConvertTransferToDiscPosition(
+ transfer_roller_velocity * time_left);
+ bool shrunk_time = false;
+ if (!IsTouchingTransfer(position_ + disc_dx)) {
+ shrunk_time = true;
+ time_left = (IndexMotor::kIndexStartPosition - position_) /
+ transfer_roller_velocity;
+ disc_dx = IndexMotor::ConvertTransferToDiscPosition(
+ transfer_roller_velocity * time_left);
+ }
+
+ if (will_negedge_bottom_disc_detect(disc_dx)) {
+ // Compute the time from the negedge to the end of the cycle assuming
+ // constant velocity.
+ const double elapsed_time =
+ (position_ + disc_dx - IndexMotor::kBottomDiscDetectStop) /
+ disc_dx * time_left;
+
+ // I am not implementing very short delays until this fails.
+ assert(elapsed_time <= after_negedge_time_left_);
+ after_negedge_time_left_ -= elapsed_time;
+ } else if (position_ >= IndexMotor::kBottomDiscDetectStop) {
+ HandleAfterNegedge(index_roller_velocity, elapsed_time, time_left);
+ }
+
+ if (shrunk_time) {
+ EXPECT_LT(0, transfer_roller_velocity);
+ position_ = IndexMotor::kIndexStartPosition;
+ } else {
+ position_ += disc_dx;
+ }
+ printf("Transfer Roller: Disc is at %f\n", position_);
+ return time_left;
+ }
+
+ // Updates the position of the disc assuming that it has started on the
+ // indexer. The elapsed time is the simulated amount of time that has
+ // elapsed since the simulation timestep started and this method was called.
+ // time_left is the amount of time left to spend during this timestep.
+ double UpdateIndexPositionForTime(double index_roller_velocity,
+ double elapsed_time,
+ double time_left) {
+ double index_dx = IndexMotor::ConvertIndexToDiscPosition(
+ index_roller_velocity * time_left);
+ bool shrunk_time = false;
+ if (!IsTouchingIndex(position_ + index_dx)) {
+ shrunk_time = true;
+ time_left = (IndexMotor::kGrabberStartPosition - position_) /
+ index_roller_velocity;
+ index_dx = IndexMotor::ConvertTransferToDiscPosition(
+ index_roller_velocity * time_left);
+ }
+
+ if (position_ >= IndexMotor::kBottomDiscDetectStop) {
+ HandleAfterNegedge(index_roller_velocity, elapsed_time, time_left);
+ }
+
+ if (will_posedge_top_disc_detect(index_dx)) {
+ // Wohoo! Find the edge.
+ // Assume constant velocity and compute the position.
+ double edge_position = index_roller_velocity > 0 ?
+ IndexMotor::kTopDiscDetectStart : IndexMotor::kTopDiscDetectStop;
+ const double disc_time =
+ (edge_position - position_) / index_roller_velocity;
+ top_disc_posedge_position_ = index_roller_position_ +
+ IndexMotor::ConvertDiscPositionToIndex(
+ index_roller_velocity * (elapsed_time + disc_time));
+ has_top_disc_posedge_position_ = true;
+ printf("Posedge on top sensor at %f\n", top_disc_posedge_position_);
+ }
+
+ if (will_negedge_top_disc_detect(index_dx)) {
+ // Wohoo! Find the edge.
+ // Assume constant velocity and compute the position.
+ double edge_position = index_roller_velocity > 0 ?
+ IndexMotor::kTopDiscDetectStop : IndexMotor::kTopDiscDetectStart;
+ const double disc_time =
+ (edge_position - position_) / index_roller_velocity;
+ top_disc_negedge_position_ = index_roller_position_ +
+ IndexMotor::ConvertDiscPositionToIndex(
+ index_roller_velocity * (elapsed_time + disc_time));
+ has_top_disc_negedge_position_ = true;
+ printf("Negedge on top sensor at %f\n", top_disc_negedge_position_);
+ }
+
+ if (shrunk_time) {
+ if (index_roller_velocity > 0) {
+ position_ = IndexMotor::kGrabberStartPosition;
+ } else {
+ position_ = IndexMotor::kIndexStartPosition;
+ }
+ } else {
+ position_ += index_dx;
+ }
+ printf("Index: Disc is at %f\n", position_);
+ return time_left;
+ }
+
+ // Updates the position given velocities, piston comands, and the time left in
+ // the simulation cycle.
+ void UpdatePositionForTime(double transfer_roller_velocity,
+ double index_roller_velocity,
+ bool clamped,
+ bool lifted,
+ bool ejected,
+ double time_left) {
+ double elapsed_time = 0.0;
+ // We are making this assumption below
+ ASSERT_LE(IndexMotor::kBottomDiscDetectStop,
+ IndexMotor::kIndexStartPosition);
+ if (IsTouchingTransfer() || position() < 0.0) {
+ double deltat = UpdateTransferPositionForTime(
+ transfer_roller_velocity, index_roller_velocity,
+ elapsed_time, time_left);
+ time_left -= deltat;
+ elapsed_time += deltat;
+ }
+
+ if (IsTouchingIndex() && time_left >= 0) {
+ // Verify that we aren't trying to grab or lift when it isn't safe.
+ EXPECT_FALSE(clamped && IsUnsafeToGrab());
+ EXPECT_FALSE(lifted && IsUnsafeToLift());
+
+ double deltat = UpdateIndexPositionForTime(
+ index_roller_velocity, elapsed_time, time_left);
+ time_left -= deltat;
+ elapsed_time += deltat;
+ }
+ if (IsTouchingGrabber()) {
+ if (clamped) {
+ const double grabber_dx =
+ IndexMotor::kGrabberMovementVelocity * time_left;
+ position_ = ::std::min(position_ + grabber_dx,
+ IndexMotor::kReadyToLiftPosition);
+ }
+ EXPECT_FALSE(lifted) << "Can't lift while in grabber";
+ EXPECT_FALSE(ejected) << "Can't eject while in grabber";
+ printf("Grabber: Disc is at %f\n", position_);
+ } else if (IsTouchingLoader()) {
+ if (lifted) {
+ const double lifter_dx =
+ IndexMotor::kLifterMovementVelocity * time_left;
+ position_ = ::std::min(position_ + lifter_dx,
+ IndexMotor::kLifterStopPosition);
+ }
+ EXPECT_TRUE(clamped);
+ EXPECT_FALSE(ejected);
+ printf("Loader: Disc is at %f\n", position_);
+ } else if (IsTouchingEjector()) {
+ EXPECT_TRUE(lifted);
+ if (ejected) {
+ const double ejector_dx =
+ IndexMotor::kEjectorMovementVelocity * time_left;
+ position_ = ::std::min(position_ + ejector_dx,
+ IndexMotor::kEjectorStopPosition);
+ EXPECT_FALSE(clamped);
+ }
+ printf("Ejector: Disc is at %f\n", position_);
+ } else if (position_ == IndexMotor::kEjectorStopPosition) {
+ printf("Shot: Disc is at %f\n", position_);
+ has_been_shot_ = true;
+ }
+ }
+
+ // Updates the position of the frisbee in the frisbee path.
+ void UpdatePosition(double transfer_roller_position,
+ double index_roller_position,
+ bool clamped,
+ bool lifted,
+ bool ejected) {
+ const double transfer_roller_velocity =
+ (transfer_roller_position - transfer_roller_position_) / 0.01;
+ const double index_roller_velocity =
+ (index_roller_position - index_roller_position_) / 0.01;
+ UpdatePositionForTime(transfer_roller_velocity,
+ index_roller_velocity,
+ clamped,
+ lifted,
+ ejected,
+ 0.01);
+ transfer_roller_position_ = transfer_roller_position;
+ index_roller_position_ = index_roller_position;
+ }
+
+ // Returns if the disc has been shot and can be removed from the robot.
+ bool has_been_shot() const {
+ return has_been_shot_;
+ }
+
+ // Returns the position of the disc in the system.
+ double position() const {
+ return position_;
+ }
+
+ // Sets whether or not we have counted the delayed negedge.
+ void set_counted_negedge_wait(bool counted_negedge_wait) {
+ counted_negedge_wait_ = counted_negedge_wait;
+ }
+
+ // Returns if we have counted the delayed negedge.
+ bool counted_negedge_wait() { return counted_negedge_wait_; }
+
+ // Returns true if the negedge wait position is valid.
+ bool has_bottom_disc_negedge_wait_position() {
+ return has_bottom_disc_negedge_wait_position_;
+ }
+
+ // Returns the negedge wait position.
+ double bottom_disc_negedge_wait_position() {
+ return bottom_disc_negedge_wait_position_;
+ }
+
+ // Returns the last position where a posedge was seen.
+ double top_disc_posedge_position() { return top_disc_posedge_position_; }
+
+ // Returns the last position where a negedge was seen.
+ double top_disc_negedge_position() { return top_disc_negedge_position_; }
+
+ // True if the top disc has seen a posedge.
+ // Reading this flag clears it.
+ bool has_top_disc_posedge_position() {
+ bool prev = has_top_disc_posedge_position_;
+ has_top_disc_posedge_position_ = false;
+ return prev;
+ }
+
+ // True if the top disc has seen a negedge.
+ // Reading this flag clears it.
+ bool has_top_disc_negedge_position() {
+ bool prev = has_top_disc_negedge_position_;
+ has_top_disc_negedge_position_ = false;
+ return prev;
+ }
+
+ // Simulates the index roller moving without the disc moving.
+ void OffsetIndex(double offset) {
+ index_roller_position_ += offset;
+ }
+
+ private:
+ // Previous transfer roller position for computing deltas.
+ double transfer_roller_position_;
+ // Previous index roller position for computing deltas.
+ double index_roller_position_;
+ // Position in the robot.
+ double position_;
+ // True if the disc has been shot.
+ bool has_been_shot_;
+ // True if the delay after the negedge of the beam break has occured.
+ bool has_bottom_disc_negedge_wait_position_;
+ // Posiiton of the indexer when the delayed negedge occures.
+ double bottom_disc_negedge_wait_position_;
+ // Time left after the negedge before we need to sample the indexer position.
+ double after_negedge_time_left_;
+ // Bool for the user to record if they have counted the negedge from this
+ // disc.
+ bool counted_negedge_wait_;
+ // True if the top disc sensor posedge has occured and
+ // hasn't been counted yet.
+ bool has_top_disc_posedge_position_;
+ // The position at which the posedge occured.
+ double top_disc_posedge_position_;
+ // True if the top disc sensor negedge has occured and
+ // hasn't been counted yet.
+ bool has_top_disc_negedge_position_;
+ // The position at which the negedge occured.
+ double top_disc_negedge_position_;
+};
+
+
+// Class which simulates the index and sends out queue messages containing the
+// position.
+class IndexMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // index, which will be treated as 0 by the encoder.
+ IndexMotorSimulation()
+ : index_plant_(new StateFeedbackPlant<2, 1, 1>(MakeIndexPlant())),
+ transfer_plant_(new StateFeedbackPlant<2, 1, 1>(MakeTransferPlant())),
+ bottom_disc_posedge_count_(0),
+ bottom_disc_negedge_count_(0),
+ bottom_disc_negedge_wait_count_(0),
+ bottom_disc_negedge_wait_position_(0),
+ top_disc_posedge_count_(0),
+ top_disc_posedge_position_(0.0),
+ top_disc_negedge_count_(0),
+ top_disc_negedge_position_(0.0),
+ my_index_loop_(".frc971.control_loops.index",
+ 0x1a7b7094, ".frc971.control_loops.index.goal",
+ ".frc971.control_loops.index.position",
+ ".frc971.control_loops.index.output",
+ ".frc971.control_loops.index.status") {
+ }
+
+ // Starts a disc offset from the start of the index.
+ void InsertDisc(double offset = IndexMotor::kBottomDiscDetectStart - 0.001) {
+ Frisbee new_frisbee(transfer_roller_position(),
+ index_roller_position(),
+ offset);
+ ASSERT_FALSE(new_frisbee.bottom_disc_detect());
+ ASSERT_FALSE(new_frisbee.top_disc_detect());
+ frisbees.push_back(new_frisbee);
+ }
+
+ // Returns true if the bottom disc sensor is triggered.
+ bool BottomDiscDetect() const {
+ bool bottom_disc_detect = false;
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ bottom_disc_detect |= frisbee->bottom_disc_detect();
+ }
+ return bottom_disc_detect;
+ }
+
+ // Returns true if the top disc sensor is triggered.
+ bool TopDiscDetect() const {
+ bool top_disc_detect = false;
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ top_disc_detect |= frisbee->top_disc_detect();
+ }
+ return top_disc_detect;
+ }
+
+ // Updates all discs, and verifies that the state of the system is sane.
+ void UpdateDiscs(bool clamped, bool lifted, bool ejected) {
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ const bool old_bottom_disc_detect = frisbee->bottom_disc_detect();
+ frisbee->UpdatePosition(transfer_roller_position(),
+ index_roller_position(),
+ clamped,
+ lifted,
+ ejected);
+
+ // Look for disc detect edges and report them.
+ const bool bottom_disc_detect = frisbee->bottom_disc_detect();
+ if (old_bottom_disc_detect && !bottom_disc_detect) {
+ printf("Negedge of disc\n");
+ ++bottom_disc_negedge_count_;
+ }
+
+ if (!old_bottom_disc_detect && frisbee->bottom_disc_detect()) {
+ printf("Posedge of disc\n");
+ ++bottom_disc_posedge_count_;
+ }
+
+ // See if the frisbee has a delayed negedge and encoder value to report
+ // back.
+ if (frisbee->has_bottom_disc_negedge_wait_position()) {
+ if (!frisbee->counted_negedge_wait()) {
+ bottom_disc_negedge_wait_position_ =
+ frisbee->bottom_disc_negedge_wait_position();
+ ++bottom_disc_negedge_wait_count_;
+ frisbee->set_counted_negedge_wait(true);
+ }
+ }
+ if (frisbee->has_top_disc_posedge_position()) {
+ ++top_disc_posedge_count_;
+ top_disc_posedge_position_ = frisbee->top_disc_posedge_position();
+ }
+ if (frisbee->has_top_disc_negedge_position()) {
+ ++top_disc_negedge_count_;
+ top_disc_negedge_position_ = frisbee->top_disc_negedge_position();
+ }
+ }
+
+ // Make sure nobody is too close to anybody else.
+ Frisbee *last_frisbee = NULL;
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ if (last_frisbee) {
+ const double distance = frisbee->position() - last_frisbee->position();
+ double min_distance;
+ if (frisbee->IsTouchingTransfer() ||
+ last_frisbee->IsTouchingTransfer()) {
+ min_distance = 0.3;
+ } else {
+ min_distance =
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI * 2.0 / 3.0);
+ }
+
+ EXPECT_LT(min_distance, ::std::abs(distance)) << "Discs too close";
+ }
+ last_frisbee = &*frisbee;
+ }
+
+ // Remove any shot frisbees.
+ for (int i = 0; i < static_cast<int>(frisbees.size()); ++i) {
+ if (frisbees[i].has_been_shot()) {
+ shot_frisbees.push_back(frisbees[i]);
+ frisbees.erase(frisbees.begin() + i);
+ --i;
+ }
+ }
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::IndexLoop::Position> position =
+ my_index_loop_.position.MakeMessage();
+ position->index_position = index_roller_position();
+ position->bottom_disc_detect = BottomDiscDetect();
+ position->top_disc_detect = TopDiscDetect();
+ position->bottom_disc_posedge_count = bottom_disc_posedge_count_;
+ position->bottom_disc_negedge_count = bottom_disc_negedge_count_;
+ position->bottom_disc_negedge_wait_count = bottom_disc_negedge_wait_count_;
+ position->bottom_disc_negedge_wait_position =
+ bottom_disc_negedge_wait_position_;
+ position->top_disc_posedge_count = top_disc_posedge_count_;
+ position->top_disc_posedge_position = top_disc_posedge_position_;
+ position->top_disc_negedge_count = top_disc_negedge_count_;
+ position->top_disc_negedge_position = top_disc_negedge_position_;
+ printf("bdd: %x tdd: %x posedge %d negedge %d "
+ "delaycount %d delaypos %f topcount %d toppos %f "
+ "topcount %d toppos %f\n",
+ position->bottom_disc_detect,
+ position->top_disc_detect,
+ position->bottom_disc_posedge_count,
+ position->bottom_disc_negedge_count,
+ position->bottom_disc_negedge_wait_count,
+ position->bottom_disc_negedge_wait_position,
+ position->top_disc_posedge_count,
+ position->top_disc_posedge_position,
+ position->top_disc_negedge_count,
+ position->top_disc_negedge_position);
+ position.Send();
+ }
+
+ // Simulates the index moving for one timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+
+ index_plant_->U << my_index_loop_.output->index_voltage;
+ index_plant_->Update();
+
+ transfer_plant_->U << my_index_loop_.output->transfer_voltage;
+ transfer_plant_->Update();
+ printf("tv: %f iv: %f tp : %f ip: %f\n",
+ my_index_loop_.output->transfer_voltage,
+ my_index_loop_.output->index_voltage,
+ transfer_roller_position(), index_roller_position());
+
+ UpdateDiscs(my_index_loop_.output->disc_clamped,
+ my_index_loop_.output->loader_up,
+ my_index_loop_.output->disc_ejected);
+ }
+
+ // Simulates the index roller moving without the disc moving.
+ void OffsetIndices(double offset) {
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ frisbee->OffsetIndex(offset);
+ }
+ }
+
+ // Plants for the index and transfer rollers.
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> index_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> transfer_plant_;
+
+ // Posedge and negedge counts for the beam break.
+ int32_t bottom_disc_posedge_count_;
+ int32_t bottom_disc_negedge_count_;
+
+ // Delayed negedge count and corrisponding position.
+ int32_t bottom_disc_negedge_wait_count_;
+ int32_t bottom_disc_negedge_wait_position_;
+
+ // Posedge count and position for the upper disc sensor.
+ int32_t top_disc_posedge_count_;
+ double top_disc_posedge_position_;
+
+ // Negedge count and position for the upper disc sensor.
+ int32_t top_disc_negedge_count_;
+ double top_disc_negedge_position_;
+
+ // Returns the absolute angle of the index.
+ double index_roller_position() const {
+ return index_plant_->Y(0, 0);
+ }
+
+ // Returns the absolute angle of the index.
+ double transfer_roller_position() const {
+ return transfer_plant_->Y(0, 0);
+ }
+
+ // Frisbees being tracked in the robot.
+ ::std::vector<Frisbee> frisbees;
+ // Frisbees that have been shot.
+ ::std::vector<Frisbee> shot_frisbees;
+
+ private:
+ // Control loop for the indexer.
+ IndexLoop my_index_loop_;
+};
+
+
+class IndexTest : public ::testing::Test {
+ protected:
+ IndexTest() : my_index_loop_(".frc971.control_loops.index",
+ 0x1a7b7094, ".frc971.control_loops.index.goal",
+ ".frc971.control_loops.index.position",
+ ".frc971.control_loops.index.output",
+ ".frc971.control_loops.index.status"),
+ index_motor_(&my_index_loop_),
+ index_motor_plant_(),
+ loop_count_(0) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ Time::EnableMockTime(Time(0, 0));
+ }
+
+ virtual ~IndexTest() {
+ ::aos::robot_state.Clear();
+ Time::DisableMockTime();
+ }
+
+ // Sends a DS packet with the enable bit set to enabled.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ // Updates the current mock time.
+ void UpdateTime() {
+ loop_count_ += 1;
+ Time::SetMockTime(Time::InMS(10 * loop_count_));
+ }
+
+ // Lets N cycles of time pass.
+ void SimulateNCycles(int cycles) {
+ for (int i = 0; i < cycles; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+ }
+
+ // Loads n discs into the indexer at the bottom.
+ void LoadNDiscs(int n) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ // Spin it up.
+ SimulateNCycles(100);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+
+ // Stuff N discs in, waiting between each one for a tiny bit of time so they
+ // don't get too close.
+ int num_grabbed = 0;
+ int wait_counter = 0;
+ while (num_grabbed < n) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (!index_motor_plant_.BottomDiscDetect()) {
+ if (wait_counter > 0) {
+ --wait_counter;
+ } else {
+ index_motor_plant_.InsertDisc();
+ ++num_grabbed;
+ wait_counter = 9;
+ }
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ // Settle.
+ for (int i = 0; i < 100; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+ }
+
+ // Loads 2 discs, and then offsets them. We then send the first disc to the
+ // grabber, and the second disc back down to the bottom. Verify that both
+ // discs get found correctly. Positive numbers shift the discs up.
+ void TestDualLostDiscs(double top_disc_offset, double bottom_disc_offset) {
+ LoadNDiscs(2);
+
+ // Move them in the indexer so they need to be re-found.
+ // The top one is moved further than the bottom one so that both edges need to
+ // be inspected.
+ index_motor_plant_.frisbees[0].OffsetIndex(
+ IndexMotor::ConvertDiscPositionToIndex(top_disc_offset));
+ index_motor_plant_.frisbees[1].OffsetIndex(
+ IndexMotor::ConvertDiscPositionToIndex(bottom_disc_offset));
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(300);
+
+ // Verify that the disc has been grabbed.
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+ // And that we are preloaded.
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->preloaded);
+
+ // Pull the disc back down.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(300);
+
+ EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
+ index_motor_plant_.frisbees[0].position(), 0.01);
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.02);
+
+ // Verify that we found the disc as accurately as the FPGA allows.
+ my_index_loop_.position.FetchLatest();
+ EXPECT_NEAR(
+ index_motor_.frisbees()[0].absolute_position(
+ my_index_loop_.position->index_position),
+ index_motor_plant_.frisbees[1].position(), 0.0001);
+ }
+
+ // Copy of core that works in this process only.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ IndexLoop my_index_loop_;
+
+ // Create a loop and simulation plant.
+ IndexMotor index_motor_;
+ IndexMotorSimulation index_motor_plant_;
+
+ // Number of loop cycles that have been executed for tracking the current
+ // time.
+ int loop_count_;
+};
+
+// Tests that the index grabs 1 disc and places it at the correct position.
+TEST_F(IndexTest, GrabSingleDisc) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 250; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ }
+ if (i > 0) {
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ IndexMotor::kIndexStartPosition + IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ index_motor_plant_.frisbees[0].position(), 0.05);
+}
+
+// Tests that the index grabs 1 disc and places it at the correct position when
+// told to hold immediately after the disc starts into the bot.
+TEST_F(IndexTest, GrabAndHold) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 200; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ } else if (i == 102) {
+ // The disc has been seen. Tell the indexer to now hold.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(0).Send();
+ } else if (i > 102) {
+ my_index_loop_.status.FetchLatest();
+ EXPECT_FALSE(my_index_loop_.status->ready_to_intake);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(0, my_index_loop_.status->shot_disc_count);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[0].position(), 0.04);
+}
+
+// Tests that the index grabs two discs and places them at the correct
+// positions.
+TEST_F(IndexTest, GrabTwoDiscs) {
+ LoadNDiscs(2);
+
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[0].position() -
+ index_motor_plant_.frisbees[1].position()), 0.10);
+}
+
+// Tests that the index grabs 2 discs, and loads one up into the loader to get
+// ready to shoot. It then pulls the second disc back down to be ready to
+// intake more.
+TEST_F(IndexTest, ReadyGrabsOneDisc) {
+ LoadNDiscs(2);
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(300);
+
+ // Verify that the disc has been grabbed.
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+ // And that we are preloaded.
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->preloaded);
+
+ // Pull the disc back down and verify that the transfer roller doesn't turn on
+ // until we are ready.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 100; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+ if (!my_index_loop_.status->ready_to_intake) {
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0)
+ << "Transfer should be off until indexer is ready";
+ }
+
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
+ index_motor_plant_.frisbees[0].position(), 0.01);
+ printf("Top disc error is %f\n",
+ IndexMotor::kReadyToLiftPosition -
+ index_motor_plant_.frisbees[0].position());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.02);
+ printf("Bottom disc error is %f\n",
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI))-
+ index_motor_plant_.frisbees[1].position());
+}
+
+// Tests that the index grabs 1 disc and continues to pull it in correctly when
+// in the READY_LOWER state. The transfer roller should be disabled then.
+TEST_F(IndexTest, GrabAndReady) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 200; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ } else if (i == 102) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
+ } else if (i > 150) {
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[0].position(), 0.04);
+}
+
+// Tests that grabbing 4 discs ends up with 4 discs in the bot and us no longer
+// ready.
+TEST_F(IndexTest, GrabFourDiscs) {
+ LoadNDiscs(4);
+
+ EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 4);
+ EXPECT_FALSE(my_index_loop_.status->ready_to_intake);
+ EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ IndexMotor::kIndexStartPosition + IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ index_motor_plant_.frisbees[3].position(), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[0].position() -
+ index_motor_plant_.frisbees[1].position()), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[1].position() -
+ index_motor_plant_.frisbees[2].position()), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[2].position() -
+ index_motor_plant_.frisbees[3].position()), 0.10);
+}
+
+// Tests that shooting 4 discs works.
+TEST_F(IndexTest, ShootFourDiscs) {
+ LoadNDiscs(4);
+
+ EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.frisbees.size());
+
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+
+ // Lifting and shooting takes a while...
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 4);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_FALSE(my_index_loop_.output->disc_clamped);
+ EXPECT_FALSE(my_index_loop_.output->loader_up);
+ EXPECT_FALSE(my_index_loop_.output->disc_ejected);
+
+ EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.shot_frisbees.size());
+}
+
+// Tests that discs aren't pulled out of the loader half way through being
+// grabbed when being asked to index.
+TEST_F(IndexTest, PreloadToIndexEarlyTransition) {
+ LoadNDiscs(2);
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ for (int i = 0; i < 300; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ // Drop out of the loop as soon as it enters the loader.
+ // This will require it to finish the job before intaking more.
+ my_index_loop_.status.FetchLatest();
+ if (index_motor_plant_.frisbees[0].position() >
+ IndexMotor::kLoaderFreeStopPosition) {
+ break;
+ }
+ }
+
+ // Pull the disc back down and verify that the transfer roller doesn't turn on
+ // until we are ready.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
+ SimulateNCycles(100);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
+ index_motor_plant_.frisbees[0].position(), 0.01);
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.10);
+}
+
+// Tests that disabling while grabbing a disc doesn't cause problems.
+TEST_F(IndexTest, HandleDisable) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 200; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ } else if (i == 102) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
+ } else if (i > 150) {
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(i < 102 || i > 110);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[0].position(), 0.04);
+}
+
+// Tests that we can shoot after grabbing in the loader.
+TEST_F(IndexTest, GrabbedToShoot) {
+ LoadNDiscs(2);
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(300);
+
+ // Verify that it is preloaded.
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->preloaded);
+
+ // Shoot them all.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(200);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
+ EXPECT_FALSE(my_index_loop_.status->preloaded);
+}
+
+// Tests that the cRIO can reboot and we don't loose discs.
+TEST_F(IndexTest, cRIOReboot) {
+ LoadNDiscs(2);
+
+ SimulateNCycles(100);
+ for (int i = 0; i < 100; ++i) {
+ // No position for a while is a cRIO reboot.
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(false);
+ UpdateTime();
+ }
+
+ // Shift the plant.
+ const double kPlantOffset = 5000.0;
+ index_motor_plant_.index_plant_->Y(0, 0) += kPlantOffset;
+ index_motor_plant_.index_plant_->X(0, 0) += kPlantOffset;
+ index_motor_plant_.bottom_disc_posedge_count_ = 971;
+ index_motor_plant_.bottom_disc_negedge_count_ = 971;
+ index_motor_plant_.bottom_disc_negedge_wait_count_ = 971;
+ index_motor_plant_.bottom_disc_negedge_wait_position_ = -1502;
+
+ // Shift the discs
+ index_motor_plant_.OffsetIndices(kPlantOffset);
+ // Let time elapse to see if the loop wants to move the discs or not.
+ SimulateNCycles(1000);
+
+ // Verify that 2 discs are at the bottom of the hopper.
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[0].position() -
+ index_motor_plant_.frisbees[1].position()), 0.10);
+}
+
+// Tests that invalid states are rejected.
+TEST_F(IndexTest, InvalidStateTest) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(10).Send();
+ SimulateNCycles(2);
+ // Verify that the goal is correct.
+ EXPECT_GE(4, static_cast<int>(index_motor_.safe_goal_));
+ EXPECT_LE(0, static_cast<int>(index_motor_.safe_goal_));
+}
+
+// Tests that the motor is turned off after a number of cycles of low power.
+TEST_F(IndexTest, ZeroPowerAfterTimeout) {
+ LoadNDiscs(4);
+ SimulateNCycles(100);
+
+ // Verify that the motor is hard off. This relies on floating point math
+ // never really getting to 0 unless you set it explicitly.
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->index_voltage, 0.0);
+}
+
+// Tests that preloading 2 discs relocates the discs if they shift on the
+// indexer. Test shifting all 4 ways.
+TEST_F(IndexTest, ShiftedDiscsAreRefound) {
+ TestDualLostDiscs(0.10, 0.15);
+}
+
+TEST_F(IndexTest, ShiftedDiscsAreRefoundOtherSeperation) {
+ TestDualLostDiscs(0.15, 0.10);
+}
+
+TEST_F(IndexTest, ShiftedDownDiscsAreRefound) {
+ TestDualLostDiscs(-0.15, -0.10);
+}
+
+TEST_F(IndexTest, ShiftedDownDiscsAreRefoundOtherSeperation) {
+ TestDualLostDiscs(-0.10, -0.15);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, IntakingAfterLoading) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(10);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, CanShootOneDiscAfterReady) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(100);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(1, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, GotExtraDisc) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+
+ double index_roller_position = index_motor_plant_.index_roller_position();
+ index_motor_plant_.InsertDisc(IndexMotor::kTopDiscDetectStart - 0.1);
+ index_motor_plant_.InsertDisc(IndexMotor::kTopDiscDetectStart - 0.6);
+ SimulateNCycles(100);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(3, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(4 * M_PI),
+ index_motor_plant_.index_roller_position() - index_roller_position);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, LostDisc) {
+ LoadNDiscs(3);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+
+ index_motor_plant_.frisbees.erase(
+ index_motor_plant_.frisbees.begin() + 1);
+
+ double index_roller_position = index_motor_plant_.index_roller_position();
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(2, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(3 * M_PI),
+ index_motor_plant_.index_roller_position() - index_roller_position);
+ EXPECT_EQ(0u, index_motor_.frisbees_.size());
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(0.0, my_index_loop_.output->index_voltage);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, CRIOReboot) {
+ index_motor_plant_.index_plant_->Y(0, 0) = 5000.0;
+ index_motor_plant_.index_plant_->X(0, 0) = 5000.0;
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(10);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ EXPECT_EQ(1, my_index_loop_.status->hopper_disc_count);
+}
+
+// Verifies that the indexer can shoot a disc and then intake and shoot another
+// one. This verifies that the code that forgets discs works correctly.
+TEST_F(IndexTest, CanShootIntakeAndShoot) {
+ for (int i = 1; i < 4; ++i) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(500);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(i, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_EQ(i, my_index_loop_.status->shot_disc_count);
+ }
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/index_main.cc b/frc971/control_loops/index/index_main.cc
new file mode 100644
index 0000000..9ca3290
--- /dev/null
+++ b/frc971/control_loops/index/index_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/index/index.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::IndexMotor indexer;
+ indexer.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
new file mode 100644
index 0000000..61e7a61
--- /dev/null
+++ b/frc971/control_loops/index/index_motor.q
@@ -0,0 +1,76 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group IndexLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // The state for the indexer to be in.
+ // 0 means hold position, in a low power state.
+ // 1 means get ready to load discs by shifting the discs down.
+ // 2 means ready the discs, spin up the transfer roller, and accept discs.
+ // 3 means get ready to shoot, and place a disc grabbed in the loader.
+ // 4 means shoot at will.
+ int32_t goal_state;
+ };
+
+ message Position {
+ // Index position
+ double index_position;
+
+ // Current values of both sensors.
+ bool top_disc_detect;
+ bool bottom_disc_detect;
+ // Counts for positive and negative edges on the bottom sensor.
+ int32_t bottom_disc_posedge_count;
+ int32_t bottom_disc_negedge_count;
+ // The most recent encoder position read after the most recent
+ // negedge and a count of how many times it's been updated.
+ double bottom_disc_negedge_wait_position;
+ int32_t bottom_disc_negedge_wait_count;
+ // The most recent index position at the posedge of the top disc detect
+ // and a count of how many edges have been seen.
+ int32_t top_disc_posedge_count;
+ double top_disc_posedge_position;
+ // The most recent index position at the negedge of the top disc detect
+ // and a count of how many edges have been seen.
+ int32_t top_disc_negedge_count;
+ double top_disc_negedge_position;
+ };
+
+ message Output {
+ // Intake roller(s) output voltage.
+ // Positive means into the robot.
+ double intake_voltage;
+ // Transfer roller output voltage.
+ // Positive means into the robot.
+ double transfer_voltage;
+ // Index roller. Positive means up towards the shooter.
+ double index_voltage;
+ // Loader pistons.
+ bool disc_clamped;
+ bool loader_up;
+ bool disc_ejected;
+ };
+
+ message Status {
+ // Number of discs in the hopper
+ int32_t hopper_disc_count;
+ // Number of discs seen by the hopper.
+ int32_t total_disc_count;
+ // Number of discs shot by the shooter.
+ int32_t shot_disc_count;
+ // Disc ready in the loader.
+ bool preloaded;
+ // Indexer ready to accept more discs.
+ bool ready_to_intake;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group IndexLoop index_loop;
diff --git a/frc971/control_loops/index/index_motor_plant.cc b/frc971/control_loops/index/index_motor_plant.cc
new file mode 100644
index 0000000..51661fb
--- /dev/null
+++ b/frc971/control_loops/index/index_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/index/index_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeIndexPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeIndexLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 2.40538224198, 0.0619371641882;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeIndexPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/index/index_motor_plant.h b/frc971/control_loops/index/index_motor_plant.h
new file mode 100644
index 0000000..4600ccf
--- /dev/null
+++ b/frc971/control_loops/index/index_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeIndexPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeIndexLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/index/transfer_motor_plant.cc b/frc971/control_loops/index/transfer_motor_plant.cc
new file mode 100644
index 0000000..9333f6a
--- /dev/null
+++ b/frc971/control_loops/index/transfer_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/index/transfer_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeTransferPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeTransferLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1.06905877421, 0.0368709177253;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeTransferPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/index/transfer_motor_plant.h b/frc971/control_loops/index/transfer_motor_plant.h
new file mode 100644
index 0000000..565e8c7
--- /dev/null
+++ b/frc971/control_loops/index/transfer_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeTransferPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeTransferLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/python/angle_adjust.py b/frc971/control_loops/python/angle_adjust.py
new file mode 100755
index 0000000..ed04198
--- /dev/null
+++ b/frc971/control_loops/python/angle_adjust.py
@@ -0,0 +1,89 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class AngleAdjust(control_loop.ControlLoop):
+ def __init__(self):
+ super(AngleAdjust, self).__init__("AngleAdjust")
+ # Stall Torque in N m
+ self.stall_torque = .428
+ # Stall Current in Amps
+ self.stall_current = 63.8
+ # Free Speed in RPM
+ self.free_speed = 16000.0
+ # Free Current in Amps
+ self.free_current = 1.2
+ # Moment of inertia of the angle adjust about the shooter's pivot in kg m^2
+ self.J = 0.41085133
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio of the gearbox multiplied by the ratio of the radii of
+ # the output and the angle adjust curve, which is essentially another gear.
+ self.G = (1.0 / 50.0) * (0.01905 / 0.41964)
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.89, .85])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ angle_adjust = AngleAdjust()
+ simulated_x = []
+ for _ in xrange(100):
+ angle_adjust.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(angle_adjust.X[0, 0])
+
+ pylab.plot(range(100), simulated_x)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ angle_adjust = AngleAdjust()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(angle_adjust.K * (R - angle_adjust.X_hat), angle_adjust.U_min, angle_adjust.U_max)
+ angle_adjust.UpdateObserver(U)
+ angle_adjust.Update(U)
+ close_loop_x.append(angle_adjust.X[0, 0])
+
+ pylab.plot(range(100), close_loop_x)
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .cc file name and .h file name"
+ else:
+ angle_adjust.DumpHeaderFile(argv[1])
+ angle_adjust.DumpCppFile(argv[2], argv[1])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/index.py b/frc971/control_loops/python/index.py
new file mode 100755
index 0000000..c65397b
--- /dev/null
+++ b/frc971/control_loops/python/index.py
@@ -0,0 +1,94 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Index(control_loop.ControlLoop):
+ def __init__(self):
+ super(Index, self).__init__("Index")
+ # Stall Torque in N m
+ self.stall_torque = 0.4862
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.5
+ # Moment of inertia of the index in kg m^2
+ self.J = 0.00013
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 1.0 / ((40.0 / 11.0) * (34.0 / 30.0))
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.5, .55])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ index = Index()
+ simulated_x = []
+ simulated_v = []
+ for _ in xrange(100):
+ index.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(index.X[0, 0])
+ simulated_v.append(index.X[1, 0])
+
+ pylab.plot(range(100), simulated_v)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ index = Index()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(index.K * (R - index.X_hat), index.U_min, index.U_max)
+ index.UpdateObserver(U)
+ index.Update(U)
+ close_loop_x.append(index.X[0, 0])
+
+ pylab.plot(range(100), close_loop_x)
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .h file name and .c file name"
+ else:
+ if argv[1][-3:] == '.cc':
+ print '.cc file is second'
+ else:
+ index.DumpHeaderFile(argv[1])
+ index.DumpCppFile(argv[2], argv[1])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/transfer.py b/frc971/control_loops/python/transfer.py
new file mode 100755
index 0000000..2ec7926
--- /dev/null
+++ b/frc971/control_loops/python/transfer.py
@@ -0,0 +1,91 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Transfer(control_loop.ControlLoop):
+ def __init__(self):
+ super(Transfer, self).__init__("Transfer")
+ # Stall Torque in N m
+ self.stall_torque = 0.4862
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.5
+ # Moment of inertia of the transfer in kg m^2
+ self.J = 0.00013
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 1.0 / ((40.0 / 11.0) * (34.0 / 30.0))
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.75, .6])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ transfer = Transfer()
+ simulated_x = []
+ simulated_v = []
+ for _ in xrange(100):
+ transfer.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(transfer.X[0, 0])
+ simulated_v.append(transfer.X[1, 0])
+
+ pylab.plot(range(100), simulated_v)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ transfer = Transfer()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(transfer.K * (R - transfer.X_hat), transfer.U_min, transfer.U_max)
+ transfer.UpdateObserver(U)
+ transfer.Update(U)
+ close_loop_x.append(transfer.X[0, 0])
+
+ #pylab.plot(range(100), close_loop_x)
+ #pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .cc file name and .h file name"
+ else:
+ transfer.DumpHeaderFile(argv[1])
+ transfer.DumpCppFile(argv[2], argv[1])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 8fe18f8..894487b 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -1,7 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
#define FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
-// wikipedia article is <http://en.wikipedia.org/wiki/State_observer>
+#include <assert.h>
// Stupid vxworks system headers define it which blows up Eigen...
#undef m_data
@@ -56,20 +56,19 @@
virtual ~StateFeedbackPlant() {}
- // If U is outside the hardware range, limit it before the plant tries to use
- // it.
- virtual void CapU() {
+ // Assert that U is within the hardware range.
+ virtual void CheckU() {
for (int i = 0; i < kNumOutputs; ++i) {
- if (U[i] > U_max[i]) {
- U[i] = U_max[i];
- } else if (U[i] < U_min[i]) {
- U[i] = U_min[i];
- }
+ assert(U[i] <= U_max[i]);
+ assert(U[i] >= U_min[i]);
}
}
+
// Computes the new X and Y given the control input.
void Update() {
- CapU();
+ // Powers outside of the range are more likely controller bugs than things
+ // that the plant should deal with.
+ CheckU();
X = A * X + B * U;
Y = C * X + D * U;
}
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
index b49e99c..13471fa 100644
--- a/frc971/control_loops/wrist/wrist.cc
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -18,59 +18,33 @@
WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
: aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
- loop_(new WristStateFeedbackLoop(MakeWristLoop(), this)),
- state_(UNINITIALIZED),
- error_count_(0),
- zero_offset_(0.0),
- capped_goal_(false) {
+ zeroed_joint_(MakeWristLoop()) {
}
-bool WristMotor::FetchConstants() {
- if (!constants::wrist_lower_limit(&wrist_lower_limit_)) {
+bool WristMotor::FetchConstants(
+ ZeroedJoint<1>::ConfigurationData *config_data) {
+ if (!constants::wrist_lower_limit(&config_data->lower_limit)) {
LOG(ERROR, "Failed to fetch the wrist lower limit constant.\n");
return false;
}
- if (!constants::wrist_upper_limit(&wrist_upper_limit_)) {
+ if (!constants::wrist_upper_limit(&config_data->upper_limit)) {
LOG(ERROR, "Failed to fetch the wrist upper limit constant.\n");
return false;
}
if (!constants::wrist_hall_effect_start_angle(
- &wrist_hall_effect_start_angle_)) {
+ &config_data->hall_effect_start_angle[0])) {
LOG(ERROR, "Failed to fetch the wrist start angle constant.\n");
return false;
}
- if (!constants::wrist_zeroing_speed(
- &wrist_zeroing_speed_)) {
+ if (!constants::wrist_zeroing_speed(&config_data->zeroing_speed)) {
LOG(ERROR, "Failed to fetch the wrist zeroing speed constant.\n");
return false;
}
+ config_data->max_zeroing_voltage = 5.0;
return true;
}
-double WristMotor::ClipGoal(double goal) const {
- return std::min(wrist_upper_limit_,
- std::max(wrist_lower_limit_, goal));
-}
-
-const double kMaxZeroingVoltage = 5.0;
-
-void WristMotor::WristStateFeedbackLoop::CapU() {
- if (wrist_motor_->state_ == READY) {
- if (Y(0, 0) >= wrist_motor_->wrist_upper_limit_) {
- U(0, 0) = std::min(0.0, U(0, 0));
- }
- if (Y(0, 0) <= wrist_motor_->wrist_lower_limit_) {
- U(0, 0) = std::max(0.0, U(0, 0));
- }
- }
-
- double limit = (wrist_motor_->state_ == READY) ? 12.0 : kMaxZeroingVoltage;
-
- U(0, 0) = std::min(limit, U(0, 0));
- U(0, 0) = std::max(-limit, U(0, 0));
-}
-
// Positive angle is up, and positive power is up.
void WristMotor::RunIteration(
const ::aos::control_loops::Goal *goal,
@@ -85,151 +59,36 @@
}
// Cache the constants to avoid error handling down below.
- if (!FetchConstants()) {
+ ZeroedJoint<1>::ConfigurationData config_data;
+ if (!FetchConstants(&config_data)) {
return;
- }
-
- // Uninitialize the bot if too many cycles pass without an encoder.
- if (position == NULL) {
- LOG(WARNING, "no new pos given\n");
- error_count_++;
} else {
- error_count_ = 0;
- }
- if (error_count_ >= 4) {
- LOG(WARNING, "err_count is %d so forcing a re-zero\n", error_count_);
- state_ = UNINITIALIZED;
+ zeroed_joint_.set_config_data(config_data);
}
- // Compute the absolute position of the wrist.
- double absolute_position;
- if (position) {
- absolute_position =
- position->pos + wrist_hall_effect_start_angle_;
- if (state_ == READY) {
- absolute_position -= zero_offset_;
- }
- loop_->Y << absolute_position;
- if (!position->hall_effect) {
- last_off_position_ = position->pos;
- }
+ ZeroedJoint<1>::PositionData transformed_position;
+ ZeroedJoint<1>::PositionData *transformed_position_ptr =
+ &transformed_position;
+ if (!position) {
+ transformed_position_ptr = NULL;
} else {
- // Dead recon for now.
- absolute_position = loop_->X_hat(0, 0);
+ transformed_position.position = position->pos;
+ transformed_position.hall_effects[0] = position->hall_effect;
+ transformed_position.hall_effect_positions[0] = position->calibration;
}
- switch (state_) {
- case UNINITIALIZED:
- if (position) {
- // Reset the zeroing goal.
- zeroing_position_ = absolute_position;
- // Clear the observer state.
- loop_->X_hat << absolute_position, 0.0;
- // Set the goal to here to make it so it doesn't move when disabled.
- loop_->R = loop_->X_hat;
- // Only progress if we are enabled.
- if (::aos::robot_state->enabled) {
- if (position->hall_effect) {
- state_ = MOVING_OFF;
- } else {
- state_ = ZEROING;
- }
- }
- }
- break;
- case MOVING_OFF:
- // Move off the hall effect sensor.
- if (!::aos::robot_state->enabled) {
- // Start over if disabled.
- state_ = UNINITIALIZED;
- } else if (position && !position->hall_effect) {
- // We are now off the sensor. Time to zero now.
- state_ = ZEROING;
- } else {
- // Slowly creep off the sensor.
- zeroing_position_ -= wrist_zeroing_speed_ / 100;
- loop_->R << zeroing_position_, -wrist_zeroing_speed_;
- break;
- }
- case ZEROING:
- if (!::aos::robot_state->enabled) {
- // Start over if disabled.
- state_ = UNINITIALIZED;
- } else if (position && position->hall_effect) {
- state_ = READY;
- // Verify that the calibration number is between the last off position
- // and the current on position. If this is not true, move off and try
- // again.
- if (position->calibration <= last_off_position_ ||
- position->calibration > position->pos) {
- LOG(ERROR, "Got a bogus calibration number. Trying again.\n");
- LOG(ERROR,
- "Last off position was %f, current is %f, calibration is %f\n",
- last_off_position_, position->pos, position->calibration);
- state_ = MOVING_OFF;
- } else {
- // Save the zero, and then offset the observer to deal with the
- // phantom step change.
- const double old_zero_offset = zero_offset_;
- zero_offset_ = position->calibration;
- loop_->X_hat(0, 0) += old_zero_offset - zero_offset_;
- loop_->Y(0, 0) += old_zero_offset - zero_offset_;
- }
- } else {
- // Slowly creep towards the sensor.
- zeroing_position_ += wrist_zeroing_speed_ / 100;
- loop_->R << zeroing_position_, wrist_zeroing_speed_;
- }
- break;
-
- case READY:
- {
- const double limited_goal = ClipGoal(goal->goal);
- loop_->R << limited_goal, 0.0;
- break;
- }
-
- case ESTOP:
- LOG(WARNING, "have already given up\n");
- return;
- }
-
- // Update the observer.
- loop_->Update(position != NULL, output == NULL);
-
- capped_goal_ = false;
- // Verify that the zeroing goal hasn't run away.
- switch (state_) {
- case UNINITIALIZED:
- case READY:
- case ESTOP:
- // Not zeroing. No worries.
- break;
- case MOVING_OFF:
- case ZEROING:
- // Check if we have cliped and adjust the goal.
- if (loop_->U_uncapped(0, 0) > kMaxZeroingVoltage) {
- double dx = (loop_->U_uncapped(0, 0) -
- kMaxZeroingVoltage) / loop_->K(0, 0);
- zeroing_position_ -= dx;
- capped_goal_ = true;
- } else if(loop_->U_uncapped(0, 0) < -kMaxZeroingVoltage) {
- double dx = (loop_->U_uncapped(0, 0) +
- kMaxZeroingVoltage) / loop_->K(0, 0);
- zeroing_position_ -= dx;
- capped_goal_ = true;
- }
- break;
- }
+ const double voltage = zeroed_joint_.Update(transformed_position_ptr,
+ output != NULL,
+ goal->goal, 0.0);
if (position) {
- LOG(DEBUG, "pos=%f zero=%f currently %f hall: %s\n",
- position->pos, zero_offset_, absolute_position,
- position->hall_effect ? "true" : "false");
+ //LOG(DEBUG, "pos=%f zero=%f currently %f hall: %s\n",
+ //position->pos, zero_offset_, absolute_position,
+ //position->hall_effect ? "true" : "false");
}
if (output) {
- output->voltage = loop_->U(0, 0);
+ output->voltage = voltage;
}
}
diff --git a/frc971/control_loops/wrist/wrist.h b/frc971/control_loops/wrist/wrist.h
index 55d14c8..a53f603 100644
--- a/frc971/control_loops/wrist/wrist.h
+++ b/frc971/control_loops/wrist/wrist.h
@@ -8,19 +8,15 @@
#include "frc971/control_loops/wrist/wrist_motor.q.h"
#include "frc971/control_loops/wrist/wrist_motor_plant.h"
+#include "frc971/control_loops/zeroed_joint.h"
+
namespace frc971 {
namespace control_loops {
-
namespace testing {
-class WristTest_RezeroWithMissingPos_Test;
-class WristTest_DisableGoesUninitialized_Test;
-class WristTest_NoWindup_Test;
class WristTest_NoWindupPositive_Test;
class WristTest_NoWindupNegative_Test;
};
-class WristMotor;
-
class WristMotor
: public aos::control_loops::ControlLoop<control_loops::WristLoop> {
public:
@@ -28,7 +24,19 @@
control_loops::WristLoop *my_wrist = &control_loops::wrist);
// True if the goal was moved to avoid goal windup.
- bool capped_goal() const { return capped_goal_; }
+ bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+
+ // True if the wrist is zeroing.
+ bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+
+ // True if the wrist is zeroing.
+ bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return zeroed_joint_.is_ready(); }
protected:
virtual void RunIteration(
@@ -39,68 +47,14 @@
private:
// Friend the test classes for acces to the internal state.
- friend class testing::WristTest_RezeroWithMissingPos_Test;
- friend class testing::WristTest_DisableGoesUninitialized_Test;
friend class testing::WristTest_NoWindupPositive_Test;
friend class testing::WristTest_NoWindupNegative_Test;
- friend class WristStateFeedbackLoop;
// Fetches and locally caches the latest set of constants.
- bool FetchConstants();
+ bool FetchConstants(ZeroedJoint<1>::ConfigurationData *config_data);
- // Clips the goal to be inside the limits and returns the clipped goal.
- // Requires the constants to have already been fetched.
- double ClipGoal(double goal) const;
-
- // This class implements the CapU function correctly given all the extra
- // information that we know about from the wrist motor.
- class WristStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
- public:
- WristStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop,
- WristMotor *wrist_motor)
- : StateFeedbackLoop<2, 1, 1>(loop),
- wrist_motor_(wrist_motor) {
- }
-
- // Caps U, but this time respects the state of the wrist as well.
- virtual void CapU();
- private:
- WristMotor *wrist_motor_;
- };
-
- // The state feedback control loop to talk to.
- ::std::unique_ptr<WristStateFeedbackLoop> loop_;
-
- // Enum to store the state of the internal zeroing state machine.
- enum State {
- UNINITIALIZED,
- MOVING_OFF,
- ZEROING,
- READY,
- ESTOP
- };
-
- // Internal state for zeroing.
- State state_;
-
- // Missed position packet count.
- int error_count_;
- // Offset from the raw encoder value to the absolute angle.
- double zero_offset_;
- // Position that gets incremented when zeroing the wrist to slowly move it to
- // the hall effect sensor.
- double zeroing_position_;
- // Last position at which the hall effect sensor was off.
- double last_off_position_;
-
- // Local cache of the wrist geometry constants.
- double wrist_lower_limit_;
- double wrist_upper_limit_;
- double wrist_hall_effect_start_angle_;
- double wrist_zeroing_speed_;
-
- // True if the zeroing goal was capped during this cycle.
- bool capped_goal_;
+ // The zeroed joint to use.
+ ZeroedJoint<1> zeroed_joint_;
DISALLOW_COPY_AND_ASSIGN(WristMotor);
};
diff --git a/frc971/control_loops/wrist/wrist_lib_test.cc b/frc971/control_loops/wrist/wrist_lib_test.cc
index aa0cb78..83703fa 100644
--- a/frc971/control_loops/wrist/wrist_lib_test.cc
+++ b/frc971/control_loops/wrist/wrist_lib_test.cc
@@ -218,12 +218,12 @@
} else {
if (i > 310) {
// Should be re-zeroing now.
- EXPECT_EQ(WristMotor::UNINITIALIZED, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_uninitialized());
}
my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
}
if (i == 430) {
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
}
wrist_motor_.Iterate();
@@ -245,7 +245,7 @@
if (i > 100) {
// Give the loop a couple cycled to get the message and then verify that
// it is in the correct state.
- EXPECT_EQ(WristMotor::UNINITIALIZED, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_uninitialized());
// When disabled, we should be applying 0 power.
EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
EXPECT_EQ(0, my_wrist_loop_.output->voltage);
@@ -255,7 +255,7 @@
}
if (i == 202) {
// Verify that we are zeroing after the bot gets enabled again.
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
}
wrist_motor_.Iterate();
@@ -273,26 +273,26 @@
for (int i = 0; i < 500; ++i) {
wrist_motor_plant_.SendPositionMessage();
if (i == 50) {
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
// Move the zeroing position far away and verify that it gets moved back.
- saved_zeroing_position = wrist_motor_.zeroing_position_;
- wrist_motor_.zeroing_position_ = -100.0;
+ saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
+ wrist_motor_.zeroed_joint_.zeroing_position_ = -100.0;
} else if (i == 51) {
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
- EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position,
+ wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
}
- if (wrist_motor_.state_ != WristMotor::READY) {
+ if (!wrist_motor_.is_ready()) {
if (wrist_motor_.capped_goal()) {
++capped_count;
// The cycle after we kick the zero position is the only cycle during
// which we should expect to see a high uncapped power during zeroing.
- ASSERT_LT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ ASSERT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
} else {
- ASSERT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ ASSERT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
}
}
-
wrist_motor_.Iterate();
wrist_motor_plant_.Simulate();
SendDSPacket(true);
@@ -310,24 +310,24 @@
for (int i = 0; i < 500; ++i) {
wrist_motor_plant_.SendPositionMessage();
if (i == 50) {
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
// Move the zeroing position far away and verify that it gets moved back.
- saved_zeroing_position = wrist_motor_.zeroing_position_;
- wrist_motor_.zeroing_position_ = 100.0;
+ saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
+ wrist_motor_.zeroed_joint_.zeroing_position_ = 100.0;
} else {
if (i == 51) {
- EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
- EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
}
}
- if (wrist_motor_.state_ != WristMotor::READY) {
+ if (!wrist_motor_.is_ready()) {
if (wrist_motor_.capped_goal()) {
++capped_count;
// The cycle after we kick the zero position is the only cycle during
// which we should expect to see a high uncapped power during zeroing.
- EXPECT_LT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ EXPECT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
} else {
- EXPECT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ EXPECT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
}
}
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
new file mode 100644
index 0000000..89a3c7b
--- /dev/null
+++ b/frc971/control_loops/zeroed_joint.h
@@ -0,0 +1,363 @@
+#ifndef FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
+#define FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class WristTest_NoWindupPositive_Test;
+class WristTest_NoWindupNegative_Test;
+};
+
+template<int kNumZeroSensors>
+class ZeroedJoint;
+
+// This class implements the CapU function correctly given all the extra
+// information that we know about from the wrist motor.
+template<int kNumZeroSensors>
+class ZeroedStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
+ public:
+ ZeroedStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop,
+ ZeroedJoint<kNumZeroSensors> *zeroed_joint)
+ : StateFeedbackLoop<2, 1, 1>(loop),
+ zeroed_joint_(zeroed_joint) {
+ }
+
+ // Caps U, but this time respects the state of the wrist as well.
+ virtual void CapU();
+ private:
+ ZeroedJoint<kNumZeroSensors> *zeroed_joint_;
+};
+
+template<int kNumZeroSensors>
+void ZeroedStateFeedbackLoop<kNumZeroSensors>::CapU() {
+ if (zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY) {
+ if (Y(0, 0) >= zeroed_joint_->config_data_.upper_limit) {
+ U(0, 0) = std::min(0.0, U(0, 0));
+ }
+ if (Y(0, 0) <= zeroed_joint_->config_data_.lower_limit) {
+ U(0, 0) = std::max(0.0, U(0, 0));
+ }
+ }
+
+ const bool is_ready =
+ zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY;
+ double limit = is_ready ?
+ 12.0 : zeroed_joint_->config_data_.max_zeroing_voltage;
+
+ U(0, 0) = std::min(limit, U(0, 0));
+ U(0, 0) = std::max(-limit, U(0, 0));
+}
+
+
+// Class to zero and control a joint with any number of zeroing sensors with a
+// state feedback controller.
+template<int kNumZeroSensors>
+class ZeroedJoint {
+ public:
+ // Sturcture to hold the hardware configuration information.
+ struct ConfigurationData {
+ // Angle at the lower hardware limit.
+ double lower_limit;
+ // Angle at the upper hardware limit.
+ double upper_limit;
+ // Speed (and direction) to move while zeroing.
+ double zeroing_speed;
+ // Maximum voltage to apply when zeroing.
+ double max_zeroing_voltage;
+ // Angles where we see a positive edge from the hall effect sensors.
+ double hall_effect_start_angle[kNumZeroSensors];
+ };
+
+ // Current position data for the encoder and hall effect information.
+ struct PositionData {
+ // Current encoder position.
+ double position;
+ // Array of hall effect values.
+ bool hall_effects[kNumZeroSensors];
+ // Array of the last positive edge position for the sensors.
+ double hall_effect_positions[kNumZeroSensors];
+ };
+
+ ZeroedJoint(StateFeedbackLoop<2, 1, 1> loop)
+ : loop_(new ZeroedStateFeedbackLoop<kNumZeroSensors>(loop, this)),
+ state_(UNINITIALIZED),
+ error_count_(0),
+ zero_offset_(0.0),
+ capped_goal_(false) {
+ }
+
+ // Copies the provided configuration data locally.
+ void set_config_data(const ConfigurationData &config_data) {
+ config_data_ = config_data;
+ }
+
+ // Clips the goal to be inside the limits and returns the clipped goal.
+ // Requires the constants to have already been fetched.
+ double ClipGoal(double goal) const {
+ return ::std::min(config_data_.upper_limit,
+ std::max(config_data_.lower_limit, goal));
+ }
+
+ // Updates the loop and state machine.
+ // position is null if the position data is stale, output_enabled is true if
+ // the output will actually go to the motors, and goal_angle and goal_velocity
+ // are the goal position and velocities.
+ double Update(const ZeroedJoint<kNumZeroSensors>::PositionData *position,
+ bool output_enabled,
+ double goal_angle, double goal_velocity);
+
+ // True if the code is zeroing.
+ bool is_zeroing() const { return state_ == ZEROING; }
+
+ // True if the code is moving off the hall effect.
+ bool is_moving_off() const { return state_ == MOVING_OFF; }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return state_ == UNINITIALIZED; }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return state_ == READY; }
+
+ // Returns the uncapped voltage.
+ double U_uncapped() const { return loop_->U_uncapped(0, 0); }
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return capped_goal_; }
+
+ // Timestamp
+ static const double dt;
+
+ private:
+ friend class ZeroedStateFeedbackLoop<kNumZeroSensors>;
+ // Friend the wrist test cases so that they can simulate windeup.
+ friend class testing::WristTest_NoWindupPositive_Test;
+ friend class testing::WristTest_NoWindupNegative_Test;
+
+ // The state feedback control loop to talk to.
+ ::std::unique_ptr<ZeroedStateFeedbackLoop<kNumZeroSensors>> loop_;
+
+ ConfigurationData config_data_;
+
+ // Returns the index of the first active sensor, or -1 if none are active.
+ int ActiveSensorIndex(
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position) {
+ if (!position) {
+ return -1;
+ }
+ int active_index = -1;
+ for (int i = 0; i < kNumZeroSensors; ++i) {
+ if (position->hall_effects[i]) {
+ if (active_index != -1) {
+ LOG(ERROR, "More than one hall effect sensor is active\n");
+ } else {
+ active_index = i;
+ }
+ }
+ }
+ return active_index;
+ }
+ // Returns true if any of the sensors are active.
+ bool AnySensorsActive(
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position) {
+ return ActiveSensorIndex(position) != -1;
+ }
+
+ // Enum to store the state of the internal zeroing state machine.
+ enum State {
+ UNINITIALIZED,
+ MOVING_OFF,
+ ZEROING,
+ READY,
+ ESTOP
+ };
+
+ // Internal state for zeroing.
+ State state_;
+
+ // Missed position packet count.
+ int error_count_;
+ // Offset from the raw encoder value to the absolute angle.
+ double zero_offset_;
+ // Position that gets incremented when zeroing the wrist to slowly move it to
+ // the hall effect sensor.
+ double zeroing_position_;
+ // Last position at which the hall effect sensor was off.
+ double last_off_position_;
+
+ // True if the zeroing goal was capped during this cycle.
+ bool capped_goal_;
+
+ // Returns true if number is between first and second inclusive.
+ bool is_between(double first, double second, double number) {
+ if ((number >= first || number >= second) &&
+ (number <= first || number <= second)) {
+ return true;
+ }
+ return false;
+ }
+
+ DISALLOW_COPY_AND_ASSIGN(ZeroedJoint);
+};
+
+template <int kNumZeroSensors>
+/*static*/ const double ZeroedJoint<kNumZeroSensors>::dt = 0.01;
+
+// Updates the zeroed joint controller and state machine.
+template <int kNumZeroSensors>
+double ZeroedJoint<kNumZeroSensors>::Update(
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position,
+ bool output_enabled,
+ double goal_angle, double goal_velocity) {
+ // Uninitialize the bot if too many cycles pass without an encoder.
+ if (position == NULL) {
+ LOG(WARNING, "no new pos given\n");
+ error_count_++;
+ } else {
+ error_count_ = 0;
+ }
+ if (error_count_ >= 4) {
+ LOG(WARNING, "err_count is %d so forcing a re-zero\n", error_count_);
+ state_ = UNINITIALIZED;
+ }
+
+ // Compute the absolute position of the wrist.
+ double absolute_position;
+ if (position) {
+ absolute_position = position->position;
+ if (state_ == READY) {
+ absolute_position -= zero_offset_;
+ }
+ loop_->Y << absolute_position;
+ if (!AnySensorsActive(position)) {
+ last_off_position_ = position->position;
+ }
+ } else {
+ // Dead recon for now.
+ absolute_position = loop_->X_hat(0, 0);
+ }
+
+ switch (state_) {
+ case UNINITIALIZED:
+ if (position) {
+ // Reset the zeroing goal.
+ zeroing_position_ = absolute_position;
+ // Clear the observer state.
+ loop_->X_hat << absolute_position, 0.0;
+ // Set the goal to here to make it so it doesn't move when disabled.
+ loop_->R = loop_->X_hat;
+ // Only progress if we are enabled.
+ if (::aos::robot_state->enabled) {
+ if (AnySensorsActive(position)) {
+ state_ = MOVING_OFF;
+ } else {
+ state_ = ZEROING;
+ }
+ }
+ }
+ break;
+ case MOVING_OFF:
+ {
+ // Move off the hall effect sensor.
+ if (!::aos::robot_state->enabled) {
+ // Start over if disabled.
+ state_ = UNINITIALIZED;
+ } else if (position && !AnySensorsActive(position)) {
+ // We are now off the sensor. Time to zero now.
+ state_ = ZEROING;
+ } else {
+ // Slowly creep off the sensor.
+ zeroing_position_ -= config_data_.zeroing_speed * dt;
+ loop_->R << zeroing_position_, -config_data_.zeroing_speed;
+ break;
+ }
+ }
+ case ZEROING:
+ {
+ int active_sensor_index = ActiveSensorIndex(position);
+ if (!::aos::robot_state->enabled) {
+ // Start over if disabled.
+ state_ = UNINITIALIZED;
+ } else if (position && active_sensor_index != -1) {
+ state_ = READY;
+ // Verify that the calibration number is between the last off position
+ // and the current on position. If this is not true, move off and try
+ // again.
+ const double calibration =
+ position->hall_effect_positions[active_sensor_index];
+ if (!is_between(last_off_position_, position->position,
+ calibration)) {
+ LOG(ERROR, "Got a bogus calibration number. Trying again.\n");
+ LOG(ERROR,
+ "Last off position was %f, current is %f, calibration is %f\n",
+ last_off_position_, position->position,
+ position->hall_effect_positions[active_sensor_index]);
+ state_ = MOVING_OFF;
+ } else {
+ // Save the zero, and then offset the observer to deal with the
+ // phantom step change.
+ const double old_zero_offset = zero_offset_;
+ zero_offset_ =
+ position->hall_effect_positions[active_sensor_index] -
+ config_data_.hall_effect_start_angle[active_sensor_index];
+ loop_->X_hat(0, 0) += old_zero_offset - zero_offset_;
+ loop_->Y(0, 0) += old_zero_offset - zero_offset_;
+ }
+ } else {
+ // Slowly creep towards the sensor.
+ zeroing_position_ += config_data_.zeroing_speed * dt;
+ loop_->R << zeroing_position_, config_data_.zeroing_speed;
+ }
+ break;
+ }
+
+ case READY:
+ {
+ const double limited_goal = ClipGoal(goal_angle);
+ loop_->R << limited_goal, goal_velocity;
+ break;
+ }
+
+ case ESTOP:
+ LOG(WARNING, "have already given up\n");
+ return 0.0;
+ }
+
+ // Update the observer.
+ loop_->Update(position != NULL, !output_enabled);
+
+ capped_goal_ = false;
+ // Verify that the zeroing goal hasn't run away.
+ switch (state_) {
+ case UNINITIALIZED:
+ case READY:
+ case ESTOP:
+ // Not zeroing. No worries.
+ break;
+ case MOVING_OFF:
+ case ZEROING:
+ // Check if we have cliped and adjust the goal.
+ if (loop_->U_uncapped(0, 0) > config_data_.max_zeroing_voltage) {
+ double dx = (loop_->U_uncapped(0, 0) -
+ config_data_.max_zeroing_voltage) / loop_->K(0, 0);
+ zeroing_position_ -= dx;
+ capped_goal_ = true;
+ } else if(loop_->U_uncapped(0, 0) < -config_data_.max_zeroing_voltage) {
+ double dx = (loop_->U_uncapped(0, 0) +
+ config_data_.max_zeroing_voltage) / loop_->K(0, 0);
+ zeroing_position_ -= dx;
+ capped_goal_ = true;
+ }
+ break;
+ }
+ return loop_->U(0, 0);
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_