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_;