Arm works
Added gravity and calibrated it. Terifying...
Change-Id: I70babb1cd3b83ddd7a81f06fb2a75cefd55bcdb8
diff --git a/frc971/control_loops/jacobian.h b/frc971/control_loops/jacobian.h
index fe3c157..8b0d4a5 100644
--- a/frc971/control_loops/jacobian.h
+++ b/frc971/control_loops/jacobian.h
@@ -9,7 +9,7 @@
template <int num_states, int num_inputs, typename F>
::Eigen::Matrix<double, num_states, num_inputs> NumericalJacobian(
const F &fn, ::Eigen::Matrix<double, num_inputs, 1> input) {
- constexpr double kEpsilon = 1e-4;
+ constexpr double kEpsilon = 1e-5;
::Eigen::Matrix<double, num_states, num_inputs> result =
::Eigen::Matrix<double, num_states, num_inputs>::Zero();