merging in analog sensor support and various other fixes
diff --git a/aos/atom_code/init.cc b/aos/atom_code/init.cc
index c4e6d44..b3668e8 100644
--- a/aos/atom_code/init.cc
+++ b/aos/atom_code/init.cc
@@ -81,7 +81,7 @@
void InitNRT() { DoInitNRT(aos_core_create::reference); }
void InitCreate() { DoInitNRT(aos_core_create::create); }
-void Init() {
+void Init(int relative_priority) {
if (getenv(kNoRealtimeEnvironmentVariable) == NULL) { // if nobody set it
LockAllMemory();
// Only let rt processes run for 1 second straight.
@@ -90,7 +90,7 @@
SetSoftRLimit(RLIMIT_RTPRIO, 40, false);
// Set our process to priority 40.
struct sched_param param;
- param.sched_priority = 40;
+ param.sched_priority = 30 + relative_priority;
if (sched_setscheduler(0, SCHED_FIFO, ¶m) != 0) {
Die("%s-init: setting SCHED_FIFO failed with %d (%s)\n",
program_invocation_short_name, errno, strerror(errno));
diff --git a/aos/atom_code/init.h b/aos/atom_code/init.h
index ffb7afc..b35a3b8 100644
--- a/aos/atom_code/init.h
+++ b/aos/atom_code/init.h
@@ -6,7 +6,9 @@
// Does the non-realtime parts of the initialization process.
void InitNRT();
// Initializes everything, including the realtime stuff.
-void Init();
+// relative_priority adjusts the priority of this process relative to all of the
+// other ones (positive for higher priority).
+void Init(int relative_priority = 0);
// Same as InitNRT, except will remove an existing shared memory file and create
// a new one.
void InitCreate();
diff --git a/aos/common/network/network.gyp b/aos/common/network/network.gyp
index 279bb53..60b37aa 100644
--- a/aos/common/network/network.gyp
+++ b/aos/common/network/network.gyp
@@ -1,6 +1,18 @@
{
'targets': [
{
+ 'target_name': 'team_number',
+ 'type': 'static_library',
+ 'sources': [
+ 'team_number.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:configuration',
+ '<(AOS)/common/common.gyp:once',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ {
'target_name': 'socket_so',
'type': 'shared_library',
'variables': {'no_rsync': 1},
diff --git a/aos/common/network/team_number.cc b/aos/common/network/team_number.cc
new file mode 100644
index 0000000..367f991
--- /dev/null
+++ b/aos/common/network/team_number.cc
@@ -0,0 +1,31 @@
+#include "aos/common/network/team_number.h"
+
+#include <netinet/in.h>
+#include <inttypes.h>
+
+#include "aos/common/once.h"
+#include "aos/atom_code/configuration.h"
+#include "aos/common/logging/logging.h"
+
+namespace aos {
+namespace network {
+namespace {
+
+uint16_t *DoGetTeamNumber() {
+ const in_addr &address = configuration::GetOwnIPAddress();
+ static uint16_t r =
+ (((address.s_addr & 0xFF00) >> 8) * 100) +
+ (((address.s_addr & 0xFF0000) >> 16) & 0xFF);
+ LOG(INFO, "team number is %" PRIu16 "\n", r);
+ return &r;
+}
+
+} // namespace
+
+uint16_t GetTeamNumber() {
+ static Once<uint16_t> once(DoGetTeamNumber);
+ return *once.Get();
+}
+
+} // namespace network
+} // namespace aos
diff --git a/aos/common/network/team_number.h b/aos/common/network/team_number.h
new file mode 100644
index 0000000..f250c85
--- /dev/null
+++ b/aos/common/network/team_number.h
@@ -0,0 +1,17 @@
+#ifndef AOS_COMMON_NETWORK_TEAM_NUMBER_H_
+#define AOS_COMMON_NETWORK_TEAM_NUMBER_H_
+
+#include <stdint.h>
+
+namespace aos {
+namespace network {
+
+// Retrieves the current team number based off of the network address.
+// This function will only do the complicated stuff once so it is cheap to call
+// repeatedly.
+uint16_t GetTeamNumber();
+
+} // namespace network
+} // namespace aos
+
+#endif // AOS_COMMON_NETWORK_TEAM_NUMBER_H_
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 13aeb51..97c2310 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -3,7 +3,6 @@
#include "aos/common/control_loop/Timing.h"
#include "aos/common/time.h"
#include "aos/common/util/trapezoid_profile.h"
-#include "aos/common/messages/RobotState.q.h"
#include "aos/common/logging/logging.h"
#include "frc971/autonomous/auto.q.h"
@@ -341,7 +340,8 @@
void HandleAuto() {
LOG(INFO, "Handling auto mode\n");
- double WRIST_UP;
+ const double WRIST_UP =
+ constants::GetValues().wrist_hall_effect_start_angle - 0.4;
const double WRIST_DOWN = -0.580;
const double WRIST_DOWN_TWO = WRIST_DOWN - 0.012;
const double ANGLE_ONE = 0.509;
@@ -356,15 +356,6 @@
//::aos::time::SleepFor(::aos::time::Time::InSeconds(20));
if (ShouldExitAuto()) return;
- ::aos::robot_state.FetchLatest();
- if (!::aos::robot_state.get() ||
- !constants::wrist_hall_effect_start_angle(&WRIST_UP)) {
- LOG(ERROR, "Constants not ready\n");
- return;
- }
- WRIST_UP -= 0.4;
- LOG(INFO, "Got constants\n");
-
control_loops::drivetrain.position.FetchLatest();
while (!control_loops::drivetrain.position.get()) {
LOG(WARNING, "No previous drivetrain position packet, trying to fetch again\n");
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index cd8ce9c..526904c 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -29,7 +29,7 @@
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
'<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
diff --git a/frc971/constants.cc b/frc971/constants.cc
new file mode 100644
index 0000000..97ff82b
--- /dev/null
+++ b/frc971/constants.cc
@@ -0,0 +1,147 @@
+#include "frc971/constants.h"
+
+#include <math.h>
+#include <stdint.h>
+#include <inttypes.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/once.h"
+#include "aos/common/network/team_number.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace frc971 {
+namespace constants {
+namespace {
+
+// It has about 0.029043 of gearbox slop.
+// For purposes of moving the end up/down by a certain amount, the wrist is 18
+// inches long.
+const double kCompWristHallEffectStartAngle = 1.285;
+const double kPracticeWristHallEffectStartAngle = 1.175;
+
+const double kWristHallEffectStopAngle = 100 * M_PI / 180.0;
+
+const double kPracticeWristUpperPhysicalLimit = 1.677562;
+const double kCompWristUpperPhysicalLimit = 1.677562;
+
+const double kPracticeWristLowerPhysicalLimit = -0.746128;
+const double kCompWristLowerPhysicalLimit = -0.746128;
+
+const double kPracticeWristUpperLimit = 1.615385;
+const double kCompWristUpperLimit = 1.615385;
+
+const double kPracticeWristLowerLimit = -0.746128;
+const double kCompWristLowerLimit = -0.746128;
+
+const double kWristZeroingSpeed = 0.125;
+const double kWristZeroingOffSpeed = 0.35;
+
+const int kAngleAdjustHallEffect = 2;
+
+// Angle measured from CAD with the top of the angle adjust at the top of the
+// wire guide is 0.773652098 radians.
+
+const double kCompAngleAdjustHallEffectStartAngle[2] = {0.301170496, 1.5};
+const double kPracticeAngleAdjustHallEffectStartAngle[2] = {0.305172594, 1.5};
+
+const double kAngleAdjustHallEffectStopAngle[2] = {0.1, 1.0};
+
+const double kPracticeAngleAdjustUpperPhysicalLimit = 0.904737;
+const double kCompAngleAdjustUpperPhysicalLimit = 0.904737;
+
+const double kPracticeAngleAdjustLowerPhysicalLimit = 0.270;
+const double kCompAngleAdjustLowerPhysicalLimit = 0.302;
+
+const double kPracticeAngleAdjustUpperLimit = 0.87;
+const double kCompAngleAdjustUpperLimit = 0.87;
+
+const double kPracticeAngleAdjustLowerLimit = 0.31;
+const double kCompAngleAdjustLowerLimit = 0.28;
+
+const double kAngleAdjustZeroingSpeed = -0.2;
+const double kAngleAdjustZeroingOffSpeed = -0.5;
+
+const double kPracticeAngleAdjustDeadband = 0.4;
+const double kCompAngleAdjustDeadband = 0.65;
+
+const int kCompCameraCenter = -2;
+const int kPracticeCameraCenter = -5;
+
+const int kCompDrivetrainGearboxPinion = 19;
+const int kPracticeDrivetrainGearboxPinion = 17;
+
+const ShifterHallEffect kCompLeftDriveShifter{1.5, 1, 1.2, 1.0};
+const ShifterHallEffect kCompRightDriveShifter{1.5, 1, 1.2, 1.0};
+
+const ShifterHallEffect kPracticeLeftDriveShifter{2.082283, 0.834433, 0.60,
+ 0.47};
+const ShifterHallEffect kPracticeRightDriveShifter{2.070124, 0.838993, 0.62,
+ 0.55};
+
+const Values *DoGetValues() {
+ uint16_t team = ::aos::network::GetTeamNumber();
+ LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+ switch (team) {
+ case kCompTeamNumber:
+ return new Values{kCompWristHallEffectStartAngle,
+ kWristHallEffectStopAngle,
+ kCompWristUpperLimit,
+ kCompWristLowerLimit,
+ kCompWristUpperPhysicalLimit,
+ kCompWristLowerPhysicalLimit,
+ kWristZeroingSpeed,
+ kWristZeroingOffSpeed,
+ kCompAngleAdjustHallEffectStartAngle,
+ kAngleAdjustHallEffectStopAngle,
+ kCompAngleAdjustUpperLimit,
+ kCompAngleAdjustLowerLimit,
+ kCompAngleAdjustUpperPhysicalLimit,
+ kCompAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
+ kAngleAdjustZeroingOffSpeed,
+ kCompAngleAdjustDeadband,
+ kCompDrivetrainGearboxPinion,
+ kCompLeftDriveShifter,
+ kCompRightDriveShifter,
+ kCompCameraCenter};
+ break;
+ case kPracticeTeamNumber:
+ return new Values{kPracticeWristHallEffectStartAngle,
+ kWristHallEffectStopAngle,
+ kPracticeWristUpperLimit,
+ kPracticeWristLowerLimit,
+ kPracticeWristUpperPhysicalLimit,
+ kPracticeWristLowerPhysicalLimit,
+ kWristZeroingSpeed,
+ kWristZeroingOffSpeed,
+ kPracticeAngleAdjustHallEffectStartAngle,
+ kAngleAdjustHallEffectStopAngle,
+ kPracticeAngleAdjustUpperLimit,
+ kPracticeAngleAdjustLowerLimit,
+ kPracticeAngleAdjustUpperPhysicalLimit,
+ kPracticeAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
+ kAngleAdjustZeroingOffSpeed,
+ kPracticeAngleAdjustDeadband,
+ kPracticeDrivetrainGearboxPinion,
+ kPracticeLeftDriveShifter,
+ kPracticeRightDriveShifter,
+ kPracticeCameraCenter};
+ break;
+ default:
+ LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
+ }
+}
+
+} // namespace
+
+const Values &GetValues() {
+ static ::aos::Once<const Values> once(DoGetValues);
+ return *once.Get();
+}
+
+} // namespace constants
+} // namespace frc971
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
deleted file mode 100644
index 3016db5..0000000
--- a/frc971/constants.cpp
+++ /dev/null
@@ -1,316 +0,0 @@
-#include "frc971/constants.h"
-
-#include <stddef.h>
-#include <math.h>
-
-#include "aos/common/inttypes.h"
-#include "aos/common/messages/RobotState.q.h"
-#include "aos/common/logging/logging.h"
-
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-namespace frc971 {
-namespace constants {
-
-namespace {
-
-// It has about 0.029043 of gearbox slop.
-// For purposes of moving the end up/down by a certain amount, the wrist is 18
-// inches long.
-const double kCompWristHallEffectStartAngle = 1.285;
-const double kPracticeWristHallEffectStartAngle = 1.175;
-
-const double kCompWristHallEffectStopAngle = 100 * M_PI / 180.0;
-const double kPracticeWristHallEffectStopAngle = 100 * M_PI / 180.0;
-
-const double kPracticeWristUpperPhysicalLimit = 1.677562;
-const double kCompWristUpperPhysicalLimit = 1.677562;
-
-const double kPracticeWristLowerPhysicalLimit = -0.746128;
-const double kCompWristLowerPhysicalLimit = -0.746128;
-
-const double kPracticeWristUpperLimit = 1.615385;
-const double kCompWristUpperLimit = 1.615385;
-
-const double kPracticeWristLowerLimit = -0.746128;
-const double kCompWristLowerLimit = -0.746128;
-
-const double kWristZeroingSpeed = 0.125;
-const double kWristZeroingOffSpeed = 0.35;
-
-const int kAngleAdjustHallEffect = 2;
-
-// Angle measured from CAD with the top of the angle adjust at the top of the
-// wire guide is 0.773652098 radians.
-
-const double kCompAngleAdjustHallEffectStartAngle[2] = {0.301170496, 1.5};
-const double kPracticeAngleAdjustHallEffectStartAngle[2] = {0.305172594, 1.5};
-
-const double kCompAngleAdjustHallEffectStopAngle[2] = {0.1, 1.0};
-const double kPracticeAngleAdjustHallEffectStopAngle[2] = {0.1, 1.0};
-
-const double kPracticeAngleAdjustUpperPhysicalLimit = 0.904737;
-const double kCompAngleAdjustUpperPhysicalLimit = 0.904737;
-
-const double kPracticeAngleAdjustLowerPhysicalLimit = 0.270;
-const double kCompAngleAdjustLowerPhysicalLimit = 0.302;
-
-const double kPracticeAngleAdjustUpperLimit = 0.87;
-const double kCompAngleAdjustUpperLimit = 0.87;
-
-const double kPracticeAngleAdjustLowerLimit = 0.31;
-const double kCompAngleAdjustLowerLimit = 0.28;
-
-const double kAngleAdjustZeroingSpeed = -0.2;
-const double kAngleAdjustZeroingOffSpeed = -0.5;
-
-const double kPracticeAngleAdjustDeadband = 0.4;
-const double kCompAngleAdjustDeadband = 0.65;
-
-const int kCompCameraCenter = -2;
-const int kPracticeCameraCenter = -5;
-
-const int kCompDrivetrainGearboxPinion = 19;
-const int kPracticeDrivetrainGearboxPinion = 17;
-
-struct Values {
- // Wrist hall effect positive and negative edges.
- double wrist_hall_effect_start_angle;
- double wrist_hall_effect_stop_angle;
-
- // Upper and lower extreme limits of travel for the wrist.
- double wrist_upper_limit;
- double wrist_lower_limit;
-
- // Physical limits. These are here for testing.
- double wrist_upper_physical_limit;
- double wrist_lower_physical_limit;
-
- // Zeroing speed.
- double wrist_zeroing_speed;
- // Zeroing off speed.
- double wrist_zeroing_off_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;
- // Zeroing off speed.
- double angle_adjust_zeroing_off_speed;
-
- // Deadband voltage.
- double angle_adjust_deadband;
-
- int drivetrain_gearbox_pinion;
-
- // what camera_center returns
- int camera_center;
-};
-
-Values *values = NULL;
-// Attempts to retrieve a new Values instance and stores it in values if
-// necessary.
-// Returns a valid Values instance or NULL.
-const Values *GetValues() {
- // TODO(brians): Make this use the new Once construct.
- if (values == NULL) {
- LOG(INFO, "creating a Constants for team %" PRIu16 "\n",
- ::aos::robot_state->team_id);
- switch (::aos::robot_state->team_id) {
- case kCompTeamNumber:
- values = new Values{kCompWristHallEffectStartAngle,
- kCompWristHallEffectStopAngle,
- kCompWristUpperLimit,
- kCompWristLowerLimit,
- kCompWristUpperPhysicalLimit,
- kCompWristLowerPhysicalLimit,
- kWristZeroingSpeed,
- kWristZeroingOffSpeed,
- kCompAngleAdjustHallEffectStartAngle,
- kCompAngleAdjustHallEffectStopAngle,
- kCompAngleAdjustUpperLimit,
- kCompAngleAdjustLowerLimit,
- kCompAngleAdjustUpperPhysicalLimit,
- kCompAngleAdjustLowerPhysicalLimit,
- kAngleAdjustZeroingSpeed,
- kAngleAdjustZeroingOffSpeed,
- kCompAngleAdjustDeadband,
- kCompDrivetrainGearboxPinion,
- kCompCameraCenter};
- break;
- case kPracticeTeamNumber:
- values = new Values{kPracticeWristHallEffectStartAngle,
- kPracticeWristHallEffectStopAngle,
- kPracticeWristUpperLimit,
- kPracticeWristLowerLimit,
- kPracticeWristUpperPhysicalLimit,
- kPracticeWristLowerPhysicalLimit,
- kWristZeroingSpeed,
- kWristZeroingOffSpeed,
- kPracticeAngleAdjustHallEffectStartAngle,
- kPracticeAngleAdjustHallEffectStopAngle,
- kPracticeAngleAdjustUpperLimit,
- kPracticeAngleAdjustLowerLimit,
- kPracticeAngleAdjustUpperPhysicalLimit,
- kPracticeAngleAdjustLowerPhysicalLimit,
- kAngleAdjustZeroingSpeed,
- kAngleAdjustZeroingOffSpeed,
- kPracticeAngleAdjustDeadband,
- kPracticeDrivetrainGearboxPinion,
- kPracticeCameraCenter};
- break;
- default:
- LOG(ERROR, "unknown team #%" PRIu16 "\n",
- aos::robot_state->team_id);
- return NULL;
- }
- }
- return values;
-}
-
-} // namespace
-
-bool wrist_hall_effect_start_angle(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_hall_effect_start_angle;
- return true;
-}
-bool wrist_hall_effect_stop_angle(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_hall_effect_stop_angle;
- return true;
-}
-bool wrist_upper_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_upper_limit;
- return true;
-}
-
-bool wrist_lower_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_lower_limit;
- return true;
-}
-
-bool wrist_upper_physical_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_upper_physical_limit;
- return true;
-}
-
-bool wrist_lower_physical_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_lower_physical_limit;
- return true;
-}
-
-bool wrist_zeroing_speed(double *speed) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *speed = values->wrist_zeroing_speed;
- return true;
-}
-
-bool wrist_zeroing_off_speed(double *speed) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *speed = values->wrist_zeroing_off_speed;
- 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 angle_adjust_zeroing_off_speed(double *speed) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *speed = values->angle_adjust_zeroing_off_speed;
- return true;
-}
-
-bool angle_adjust_deadband(double *voltage) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *voltage = values->angle_adjust_deadband;
- return true;
-}
-
-bool drivetrain_gearbox_pinion(int *pinion) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *pinion = values->drivetrain_gearbox_pinion;
- return true;
-}
-
-bool camera_center(int *center) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *center = values->camera_center;
- return true;
-}
-
-} // namespace constants
-} // namespace frc971
diff --git a/frc971/constants.h b/frc971/constants.h
index 414d10d..da748d9 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -1,3 +1,6 @@
+#ifndef FRC971_CONSTANTS_H_
+#define FRC971_CONSTANTS_H_
+
#include <stdint.h>
namespace frc971 {
@@ -5,51 +8,73 @@
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
-//
-// All of the public functions to retrieve various values take a pointer to
-// store their output value into and assume that aos::robot_state->get() is
-// not null and is correct. They return true on success.
const uint16_t kCompTeamNumber = 5971;
const uint16_t kPracticeTeamNumber = 971;
-// Sets *angle to how many radians from horizontal to the location of interest.
-bool wrist_hall_effect_start_angle(double *angle);
-bool wrist_hall_effect_stop_angle(double *angle);
-// These are the soft stops for up and down.
-bool wrist_lower_limit(double *angle);
-bool wrist_upper_limit(double *angle);
-// These are the hard stops. Don't use these for anything but testing.
-bool wrist_lower_physical_limit(double *angle);
-bool wrist_upper_physical_limit(double *angle);
+// Contains the voltages for an analog hall effect sensor on a shifter.
+struct ShifterHallEffect {
+ double high, low;
-// Returns the speed to move the wrist at when zeroing in rad/sec
-bool wrist_zeroing_speed(double *speed);
-bool wrist_zeroing_off_speed(double *speed);
+ double clear_high, clear_low;
+};
-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);
+// This structure contains current values for all of the things that change.
+struct Values {
+ // Wrist hall effect positive and negative edges.
+ // How many radians from horizontal to the location of interest.
+ double wrist_hall_effect_start_angle;
+ double wrist_hall_effect_stop_angle;
-// Returns speed to move the angle adjust when zeroing, in rad/sec
-bool angle_adjust_zeroing_speed(double *speed);
-bool angle_adjust_zeroing_off_speed(double *speed);
+ // Upper and lower extreme limits of travel for the wrist.
+ // These are the soft stops for up and down.
+ double wrist_upper_limit;
+ double wrist_lower_limit;
-// Returns the deadband voltage to use.
-bool angle_adjust_deadband(double *voltage);
+ // Physical limits. These are here for testing.
+ double wrist_upper_physical_limit;
+ double wrist_lower_physical_limit;
-// Sets *pinion to the number of teeth on the pinion that drives the drivetrain
-// wheels.
-bool drivetrain_gearbox_pinion(int *pinion);
+ // Zeroing speed.
+ // The speed to move the wrist at when zeroing in rad/sec
+ double wrist_zeroing_speed;
+ // Zeroing off speed (in rad/sec).
+ double wrist_zeroing_off_speed;
-// Sets *center to how many pixels off center the vertical line
-// on the camera view is.
-bool camera_center(int *center);
+ // AngleAdjust hall effect positive and negative edges.
+ // These are the soft stops for up and down.
+ const double (&angle_adjust_hall_effect_start_angle)[2];
+ const double (&angle_adjust_hall_effect_stop_angle)[2];
+
+ // 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;
+
+ // The speed to move the angle adjust when zeroing, in rad/sec
+ double angle_adjust_zeroing_speed;
+ // Zeroing off speed.
+ double angle_adjust_zeroing_off_speed;
+
+ // Deadband voltage.
+ double angle_adjust_deadband;
+
+ // The number of teeth on the pinion that drives the drivetrain wheels.
+ int drivetrain_gearbox_pinion;
+
+ ShifterHallEffect left_drive, right_drive;
+
+ // How many pixels off center the vertical line
+ // on the camera view is.
+ int camera_center;
+};
+
+// Creates (once) a Values instance and returns a reference to it.
+const Values &GetValues();
} // namespace constants
} // namespace frc971
+
+#endif // FRC971_CONSTANTS_H_
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.cc b/frc971/control_loops/angle_adjust/angle_adjust.cc
index 3ecafa6..ffb98f2 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust.cc
@@ -2,7 +2,6 @@
#include <algorithm>
-#include "aos/common/messages/RobotState.q.h"
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -17,43 +16,23 @@
: aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop>(
my_angle_adjust),
zeroed_joint_(MakeAngleAdjustLoop()) {
-}
+ {
+ using ::frc971::constants::GetValues;
+ ZeroedJoint<2>::ConfigurationData config_data;
-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_off_speed(
- &config_data->zeroing_off_speed)) {
- LOG(ERROR,
- "Failed to fetch the angle adjust zeroing off speed constant.\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.lower_limit = GetValues().angle_adjust_lower_limit;
+ config_data.upper_limit = GetValues().angle_adjust_upper_limit;
+ memcpy(config_data.hall_effect_start_angle,
+ GetValues().angle_adjust_hall_effect_start_angle,
+ sizeof(config_data.hall_effect_start_angle));
+ config_data.zeroing_off_speed = GetValues().angle_adjust_zeroing_off_speed;
+ config_data.zeroing_speed = GetValues().angle_adjust_zeroing_speed;
- config_data->max_zeroing_voltage = 4.0;
- if (!constants::angle_adjust_deadband(&config_data->deadband_voltage)) {
- LOG(ERROR, "Failed to fetch the angle adjust deadband voltage constant.\n");
- return false;
+ config_data.max_zeroing_voltage = 4.0;
+ config_data.deadband_voltage = GetValues().angle_adjust_deadband;
+
+ zeroed_joint_.set_config_data(config_data);
}
- return true;
}
// Positive angle is up, and positive power is up.
@@ -69,15 +48,6 @@
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;
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.gyp b/frc971/control_loops/angle_adjust/angle_adjust.gyp
index c48ded3..323cb56 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.gyp
+++ b/frc971/control_loops/angle_adjust/angle_adjust.gyp
@@ -25,7 +25,7 @@
],
'dependencies': [
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'angle_adjust_loop',
],
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.h b/frc971/control_loops/angle_adjust/angle_adjust.h
index dfa8ad2..319e293 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.h
+++ b/frc971/control_loops/angle_adjust/angle_adjust.h
@@ -52,10 +52,6 @@
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_;
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
index 51cd641..0ea7201 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -62,17 +62,15 @@
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;
+ const double (&hall_effect_start_angle)[2] =
+ constants::GetValues().angle_adjust_hall_effect_start_angle;
+ const double (&hall_effect_stop_angle)[2] =
+ constants::GetValues().angle_adjust_hall_effect_stop_angle;
+
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
double abs_position = GetAbsolutePosition();
@@ -115,16 +113,10 @@
angle_adjust_plant_->U << last_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));
+ EXPECT_GE(constants::GetValues().angle_adjust_upper_physical_limit,
+ angle_adjust_plant_->Y(0, 0));
+ EXPECT_LE(constants::GetValues().angle_adjust_lower_physical_limit,
+ angle_adjust_plant_->Y(0, 0));
last_voltage_ = my_angle_adjust_loop_.output->voltage;
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index e925de3..2b98578 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -29,7 +29,7 @@
'dependencies': [
'drivetrain_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/aos/build/externals.gyp:libcdd',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
diff --git a/frc971/control_loops/index/index.gyp b/frc971/control_loops/index/index.gyp
index 9cfb019..014f266 100644
--- a/frc971/control_loops/index/index.gyp
+++ b/frc971/control_loops/index/index.gyp
@@ -27,7 +27,7 @@
'dependencies': [
'index_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
],
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index 41c33c1..64e3a20 100644
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -27,7 +27,7 @@
'dependencies': [
'shooter_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
],
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
index 0ca5caa..8229c93 100644
--- a/frc971/control_loops/wrist/wrist.cc
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -4,7 +4,6 @@
#include <algorithm>
-#include "aos/common/messages/RobotState.q.h"
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -17,42 +16,22 @@
WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
: aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
zeroed_joint_(MakeWristLoop()) {
-}
+ {
+ using ::frc971::constants::GetValues;
+ ZeroedJoint<1>::ConfigurationData config_data;
-bool WristMotor::FetchConstants(
- ZeroedJoint<1>::ConfigurationData *config_data) {
- if (::aos::robot_state.get() == NULL) {
- if (!::aos::robot_state.FetchNext()) {
- LOG(ERROR, "Failed to fetch robot state to get constants.\n");
- return false;
- }
- }
- 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(&config_data->upper_limit)) {
- LOG(ERROR, "Failed to fetch the wrist upper limit constant.\n");
- return false;
- }
- if (!constants::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_off_speed(&config_data->zeroing_off_speed)) {
- LOG(ERROR, "Failed to fetch the wrist zeroing off speed constant.\n");
- return false;
- }
+ config_data.lower_limit = GetValues().wrist_lower_limit;
+ config_data.upper_limit = GetValues().wrist_upper_limit;
+ config_data.hall_effect_start_angle[0] =
+ GetValues().wrist_hall_effect_start_angle;
+ config_data.zeroing_off_speed = GetValues().wrist_zeroing_off_speed;
+ config_data.zeroing_speed = GetValues().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;
+ config_data.deadband_voltage = 0.0;
- config_data->max_zeroing_voltage = 5.0;
- config_data->deadband_voltage = 0.0;
- return true;
+ zeroed_joint_.set_config_data(config_data);
+ }
}
// Positive angle is up, and positive power is up.
@@ -68,14 +47,6 @@
output->voltage = 0;
}
- // Cache the constants to avoid error handling down below.
- ZeroedJoint<1>::ConfigurationData config_data;
- if (!FetchConstants(&config_data)) {
- return;
- } else {
- zeroed_joint_.set_config_data(config_data);
- }
-
ZeroedJoint<1>::PositionData transformed_position;
ZeroedJoint<1>::PositionData *transformed_position_ptr =
&transformed_position;
diff --git a/frc971/control_loops/wrist/wrist.gyp b/frc971/control_loops/wrist/wrist.gyp
index 3beee8b..d66bf6c 100644
--- a/frc971/control_loops/wrist/wrist.gyp
+++ b/frc971/control_loops/wrist/wrist.gyp
@@ -28,7 +28,7 @@
'dependencies': [
'wrist_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
],
'export_dependent_settings': [
diff --git a/frc971/control_loops/wrist/wrist.h b/frc971/control_loops/wrist/wrist.h
index fe2601b..a3bad80 100644
--- a/frc971/control_loops/wrist/wrist.h
+++ b/frc971/control_loops/wrist/wrist.h
@@ -51,9 +51,6 @@
friend class testing::WristTest_NoWindupPositive_Test;
friend class testing::WristTest_NoWindupNegative_Test;
- // Fetches and locally caches the latest set of constants.
- bool FetchConstants(ZeroedJoint<1>::ConfigurationData *config_data);
-
// The zeroed joint to use.
ZeroedJoint<1> zeroed_joint_;
diff --git a/frc971/control_loops/wrist/wrist_lib_test.cc b/frc971/control_loops/wrist/wrist_lib_test.cc
index 4903a28..7d248ce 100644
--- a/frc971/control_loops/wrist/wrist_lib_test.cc
+++ b/frc971/control_loops/wrist/wrist_lib_test.cc
@@ -58,13 +58,6 @@
void SendPositionMessage() {
const double angle = GetPosition();
- double wrist_hall_effect_start_angle;
- ASSERT_TRUE(constants::wrist_hall_effect_start_angle(
- &wrist_hall_effect_start_angle));
- double wrist_hall_effect_stop_angle;
- ASSERT_TRUE(constants::wrist_hall_effect_stop_angle(
- &wrist_hall_effect_stop_angle));
-
::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
my_wrist_loop_.position.MakeMessage();
position->pos = angle;
@@ -72,8 +65,8 @@
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
double abs_position = GetAbsolutePosition();
- if (abs_position >= wrist_hall_effect_start_angle &&
- abs_position <= wrist_hall_effect_stop_angle) {
+ if (abs_position >= constants::GetValues().wrist_hall_effect_start_angle &&
+ abs_position <= constants::GetValues().wrist_hall_effect_stop_angle) {
position->hall_effect = true;
} else {
position->hall_effect = false;
@@ -81,11 +74,14 @@
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
- if ((last_position_ < wrist_hall_effect_start_angle ||
- last_position_ > wrist_hall_effect_stop_angle) &&
- position->hall_effect) {
+ if ((last_position_ <
+ constants::GetValues().wrist_hall_effect_start_angle ||
+ last_position_ >
+ constants::GetValues().wrist_hall_effect_stop_angle) &&
+ position->hall_effect) {
calibration_value_ =
- wrist_hall_effect_start_angle - initial_position_;
+ constants::GetValues().wrist_hall_effect_start_angle -
+ initial_position_;
}
position->calibration = calibration_value_;
@@ -99,17 +95,9 @@
wrist_plant_->U << last_voltage_;
wrist_plant_->Update();
- // Assert that we are in the right physical range.
- double wrist_upper_physical_limit;
- ASSERT_TRUE(constants::wrist_upper_physical_limit(
- &wrist_upper_physical_limit));
- double wrist_lower_physical_limit;
- ASSERT_TRUE(constants::wrist_lower_physical_limit(
- &wrist_lower_physical_limit));
-
- EXPECT_GE(wrist_upper_physical_limit,
+ EXPECT_GE(constants::GetValues().wrist_upper_physical_limit,
wrist_plant_->Y(0, 0));
- EXPECT_LE(wrist_lower_physical_limit,
+ EXPECT_LE(constants::GetValues().wrist_lower_physical_limit,
wrist_plant_->Y(0, 0));
last_voltage_ = my_wrist_loop_.output->voltage;
}
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index 597abee..6d5202c 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -1,14 +1,15 @@
{
'targets': [
{
- 'target_name': 'common',
+ 'target_name': 'constants',
'type': 'static_library',
'sources': [
- 'constants.cpp',
+ 'constants.cc',
],
'dependencies': [
'<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/common/common.gyp:once',
+ '<(AOS)/common/network/network.gyp:team_number',
],
}
],
diff --git a/frc971/input/gyro_board_data.h b/frc971/input/gyro_board_data.h
index fb47642..94200c9 100644
--- a/frc971/input/gyro_board_data.h
+++ b/frc971/input/gyro_board_data.h
@@ -1,6 +1,8 @@
#ifndef FRC971_INPUT_GYRO_BOARD_DATA_H_
#define FRC971_INPUT_GYRO_BOARD_DATA_H_
+#include <stdint.h>
+
namespace frc971 {
#define DATA_STRUCT_NAME GyroBoardData
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 5f0ce57..c554173 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -1,3 +1,5 @@
+#include <inttypes.h>
+
#include "aos/atom_code/init.h"
#include "aos/common/logging/logging.h"
#include "aos/common/util/wrapping_counter.h"
@@ -9,6 +11,7 @@
#include "frc971/control_loops/shooter/shooter_motor.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/input/usb_receiver.h"
+#include "frc971/constants.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -25,61 +28,84 @@
namespace frc971 {
namespace {
-inline double drivetrain_translate(int32_t in) {
+double drivetrain_translate(int32_t in) {
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
(19.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/ *
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
-inline double wrist_translate(int32_t in) {
+double wrist_translate(int32_t in) {
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
(14.0 / 50.0 * 20.0 / 84.0) /*gears*/ * (2 * M_PI);
}
-inline double angle_adjust_translate(int32_t in) {
+double angle_adjust_translate(int32_t in) {
static const double kCableDiameter = 0.060;
return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
((0.75 + kCableDiameter) / (16.61125 + kCableDiameter)) /*pulleys*/ *
(2 * M_PI);
}
-inline double shooter_translate(int32_t in) {
+double shooter_translate(int32_t in) {
return static_cast<double>(in) / (32.0 /*cpr*/ * 4.0 /*quad*/) *
(15.0 / 34.0) /*gears*/ * (2 * M_PI);
}
-inline double index_translate(int32_t in) {
+double index_translate(int32_t in) {
return -static_cast<double>(in) / (128.0 /*cpr*/ * 4.0 /*quad*/) *
(1.0) /*gears*/ * (2 * M_PI);
}
// Translates values from the ADC into voltage.
-inline double adc_translate(uint16_t in) {
+double adc_translate(uint16_t in) {
static const double kVRefN = 0;
static const double kVRefP = 3.3;
- static const int kMaximumValue = 0x3FF;
- return kVRefN +
+ static const int kMaximumValue = 0xFFF;
+ static const double kDividerGnd = 31.6, kDividerOther = 28;
+ return (kVRefN +
(static_cast<double>(in) / static_cast<double>(kMaximumValue) *
- (kVRefP - kVRefN));
+ (kVRefP - kVRefN))) * (kDividerGnd + kDividerOther) / kDividerGnd;
+}
+
+double gyro_translate(int64_t in) {
+ return in / 16.0 / 1000.0 / (180.0 / M_PI);
+}
+
+double battery_translate(uint16_t in) {
+ static const double kDividerBig = 98.9, kDividerSmall = 17.8;
+ return adc_translate(in) * kDividerBig / kDividerSmall;
+}
+
+double hall_translate(const constants::ShifterHallEffect &k, uint16_t in) {
+ const double voltage = adc_translate(in);
+ return (voltage - k.low) / (k.high - k.low);
}
} // namespace
class GyroSensorReceiver : public USBReceiver {
- virtual void ProcessData() override {
- if (data()->robot_id != 2) {
- LOG(ERROR, "gyro board sent data for robot id %hhd!"
- " dip switches are %x\n",
- data()->robot_id, data()->base_status & 0xF);
- return;
- } else {
- LOG(DEBUG, "processing a packet dip switches %x\n",
- data()->base_status & 0xF);
- }
+ public:
+ GyroSensorReceiver() : USBReceiver(2) {}
- gyro.MakeWithBuilder()
- .angle(data()->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
- .Send();
+ virtual void PacketReceived(const ::aos::time::Time &/*timestamp*/) override {
+ if (data()->bad_gyro) {
+ LOG(ERROR, "bad gyro\n");
+ bad_gyro_ = true;
+ gyro.MakeWithBuilder().angle(0).Send();
+ } else if (data()->old_gyro_reading) {
+ LOG(WARNING, "old gyro reading\n");
+ bad_gyro_ = true;
+ } else {
+ bad_gyro_ = false;
+ }
+ }
+
+ virtual void ProcessData(const ::aos::time::Time &/*timestamp*/) override {
+ if (!bad_gyro_) {
+ gyro.MakeWithBuilder()
+ .angle(gyro_translate(data()->gyro_angle))
+ .Send();
+ }
drivetrain.position.MakeWithBuilder()
.right_encoder(drivetrain_translate(data()->main.right_drive))
@@ -127,8 +153,25 @@
.loader_top(data()->main.loader_top)
.loader_bottom(data()->main.loader_bottom)
.Send();
+
+ LOG(DEBUG, "battery %f %f %" PRIu16 "\n",
+ battery_translate(data()->main.battery_voltage),
+ adc_translate(data()->main.battery_voltage),
+ data()->main.battery_voltage);
+ LOG(DEBUG, "halls l=%f r=%f %" PRIx16 " %" PRIx16 "\n",
+ adc_translate(data()->main.left_drive_hall),
+ adc_translate(data()->main.right_drive_hall),
+ data()->main.left_drive_hall,
+ data()->main.right_drive_hall);
+ LOG(DEBUG, "real l=%f r=%f\n",
+ hall_translate(constants::GetValues().left_drive,
+ data()->main.left_drive_hall),
+ hall_translate(constants::GetValues().right_drive,
+ data()->main.right_drive_hall));
}
+ bool bad_gyro_;
+
WrappingCounter top_rise_;
WrappingCounter top_fall_;
WrappingCounter bottom_rise_;
@@ -139,7 +182,7 @@
} // namespace frc971
int main() {
- ::aos::Init();
+ ::aos::Init(frc971::USBReceiver::kRelativePriority);
::frc971::GyroSensorReceiver receiver;
while (true) {
receiver.RunIteration();
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 23dfda6..8d5ad90 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -38,6 +38,7 @@
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/util/util.gyp:wrapping_counter',
'usb_receiver',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
],
},
{
@@ -56,6 +57,13 @@
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
'<(AOS)/common/common.gyp:time',
],
+ 'variables': {
+ # TODO(brians): Add dependency on this file too (or something).
+ 'checksum': '<!(<(DEPTH)/gyro_board/src/usb/data_struct_checksum.sh)',
+ },
+ 'defines': [
+ 'GYRO_BOARD_DATA_CHECKSUM=<(checksum)',
+ ],
},
],
}
diff --git a/frc971/input/usb_receiver.cc b/frc971/input/usb_receiver.cc
index bd2e665..06eb7df 100644
--- a/frc971/input/usb_receiver.cc
+++ b/frc971/input/usb_receiver.cc
@@ -9,7 +9,8 @@
namespace frc971 {
-USBReceiver::USBReceiver() {
+USBReceiver::USBReceiver(uint8_t expected_robot_id)
+ : expected_robot_id_(expected_robot_id) {
Reset();
}
@@ -17,10 +18,29 @@
if (ReceiveData()) {
Reset();
} else {
- const ::aos::time::Time received_time = ::aos::time::Time::Now();
- if (phase_locker_.IsCurrentPacketGood(received_time, sequence_)) {
- LOG(DEBUG, "processing data %" PRIu32 "\n", sequence_);
- ProcessData();
+ const ::aos::time::Time timestamp =
+ ::aos::time::Time::InMS(data()->frame_number);
+
+ if (data()->robot_id != expected_robot_id_) {
+ LOG(ERROR, "gyro board sent data for robot id %hhd instead of %hhd!"
+ " dip switches are %hhx\n",
+ data()->robot_id, expected_robot_id_, data()->dip_switches);
+ return;
+ }
+ if (data()->checksum != GYRO_BOARD_DATA_CHECKSUM) {
+ LOG(ERROR,
+ "gyro board sent checksum %" PRIu16 " instead of %" PRIu16 "!\n",
+ data()->checksum, GYRO_BOARD_DATA_CHECKSUM);
+ return;
+ }
+
+ PacketReceived(timestamp);
+
+ if (phase_locker_.IsCurrentPacketGood(transfer_received_time_, frame_number_)) {
+ LOG(DEBUG, "processing dips %hhx frame %" PRId32 " at %f\n",
+ data()->dip_switches, data()->frame_number, timestamp.ToSeconds());
+
+ ProcessData(timestamp);
}
}
}
@@ -34,6 +54,9 @@
good_phase_early_ = good_phase_late_ = 0;
}
+void USBReceiver::PacketReceived(const ::aos::time::Time &/*timestamp*/) {}
+void USBReceiver::ProcessData(const ::aos::time::Time &/*timestamp*/) {}
+
bool USBReceiver::PhaseLocker::IsCurrentPacketGood(
const ::aos::time::Time &received_time,
uint32_t sequence) {
@@ -48,11 +71,6 @@
Reset();
return false;
}
- if (sequence < last_good_sequence_) {
- LOG(WARNING, "sequence went down. gyro board reset?\n");
- Reset();
- return false;
- }
using ::aos::control_loops::kLoopFrequency;
// How often we (should) receive packets.
@@ -163,8 +181,8 @@
}
void USBReceiver::TransferCallback(libusb::Transfer *transfer) {
+ transfer_received_time_ = ::aos::time::Time::Now();
if (transfer->status() == LIBUSB_TRANSFER_COMPLETED) {
- LOG(DEBUG, "transfer %p completed\n", transfer);
completed_transfer_ = transfer;
} else if (transfer->status() == LIBUSB_TRANSFER_TIMED_OUT) {
LOG(WARNING, "transfer %p timed out\n", transfer);
@@ -199,14 +217,27 @@
memcpy(data(), completed_transfer_->data(),
sizeof(GyroBoardData));
- uint32_t sequence_before = sequence_;
- sequence_ = data()->sequence;
- if (sequence_before == 0) {
- LOG(INFO, "count starting at %" PRIu32 "\n", sequence_);
- } else if (sequence_ - sequence_before != 1) {
- LOG(WARNING, "count went from %" PRIu32" to %" PRIu32 "\n",
- sequence_before, sequence_);
+ if (data()->unknown_frame) {
+ LOG(WARNING, "unknown frame number\n");
+ return true;
}
+ uint32_t frame_number_before = frame_number_;
+ frame_number_ = data()->frame_number;
+ if (frame_number_ < 0) {
+ LOG(WARNING, "negative frame number %" PRId32 "\n", frame_number_);
+ return true;
+ }
+ if (frame_number_before == 0) {
+ LOG(INFO, "frame number starting at %" PRId32 "\n", frame_number_);
+ } else if (frame_number_ - frame_number_before != 1) {
+ LOG(WARNING, "frame number went from %" PRId32" to %" PRId32 "\n",
+ frame_number_before, frame_number_);
+ }
+ if (frame_number_ < last_frame_number_ - 2) {
+ LOG(WARNING, "frame number went down a lot\n");
+ return true;
+ }
+ last_frame_number_ = frame_number_;
return false;
}
@@ -230,7 +261,7 @@
c->Submit();
}
- sequence_ = 0;
+ last_frame_number_ = frame_number_ = 0;
phase_locker_.Reset();
}
diff --git a/frc971/input/usb_receiver.h b/frc971/input/usb_receiver.h
index a0c645a..fec847f 100644
--- a/frc971/input/usb_receiver.h
+++ b/frc971/input/usb_receiver.h
@@ -4,6 +4,7 @@
#include <memory>
#include "aos/common/time.h"
+#include "aos/common/macros.h"
#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/input/gyro_board_data.h"
@@ -14,10 +15,14 @@
// us.
class USBReceiver {
public:
- USBReceiver();
+ USBReceiver(uint8_t expected_robot_id);
void RunIteration();
+ // The relative priority that tasks doing this should get run at (ie what to
+ // pass to ::aos::Init(int)).
+ static const int kRelativePriority = 5;
+
protected:
GyroBoardData *data() { return &data_; }
@@ -97,18 +102,36 @@
void Reset();
- virtual void ProcessData() = 0;
+ // These 2 are the functions for subclasses to override and do stuff in.
+ // timestamp for both of them is the time (as best as this code can determine)
+ // that the values in the packet were captured.
+ // They both have empty implementations here for subclasses that don't want to
+ // do anything in one of them.
+
+ // Gets called after each packet is received (possibly before ProcessData for
+ // the same packet).
+ virtual void PacketReceived(const ::aos::time::Time ×tamp);
+ // Gets called every 10th packet (or so) (at the right time for data for
+ // control loops to get read). PacketReceived will always be called right
+ // before this.
+ virtual void ProcessData(const ::aos::time::Time ×tamp);
+
+ const uint8_t expected_robot_id_;
GyroBoardData data_;
- uint32_t sequence_;
+ int32_t last_frame_number_, frame_number_;
LibUSB libusb_;
::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
::std::unique_ptr<libusb::IsochronousTransfer> transfers_[kNumTransfers];
- // "Temporary" variable for holding a completed transfer to communicate that
- // information from the callback to the code that wants it.
+
+ // "Temporary" variables for communicating information about a transfer that
+ // finished from the callback to the rest of the code.
libusb::Transfer *completed_transfer_;
+ ::aos::time::Time transfer_received_time_{0, 0};
+
+ DISALLOW_COPY_AND_ASSIGN(USBReceiver);
};
} // namespace frc971
diff --git a/frc971/output/CameraServer.cc b/frc971/output/CameraServer.cc
index 939731d..93a83c7 100644
--- a/frc971/output/CameraServer.cc
+++ b/frc971/output/CameraServer.cc
@@ -3,7 +3,6 @@
#include "aos/atom_code/output/HTTPServer.h"
#include "aos/atom_code/output/evhttp_ctemplate_emitter.h"
#include "aos/atom_code/output/ctemplate_cache.h"
-#include "aos/common/messages/RobotState.q.h"
#include "ctemplate/template.h"
#include "aos/atom_code/init.h"
#include "aos/common/logging/logging.h"
@@ -55,17 +54,7 @@
// after it.
dict.SetValue("HOST", ctemplate::TemplateString(host, length));
- if (!aos::robot_state.FetchLatest()) {
- LOG(WARNING, "getting a RobotState message failed\n");
- evhttp_send_error(request, HTTP_INTERNAL, NULL);
- return;
- }
- int center;
- if (!constants::camera_center(¢er)) {
- evhttp_send_error(request, HTTP_INTERNAL, NULL);
- return;
- }
- dict.SetIntValue("CENTER", center);
+ dict.SetIntValue("CENTER", constants::GetValues().camera_center);
aos::http::EvhttpCtemplateEmitter emitter(buf_);
if (!aos::http::get_template_cache()->
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 90fccb2..1a8b1c9 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -8,10 +8,9 @@
],
'dependencies': [
'<(AOS)/atom_code/output/output.gyp:http_server',
- '../frc971.gyp:common',
+ '../frc971.gyp:constants',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/messages/messages.gyp:aos_queues',
'<(AOS)/atom_code/atom_code.gyp:configuration',
],
'copies': [
diff --git a/gyro_board/schematic/.gitignore b/gyro_board/schematic/.gitignore
new file mode 100644
index 0000000..d6b9d75
--- /dev/null
+++ b/gyro_board/schematic/.gitignore
@@ -0,0 +1,2 @@
+*.b#*
+*.s#*
diff --git a/gyro_board/src/usb/CAN.c b/gyro_board/src/usb/CAN.c
index 35f811f..61e7f47 100644
--- a/gyro_board/src/usb/CAN.c
+++ b/gyro_board/src/usb/CAN.c
@@ -231,10 +231,6 @@
vTaskDelete(NULL);
}
- // Set all of the CAN stuff to run at CCLK/6, which translates to about 1 Mbit
- // (1.042).
- SC->PCLKSEL0 |= 3 << 26 | 3 << 28 | 3 << 30;
-
// Enable CAN
SC->PCONP |= PCONP_PCCAN1;
@@ -279,6 +275,11 @@
}
}
+void CAN_PCLKSEL(void) {
+ // Set all of the CAN stuff to run at CCLK/6, which translates to about 1 Mbit
+ // (1.042).
+ SC->PCLKSEL0 |= 3 << 26 | 3 << 28 | 3 << 30;
+}
void initCAN(void){
xTaskCreate(vCAN1, (signed char *) "CAN1rx", configMINIMAL_STACK_SIZE + 400, NULL, tskIDLE_PRIORITY + 1, NULL);
diff --git a/gyro_board/src/usb/CAN.h b/gyro_board/src/usb/CAN.h
index 1ef39d5..60f2d10 100644
--- a/gyro_board/src/usb/CAN.h
+++ b/gyro_board/src/usb/CAN.h
@@ -14,6 +14,9 @@
} can_message;
int CAN_get(can_message *message);
+
+// Sets up PCLKSEL for CAN stuff (errata PCLKSELx.1).
+void CAN_PCLKSEL(void);
void initCAN(void);
#endif
diff --git a/gyro_board/src/usb/LPCUSB/usbhw_lpc.c b/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
index 2fa038e..e39a636 100644
--- a/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
+++ b/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
@@ -59,7 +59,7 @@
@param [in] dwIntr Bitmask of interrupts to wait for
*/
-static void Wait4DevInt(unsigned long dwIntr)
+void Wait4DevInt(unsigned long dwIntr)
{
// wait for specific interrupt
while ((USB->USBDevIntSt & dwIntr) != dwIntr);
@@ -107,7 +107,7 @@
@return the data
*/
-static unsigned char USBHwCmdRead(unsigned char bCmd)
+unsigned char USBHwCmdRead(unsigned char bCmd)
{
// write command code
USBHwCmd(bCmd);
diff --git a/gyro_board/src/usb/Makefile b/gyro_board/src/usb/Makefile
index 2dc2286..e2f81f4 100644
--- a/gyro_board/src/usb/Makefile
+++ b/gyro_board/src/usb/Makefile
@@ -44,18 +44,20 @@
CAN.c \
LPCUSB/usbinit.c \
LPCUSB/usbcontrol.c \
- LPCUSB/USB_SENSOR_STREAM.c \
+ usb_device.c \
LPCUSB/usbhw_lpc.c \
gyro.c \
LPCUSB/usbstdreq.c
+DATA_STRUCT_CHECKSUM=$(shell ./data_struct_checksum.sh)
+
all: $(NAME).hex
$(NAME).elf: Makefile $(SOURCES:.c=.o) $(LDSCRIPT)
$(CC) $(CFLAGS) -nostartfiles -nostdlib -T $(LDSCRIPT) -o $@ -L$(TOOLS_PREFIX)/lib/gcc/arm-eabi/4.5.1/ $(SOURCES:.c=.o) -Wa,-Map -Wa,main.map -lgcc
-%.o: %.c Makefile
- $(CC) $(CFLAGS) -nostartfiles -nostdlib -c -o $@ $< -Wall -std=gnu99
+%.o: %.c Makefile data_struct_checksum.sh
+ $(CC) $(CFLAGS) -nostartfiles -nostdlib -c -o $@ $< -Wall -std=gnu99 -DDATA_STRUCT_CHECKSUM=$(DATA_STRUCT_CHECKSUM)
run: deploy
$(FLASHER) -termonly -control $(NAME).hex $(PORT) $(SPEED) $(OSC)
diff --git a/gyro_board/src/usb/analog.c b/gyro_board/src/usb/analog.c
index b00d1b4..7968094 100644
--- a/gyro_board/src/usb/analog.c
+++ b/gyro_board/src/usb/analog.c
@@ -1,40 +1,81 @@
#include "analog.h"
#include "LPC17xx.h"
+#include "FreeRTOS.h"
+
+static int discarded_samples[4];
+
+static uint16_t raw_analog(int channel) {
+ uint32_t value;
+ switch (channel) {
+ case 0:
+ value = ADC->ADDR0;
+ break;
+ case 1:
+ value = ADC->ADDR1;
+ break;
+ case 2:
+ value = ADC->ADDR2;
+ break;
+ case 3:
+ value = ADC->ADDR3;
+ break;
+ }
+
+ return (value >> 4) & 0xFFF;
+}
+
+void TIMER1_IRQHandler(void) {
+ TIM1->IR = 1 << 0; // clear channel 0 match
+
+ static const int kBadSampleThreshold = 175;
+ static const int kMaxBadSamples = 4;
+
+ static const uint32_t kBitShift = 16;
+ static const uint32_t kA =
+ (1.0 - 0.8408964152537146 /*0.5^0.25*/) * (1 << kBitShift) + 0.5;
+ for (int i = 0; i < 4; ++i) {
+ uint16_t current = raw_analog(i);
+ uint16_t average = averaged_values[i];
+ if ((current - average) < -kBadSampleThreshold ||
+ (current - average) > kBadSampleThreshold) {
+ ++discarded_samples[i];
+ if (discarded_samples[i] >= kMaxBadSamples) {
+ discarded_samples[i] = 0;
+ averaged_values[i] = current;
+ }
+ } else {
+ discarded_samples[i] = 0;
+ averaged_values[i] =
+ ((uint32_t)current * kA +
+ (uint32_t)average * ((1 << kBitShift) - kA)) >> kBitShift;
+ }
+ }
+}
void analog_init(void) {
SC->PCONP |= PCONP_PCAD;
- // Enable AD0.0, AD0.1, AD0.2, AD0.3
- PINCON->PINSEL1 &= ~(2 << 14 | 2 << 16 | 2 << 18 | 2 << 20);
+ // Enable AD0.0, AD0.1, AD0.2, and AD0.3 (0.23 - 0.26).
+ PINCON->PINSEL1 &= ~(3 << 14 | 3 << 16 | 3 << 18 | 3 << 20);
PINCON->PINSEL1 |= 1 << 14 | 1 << 16 | 1 << 18 | 1 << 20;
- ADC->ADCR = 0x00200500;
+ PINCON->PINMODE1 &= ~(3 << 14 | 3 << 16 | 3 << 18 | 3 << 20);
+ PINCON->PINMODE1 |= 2 << 14 | 2 << 16 | 2 << 18 | 2 << 20;
+
ADC->ADCR = (1 << 0 | 1 << 1 | 1 << 2 | 1 << 3) /* enable all 4 */ |
- 7 << 8 /* 100MHz / 8 = 12.5MHz */ |
+ 79 << 8 /* takes 208us to scan through all 4 */ |
1 << 16 /* enable burst mode */ |
1 << 21 /* turn on ADC */;
-}
-uint16_t analog(int channel) {
- uint32_t value;
- do {
- switch (channel) {
- case 0:
- value = ADC->ADDR0;
- break;
- case 1:
- value = ADC->ADDR1;
- break;
- case 2:
- value = ADC->ADDR2;
- break;
- case 3:
- value = ADC->ADDR3;
- break;
- default:
- return 0xFFFF;
- }
- } while (!(value & 1 << 31));
-
- return ((value & 0xFFF0) >> 4);
+ // Set up the timer for the low-pass filter.
+ SC->PCONP |= 1 << 2;
+ TIM1->PR = (configCPU_CLOCK_HZ / 2000) - 1;
+ TIM1->TC = 0; // don't match the first time around
+ TIM1->MR0 = 1; // match every time it wraps
+ TIM1->MCR = 1 << 0 | 1 << 1; // interrupt and reset on match channel 0
+ // Priority 4 is higher than any FreeRTOS-managed stuff (ie USB), but lower
+ // than encoders etc.
+ NVIC_SetPriority(TIMER1_IRQn, 4);
+ NVIC_EnableIRQ(TIMER1_IRQn);
+ TIM1->TCR = 1; // enable it
}
diff --git a/gyro_board/src/usb/analog.h b/gyro_board/src/usb/analog.h
index 5a06290..acbf679 100644
--- a/gyro_board/src/usb/analog.h
+++ b/gyro_board/src/usb/analog.h
@@ -3,13 +3,22 @@
#include <stdint.h>
+// Internal variable for holding the averaged value. USE analog TO GET TO THIS
+// IN CASE IT CHANGES!
+uint16_t averaged_values[4];
+
// Starts the hardware constantly doing conversions on all 4 of our analog
// inputs.
void analog_init(void);
// Retrieves the most recent reading on channel (0-3).
// Returns 0xFFFF for invalid channel.
-// 0 means 0V and 0x3FF means 3.3V.
-uint16_t analog(int channel);
+// 0 means 0V and 0xFFF means 3.3V.
+// These values are run through a low-pass filter with unreasonable readings
+// discarded first.
+uint16_t analog(int channel) {
+ if (channel < 0 || channel > 3) return 0xFFFF;
+ return averaged_values[channel];
+}
#endif // __ANALOG_H__
diff --git a/gyro_board/src/usb/data_struct.h b/gyro_board/src/usb/data_struct.h
index 8c069b9..975084a 100644
--- a/gyro_board/src/usb/data_struct.h
+++ b/gyro_board/src/usb/data_struct.h
@@ -3,6 +3,7 @@
// guards.
// This means that it can not #include anything else because it (sometimes) gets
// #included inside a namespace.
+// <stdint.h> must be #included by the containing file.
// In the gyro board code, fill_packet.h #includes this file.
// In the fitpc code, frc971/input/gyro_board_data.h #includes this file.
@@ -14,6 +15,29 @@
union {
struct {
+ // This is the USB frame number for this data. It (theoretically) gets
+ // incremented on every packet sent, but the gyro board will deal with it
+ // correctly if it misses a frame or whatever by tracking the frame
+ // numbers sent out by the host.
+ // Negative numbers mean that the gyro board has no idea what the right
+ // answer is.
+ // This value going down at all indicates that the code on the gyro board
+ // dealing with it reset.
+ //
+ // The USB 2.0 standard says that timing of frames is 1.000ms +- 500ns.
+ // Testing with a fitpc and gyro board on 2013-10-30 by Brian gave 10us
+ // (the resolution of the timer on the gyro board that was used) of drift
+ // every 90-130 frames (~100ns per frame) and no jitter (and the timer on
+ // the gyro board isn't necessarily that good). This is plenty accurate
+ // for what we need for timing, so this number is what the code uses to do
+ // all timing calculations.
+ int32_t frame_number;
+
+ // Checksum of this file calculated with sum(1).
+ // The gyro board sets this and then the fitpc checks it to make sure that
+ // they're both using the same version of this file.
+ uint16_t checksum;
+
// Which robot (+version) the gyro board is sending out data for.
// We should keep this in the same place for all gyro board software
// versions so that the fitpc can detect when it's reading from a gyro
@@ -36,23 +60,26 @@
uint8_t dip_switch1 : 1;
uint8_t dip_switch2 : 1;
uint8_t dip_switch3 : 1;
- // If the current gyro_angle has been not updated because of a bad
- // reading from the sensor.
- uint8_t old_gyro_reading : 1;
- // If we're not going to get any more good gyro_angles.
- uint8_t bad_gyro : 1;
};
- uint8_t base_status;
+ uint8_t dip_switches;
+ };
+ struct {
+ // If the current gyro_angle has been not updated because of a bad
+ // reading from the sensor.
+ uint8_t old_gyro_reading : 1;
+ // If we're not going to get any more good gyro_angles.
+ uint8_t bad_gyro : 1;
+
+ // We're not sure what frame number this packet was sent in.
+ uint8_t unknown_frame : 1;
};
};
- uint32_t header;
+ struct {
+ uint64_t header0, header1;
+ };
};
- // This is a counter that gets incremented with each packet sent.
- uint32_t sequence;
-
- // We are 64-bit aligned at this point if it matters for anything other than
- // the gyro angle.
+ // We are 64-bit aligned at this point.
union {
struct {
diff --git a/gyro_board/src/usb/data_struct_checksum.sh b/gyro_board/src/usb/data_struct_checksum.sh
new file mode 100755
index 0000000..147a365
--- /dev/null
+++ b/gyro_board/src/usb/data_struct_checksum.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+sum $(dirname $0)/data_struct.h | sed 's/^\([0-9]*\) .*$/\1/'
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
index 9277340..c9155d4 100644
--- a/gyro_board/src/usb/encoder.c
+++ b/gyro_board/src/usb/encoder.c
@@ -383,6 +383,8 @@
}
}
+static volatile uint32_t sensor_timing_wraps = 0;
+
void encoder_init(void) {
// Setup the encoder interface.
SC->PCONP |= PCONP_PCQEI;
@@ -485,6 +487,8 @@
packet->bad_gyro = 0;
}
+ packet->checksum = DATA_STRUCT_CHECKSUM;
+
packet->dip_switch0 = dip_switch(0);
packet->dip_switch1 = dip_switch(1);
packet->dip_switch2 = dip_switch(2);
@@ -505,9 +509,6 @@
packet->main.left_drive = encoder5_val;
packet->main.right_drive = encoder4_val;
packet->main.indexer = encoder3_val;
- packet->main.battery_voltage = analog(0);
- packet->main.left_drive_hall = analog(1);
- packet->main.right_drive_hall = analog(3);
NVIC_DisableIRQ(EINT3_IRQn);
@@ -531,6 +532,7 @@
packet->main.capture_bottom_fall_delay = capture_bottom_fall_delay;
packet->main.bottom_fall_delay_count = bottom_fall_delay_count;
packet->main.bottom_fall_count = bottom_fall_count;
+ packet->main.bottom_rise_count = bottom_rise_count;
packet->main.bottom_disc = !digital(1);
NVIC_EnableIRQ(EINT3_IRQn);
@@ -549,6 +551,10 @@
NVIC_EnableIRQ(EINT3_IRQn);
- packet->main.bottom_rise_count = bottom_rise_count;
+ // Do all of the analogs last because they have the potential to be slow and
+ // jittery.
+ packet->main.battery_voltage = analog(1);
+ packet->main.left_drive_hall = analog(3);
+ packet->main.right_drive_hall = analog(2);
}
}
diff --git a/gyro_board/src/usb/gyro.c b/gyro_board/src/usb/gyro.c
index ad49658..40734ff 100644
--- a/gyro_board/src/usb/gyro.c
+++ b/gyro_board/src/usb/gyro.c
@@ -187,10 +187,6 @@
static portTASK_FUNCTION(gyro_read_task, pvParameters) {
SC->PCONP |= PCONP_PCSSP0;
- // Make sure that the clock is set up right.
- SC->PCLKSEL1 &= ~(3 << 10);
- SC->PCLKSEL1 |= 1 << 10;
-
// Set up SSEL.
// It's is just a GPIO pin because we're the master (it would be special if we
// were a slave).
diff --git a/gyro_board/src/usb/main.c b/gyro_board/src/usb/main.c
index 4e74ce7..e186bba 100644
--- a/gyro_board/src/usb/main.c
+++ b/gyro_board/src/usb/main.c
@@ -149,14 +149,19 @@
setup_PLL1();
- // Set everything to run at full CCLK by default.
- SC->PCLKSEL0 = 0x55555555;
-
/* Configure the LEDs. */
vParTestInitialise();
}
int main(void) {
+ // Errata PCLKSELx.1 says that we have to do all of this BEFORE setting up
+ // PLL0 or it might not work.
+
+ // Set everything to run at full CCLK by default.
+ SC->PCLKSEL0 = 0x55555555;
+
+ CAN_PCLKSEL();
+
setup_hardware();
digital_init();
@@ -203,7 +208,6 @@
/* Power up and feed the timer. */
SC->PCONP |= 0x02UL;
- SC->PCLKSEL0 = (SC->PCLKSEL0 & (~(0x3 << 2))) | (0x01 << 2);
/* Reset Timer 0 */
TIM0->TCR = TCR_COUNT_RESET;
diff --git a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c b/gyro_board/src/usb/usb_device.c
similarity index 64%
rename from gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
rename to gyro_board/src/usb/usb_device.c
index 7265393..c90bd0d 100644
--- a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
+++ b/gyro_board/src/usb/usb_device.c
@@ -32,9 +32,19 @@
#include <stdio.h>
#include <string.h>
-#include "usbapi.h"
-#include "usbdebug.h"
-#include "usbstruct.h"
+#include "LPCUSB/usbapi.h"
+#include "LPCUSB/usbdebug.h"
+#include "LPCUSB/usbstruct.h"
+
+// This file is marked private and most of the functions in its associated .c
+// file started out static, but we want to use some of them to do frame handling
+// stuff because we do special stuff with it (handle it ourselves for reduced
+// jitter and actually deal with the frame number correctly), so it's nice to
+// have the utility functions for accessing the hardware available instead of
+// having to rewrite them.
+#include "LPCUSB/usbhw_lpc.h"
+unsigned char USBHwCmdRead(unsigned char bCmd);
+void Wait4DevInt(unsigned long dwIntr);
#include "LPC17xx.h"
@@ -55,8 +65,6 @@
#define LE_WORD(x) ((x)&0xFF),((x)>>8)
-static struct DataStruct usbPacket;
-
static xQueueHandle xRxedChars = NULL, xCharsForTx = NULL;
// This gets cleared each time the ISR is entered and then checked as it's
@@ -144,6 +152,12 @@
0
};
+// Enables interrupts to write data instead of NAKing on the bulk in endpoints.
+// This is in a centralized place so that other NAK interrupts can be enabled
+// all of the time easily in the future.
+static void bulk_in_nak_int(int have_data) {
+ USBHwNakIntEnable(have_data ? INACK_BI : 0);
+}
/**
* Local function to handle incoming bulk data
@@ -179,8 +193,8 @@
(void) bEPStatus;
if (uxQueueMessagesWaitingFromISR(xCharsForTx) == 0) {
- // no more data, disable further NAK interrupts until next USB frame
- USBHwNakIntEnable(INACK_II);
+ // no more data
+ bulk_in_nak_int(0);
return;
}
@@ -245,31 +259,94 @@
return -1;
}
-/**
- * Interrupt handler
- *
- * Simply calls the USB ISR
- */
-void USB_IRQHandler(void) {
- higher_priority_task_woken = pdFALSE;
- USBHwISR();
- portEND_SWITCHING_ISR(higher_priority_task_woken);
-}
+// Instead of registering an lpcusb handler for this, we do it ourself so that
+// we can get the timing jitter down and deal with the frame number right.
+static void HandleFrame(void) {
+ USB->USBDevIntClr = FRAME;
-static void USBFrameHandler(unsigned short wFrame) {
- (void) wFrame;
- if (uxQueueMessagesWaitingFromISR(xCharsForTx) > 0) {
- // Data to send is available so enable interrupt instead of NAK on bulk in
- // too.
- USBHwNakIntEnable(INACK_BI | INACK_II);
+ static struct DataStruct sensor_values;
+ fillSensorPacket(&sensor_values);
+
+ // What the last good frame number that we got was.
+ // Values <0 are uninitialized.
+ static int32_t current_frame = -1;
+ // How many extra frames we're guessing happened since we got a good one.
+ static int guessed_frames = 0;
+
+ uint8_t error_status = USBHwCmdRead(CMD_DEV_READ_ERROR_STATUS);
+ if (error_status & PID_ERR) {
+ ++guessed_frames;
} else {
- USBHwNakIntEnable(INACK_II);
+ int16_t read_frame = USBHwCmdRead(CMD_DEV_READ_CUR_FRAME_NR);
+ USB->USBCmdCode = 0x00000200 | (CMD_DEV_READ_CUR_FRAME_NR << 16);
+ Wait4DevInt(CDFULL);
+ read_frame |= USB->USBCmdData << 8;
+
+ if (current_frame < 0) {
+ current_frame = read_frame;
+ guessed_frames = 0;
+ } else {
+ // All of the complicated stuff in here tracks the frame number from
+ // hardware (which comes from the SOF tokens sent out by the host) except
+ // deal with it if we miss a couple or get off by a little bit (and reset
+ // completely if we get off by a lot or miss a lot in a row).
+
+ static const uint32_t kMaxReadFrame = 0x800;
+ static const uint32_t kReadMask = kMaxReadFrame - 1;
+ if ((current_frame & kReadMask) == read_frame) {
+ // This seems like it must mean that we didn't receive the SOF token.
+ ++guessed_frames;
+ } else {
+ guessed_frames = 0;
+ // The frame number that we think we should have gotten.
+ int32_t expected_frame = current_frame + guessed_frames + 1;
+ int16_t difference =
+ read_frame - (int16_t)(expected_frame & kReadMask);
+ // If we're off by only a little.
+ if (difference > -10 && difference < 10) {
+ current_frame = (expected_frame & ~kReadMask) | read_frame;
+ // If we're ahead by only a little (or dead on) but we wrapped.
+ } else if (difference > kMaxReadFrame - 10) {
+ current_frame =
+ ((expected_frame & ~kReadMask) - kMaxReadFrame) | read_frame;
+ // If we're behind by only a little (or dead on) but the number in the
+ // token wrapped.
+ } else if (difference < -(kMaxReadFrame - 10)) {
+ current_frame =
+ ((expected_frame & ~kReadMask) + kMaxReadFrame) | read_frame;
+ } else {
+ // We're way off, so give up and reset.
+ current_frame = -1;
+ }
+ }
+ }
}
- fillSensorPacket(&usbPacket);
- static uint32_t sequence = 0;
- usbPacket.sequence = sequence++;
- USBHwEPWrite(ISOC_IN_EP, (unsigned char *)&usbPacket, DATA_PACKET_SIZE);
+ sensor_values.frame_number = current_frame + guessed_frames;
+ sensor_values.unknown_frame = guessed_frames > 10;
+
+ USBHwEPWrite(ISOC_IN_EP, (unsigned char *)&sensor_values, DATA_PACKET_SIZE);
+
+ if (uxQueueMessagesWaitingFromISR(xCharsForTx) > 0) {
+ // Data to send is available so enable interrupt instead of NAK.
+ bulk_in_nak_int(1);
+ } else {
+ bulk_in_nak_int(0);
+ }
+}
+
+void USB_IRQHandler(void) {
+ higher_priority_task_woken = pdFALSE;
+ uint32_t status = SC->USBIntSt;
+ if (status & USB_INT_REQ_HP) {
+ // We set the frame interrupt to get routed to the high priority line.
+ HandleFrame();
+ }
+ //if (status & USB_INT_REQ_LP) {
+ // Call lpcusb to let it handle all of the other interrupts.
+ USBHwISR();
+ //}
+ portEND_SWITCHING_ISR(higher_priority_task_woken);
}
void usb_init(void) {
@@ -298,8 +375,11 @@
USBHwRegisterEPIntHandler(BULK_IN_EP, DebugIn);
USBHwRegisterEPIntHandler(BULK_OUT_EP, DebugOut);
+ USB->USBDevIntPri = 1; // route frame interrupt to high priority line
+ USB->USBDevIntEn |= FRAME; // enable frame interrupt
+
// register frame handler
- USBHwRegisterFrameHandler(USBFrameHandler);
+ //USBHwRegisterFrameHandler(USBFrameHandler);
DBG("Starting USB communication\n");