Added angle adjust control loop.
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index a7bbfa5..6c7c54b 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -2,6 +2,7 @@
#include <stddef.h>
#include <math.h>
+#include <array>
#include "aos/common/inttypes.h"
#include "aos/common/messages/RobotState.q.h"
@@ -11,6 +12,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 {
@@ -28,6 +32,23 @@
const double kPracticeWristLowerPhysicalLimit = -37.5 * M_PI / 180.0;
const double kCompWristLowerPhysicalLimit = -37.5 * M_PI / 180.0;
+const int kAngleAdjustHallEffect = 2;
+
+const ::std::array<double, kAngleAdjustHallEffect>
+ kCompAngleAdjustHorizontalHallEffectStartAngle = {{-0.1, 1.0}};
+const ::std::array<double, kAngleAdjustHallEffect>
+ kPracticeAngleAdjustHorizontalHallEffectStartAngle = {{-0.1, 1.0}};
+
+const ::std::array<double, kAngleAdjustHallEffect>
+ kCompAngleAdjustHorizontalHallEffectStopAngle = {{0.5, 1.5}};
+const ::std::array<double, kAngleAdjustHallEffect>
+ kPracticeAngleAdjustHorizontalHallEffectStopAngle = {{0.5, 1.5}};
+
+const double kPracticeAngleAdjustHorizontalUpperPhysicalLimit =
+ 3.0;
+const double kCompAngleAdjustHorizontalUpperPhysicalLimit =
+ 3.0;
+
const double kPracticeWristUpperLimit = 93 * M_PI / 180.0;
const double kCompWristUpperLimit = 93 * M_PI / 180.0;
@@ -36,6 +57,23 @@
const double kWristZeroingSpeed = 1.0;
+const double kPracticeAngleAdjustHorizontalLowerPhysicalLimit =
+ 0.0;
+const double kCompAngleAdjustHorizontalLowerPhysicalLimit =
+ 0.0;
+
+const double kPracticeAngleAdjustHorizontalUpperLimit =
+ 3.0;
+const double kCompAngleAdjustHorizontalUpperLimit =
+ 3.0;
+
+const double kPracticeAngleAdjustHorizontalLowerLimit =
+ 0.0;
+const double kCompAngleAdjustHorizontalLowerLimit =
+ 0.0;
+
+const double kAngleAdjustHorizontalZeroingSpeed = 1.0;
+
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
@@ -55,6 +93,20 @@
// Zeroing speed.
double wrist_zeroing_speed;
+ // AngleAdjust hall effect positive and negative edges.
+ ::std::array<double, 2> angle_adjust_horizontal_hall_effect_start_angle;
+ ::std::array<double, 2> angle_adjust_horizontal_hall_effect_stop_angle;
+
+ // Upper and lower extreme limits of travel for the angle adjust.
+ double angle_adjust_horizontal_upper_limit;
+ double angle_adjust_horizontal_lower_limit;
+ // Physical limits. These are here for testing.
+ double angle_adjust_horizontal_upper_physical_limit;
+ double angle_adjust_horizontal_lower_physical_limit;
+
+ // Zeroing speed.
+ double angle_adjust_horizontal_zeroing_speed;
+
// what camera_center returns
int camera_center;
};
@@ -77,6 +129,13 @@
kCompWristUpperPhysicalLimit,
kCompWristLowerPhysicalLimit,
kWristZeroingSpeed,
+ kCompAngleAdjustHorizontalHallEffectStartAngle,
+ kCompAngleAdjustHorizontalHallEffectStopAngle,
+ kCompAngleAdjustHorizontalUpperLimit,
+ kCompAngleAdjustHorizontalLowerLimit,
+ kCompAngleAdjustHorizontalUpperPhysicalLimit,
+ kCompAngleAdjustHorizontalLowerPhysicalLimit,
+ kAngleAdjustHorizontalZeroingSpeed,
kCompCameraCenter};
break;
case kPracticeTeamNumber:
@@ -87,6 +146,13 @@
kPracticeWristUpperPhysicalLimit,
kPracticeWristLowerPhysicalLimit,
kWristZeroingSpeed,
+ kPracticeAngleAdjustHorizontalHallEffectStartAngle,
+ kPracticeAngleAdjustHorizontalHallEffectStopAngle,
+ kPracticeAngleAdjustHorizontalUpperLimit,
+ kPracticeAngleAdjustHorizontalLowerLimit,
+ kPracticeAngleAdjustHorizontalUpperPhysicalLimit,
+ kPracticeAngleAdjustHorizontalLowerPhysicalLimit,
+ kAngleAdjustHorizontalZeroingSpeed,
kPracticeCameraCenter};
break;
default:
@@ -147,6 +213,55 @@
return true;
}
+bool angle_adjust_horizontal_hall_effect_start_angle(
+ ::std::array<double, 2> *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_hall_effect_start_angle;
+ return true;
+}
+bool angle_adjust_horizontal_hall_effect_stop_angle(
+ ::std::array<double, 2> *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_hall_effect_stop_angle;
+ return true;
+}
+bool angle_adjust_horizontal_upper_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_upper_limit;
+ return true;
+}
+
+bool angle_adjust_horizontal_lower_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_lower_limit;
+ return true;
+}
+
+bool angle_adjust_horizontal_upper_physical_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_upper_physical_limit;
+ return true;
+}
+
+bool angle_adjust_horizontal_lower_physical_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_horizontal_lower_physical_limit;
+ return true;
+}
+
+bool angle_adjust_horizontal_zeroing_speed(double *speed) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *speed = values->angle_adjust_horizontal_zeroing_speed;
+ return true;
+}
+
bool camera_center(int *center) {
const Values *const values = GetValues();
if (values == NULL) return false;