Merged some code from Brian so that it builds on the bot.
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 01545ff..e0f9bb2 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -6,6 +6,7 @@
'dependencies': [
'<(AOS)/build/aos_all.gyp:Atom',
'../control_loops/control_loops.gyp:DriveTrain',
+ '../control_loops/control_loops.gyp:wrist',
'../control_loops/control_loops.gyp:wrist_lib_test',
'../input/input.gyp:JoystickReader',
'../input/input.gyp:SensorReader',
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 5048007..796ff62 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -2,6 +2,7 @@
#include <stddef.h>
#include <inttypes.h>
+#include <math.h>
#include "aos/common/messages/RobotState.q.h"
#include "aos/atom_code/output/MotorOutput.h"
@@ -11,25 +12,25 @@
namespace {
-constexpr double kCompHorizontalHallEffectStartAngle = 72 * M_PI / 180.0;
-constexpr double kPracticeHorizontalHallEffectStartAngle = 72 * M_PI / 180.0;
+const double kCompHorizontalHallEffectStartAngle = 72 * M_PI / 180.0;
+const double kPracticeHorizontalHallEffectStartAngle = 72 * M_PI / 180.0;
-constexpr double kCompHorizontalHallEffectStopAngle = 100 * M_PI / 180.0;
-constexpr double kPracticeHorizontalHallEffectStopAngle = 100 * M_PI / 180.0;
+const double kCompHorizontalHallEffectStopAngle = 100 * M_PI / 180.0;
+const double kPracticeHorizontalHallEffectStopAngle = 100 * M_PI / 180.0;
-constexpr double kPracticeHorizontalUpperPhysicalLimit = 95 * M_PI / 180.0;
-constexpr double kCompHorizontalUpperPhysicalLimit = 95 * M_PI / 180.0;
+const double kPracticeHorizontalUpperPhysicalLimit = 95 * M_PI / 180.0;
+const double kCompHorizontalUpperPhysicalLimit = 95 * M_PI / 180.0;
-constexpr double kPracticeHorizontalLowerPhysicalLimit = -37.5 * M_PI / 180.0;
-constexpr double kCompHorizontalLowerPhysicalLimit = -37.5 * M_PI / 180.0;
+const double kPracticeHorizontalLowerPhysicalLimit = -37.5 * M_PI / 180.0;
+const double kCompHorizontalLowerPhysicalLimit = -37.5 * M_PI / 180.0;
-constexpr double kPracticeHorizontalUpperLimit = 93 * M_PI / 180.0;
-constexpr double kCompHorizontalUpperLimit = 93 * M_PI / 180.0;
+const double kPracticeHorizontalUpperLimit = 93 * M_PI / 180.0;
+const double kCompHorizontalUpperLimit = 93 * M_PI / 180.0;
-constexpr double kPracticeHorizontalLowerLimit = -36 * M_PI / 180.0;
-constexpr double kCompHorizontalLowerLimit = -36 * M_PI / 180.0;
+const double kPracticeHorizontalLowerLimit = -36 * M_PI / 180.0;
+const double kCompHorizontalLowerLimit = -36 * M_PI / 180.0;
-constexpr double kHorizontalZeroingSpeed = 1.0;
+const double kHorizontalZeroingSpeed = 1.0;
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index d0fe261..f9f160f 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -37,6 +37,11 @@
'<(DEPTH)/frc971/frc971.gyp:common',
'<(EXTERNALS):eigen',
],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):eigen',
+ '<(AOS)/common/common.gyp:controls',
+ 'control_loops',
+ ],
},
{
'target_name': 'wrist_lib_test',
diff --git a/frc971/control_loops/wrist.cc b/frc971/control_loops/wrist.cc
index edbd8a0..dee0438 100644
--- a/frc971/control_loops/wrist.cc
+++ b/frc971/control_loops/wrist.cc
@@ -125,6 +125,8 @@
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) {
diff --git a/frc971/control_loops/wrist_main.cc b/frc971/control_loops/wrist_main.cc
index cf18412..0c0b66d 100644
--- a/frc971/control_loops/wrist_main.cc
+++ b/frc971/control_loops/wrist_main.cc
@@ -5,7 +5,7 @@
int main() {
::aos::Init();
frc971::control_loops::WristMotor wrist;
- looper.Run();
+ wrist.Run();
::aos::Cleanup();
return 0;
}