Tuned and connected down estimator.
Change-Id: I2182af89a664a376103b49c54b96e0a2368ffb3c
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 72a32cc..05fc4a3 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -93,6 +93,32 @@
],
)
+genrule(
+ name = 'genrule_down_estimator',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //frc971/control_loops/python:down_estimator) $(OUTS)',
+ tools = [
+ '//frc971/control_loops/python:down_estimator',
+ ],
+ outs = [
+ 'down_estimator.h',
+ 'down_estimator.cc',
+ ],
+)
+
+cc_library(
+ name = 'down_estimator',
+ hdrs = [
+ 'down_estimator.h',
+ ],
+ srcs = [
+ 'down_estimator.cc',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
cc_library(
name = 'drivetrain_lib',
srcs = [
@@ -102,12 +128,14 @@
'drivetrain.h',
],
deps = [
+ ':down_estimator',
':drivetrain_queue',
':gear',
':polydrivetrain',
':ssdrivetrain',
'//aos/common/controls:control_loop',
'//frc971/queues:gyro',
+ '//frc971/wpilib:imu_queue',
'//aos/common/util:log_interval',
'//aos/common/logging:queue_logging',
'//aos/common/logging:matrix_logging',
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b4c04a3..7c4f760 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -13,9 +13,11 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/down_estimator.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/shifter_hall_effect.h"
+#include "frc971/wpilib/imu.q.h"
using frc971::sensors::gyro_reading;
@@ -32,11 +34,13 @@
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+ down_estimator_(MakeDownEstimatorLoop()),
left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
left_high_requested_(dt_config_.default_high_gear),
right_high_requested_(dt_config_.default_high_gear) {
::aos::controls::HPolytope<0>::Init();
+ down_U_.setZero();
}
int DrivetrainLoop::ControllerIndexFromGears() {
@@ -111,6 +115,36 @@
LOG_STRUCT(DEBUG, "state", gear_logging);
}
+ if (::frc971::imu_values.FetchLatest()) {
+ const double rate = -::frc971::imu_values->gyro_y;
+ const double accel_squared = ::frc971::imu_values->accelerometer_x *
+ ::frc971::imu_values->accelerometer_x +
+ ::frc971::imu_values->accelerometer_y *
+ ::frc971::imu_values->accelerometer_y +
+ ::frc971::imu_values->accelerometer_z *
+ ::frc971::imu_values->accelerometer_z;
+ const double angle = ::std::atan2(-::frc971::imu_values->accelerometer_x,
+ -::frc971::imu_values->accelerometer_z) +
+ 0.008;
+ if (accel_squared > 1.03 || accel_squared < 0.97) {
+ LOG(DEBUG, "New IMU value, rejecting reading\n");
+ } else {
+ // -y is our gyro.
+ // -z accel is down
+ // -x accel is the front of the robot pointed down.
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << angle;
+ down_estimator_.Correct(Y);
+ }
+
+ LOG(DEBUG,
+ "New IMU value from ADIS16448, rate is %f, angle %f, fused %f, bias "
+ "%f\n",
+ rate, angle, down_estimator_.X_hat(0, 0), down_estimator_.X_hat(1, 0));
+ down_U_ << rate;
+ }
+ down_estimator_.UpdateObserver(down_U_);
+
// TODO(austin): Signal the current gear to both loops.
if (gyro_reading.FetchLatest()) {
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a9541b7..69e9794 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -48,6 +48,9 @@
PolyDrivetrain dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
+ StateFeedbackLoop<2, 1, 1> down_estimator_;
+ Eigen::Matrix<double, 1, 1> down_U_;
+
// Current gears for each drive side.
Gear left_gear_;
Gear right_gear_;
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index 1d776b3..6db8f34 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -10,6 +10,7 @@
from matplotlib import pylab
from frc971.control_loops.python import controls
+from frc971.control_loops.python import control_loop
FLAGS = gflags.FLAGS
@@ -29,16 +30,23 @@
self.B_continuous = numpy.matrix([[1],
[0]])
- self.A, self.B = c2d(self.A_continuous, self.B_continuous, dt):
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
q_gyro_noise = 1e-6
+ q_gyro_bias_noise = 1e-3
self.Q = numpy.matrix([[1.0 / (q_gyro_noise ** 2.0), 0.0],
- [0.0, 1.0 / (q_gyro_noise ** 2.0)]])
+ [0.0, 1.0 / (q_gyro_bias_noise ** 2.0)]])
- r_accelerometer_angle_noise = 5e-3
+ r_accelerometer_angle_noise = 5e+7
self.R = numpy.matrix([[(r_accelerometer_angle_noise ** 2.0)]])
self.C = numpy.matrix([[1.0, 0.0]])
+ self.D = numpy.matrix([[0]])
+
+ self.U_max = numpy.matrix([[numpy.pi]])
+ self.U_min = numpy.matrix([[-numpy.pi]])
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
@@ -100,5 +108,14 @@
pylab.plot(range(len(estimated_velocities)), estimated_velocities)
pylab.show()
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['frc971', 'control_loops', 'drivetrain']
+ kf_loop_writer = control_loop.ControlLoopWriter(
+ "DownEstimator", [estimator],
+ namespaces = namespaces)
+ kf_loop_writer.Write(argv[1], argv[2])
+
if __name__ == '__main__':
sys.exit(main(sys.argv))