Fixed bug where kalman filter was being coppied rather than passed by pointer.
Change-Id: Ib2d0fa0ec2e5a0b19592ca419fec339252a3f698
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 50c5df7..aabd89d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -29,7 +29,9 @@
: aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
my_drivetrain),
dt_config_(dt_config),
- kf_(dt_config_.make_kf_drivetrain_loop()) {
+ kf_(dt_config_.make_kf_drivetrain_loop()),
+ dt_openloop_(dt_config_, &kf_),
+ dt_closedloop_(dt_config_) {
::aos::controls::HPolytope<0>::Init();
}
@@ -116,6 +118,8 @@
status->output_was_capped = dt_closedloop_.OutputWasCapped();
status->uncapped_left_voltage = dt_closedloop_.loop().U_uncapped(0, 0);
status->uncapped_right_voltage = dt_closedloop_.loop().U_uncapped(1, 0);
+
+ dt_openloop_.PopulateStatus(status);
}
double left_voltage = 0.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 22a6c32..a2f2015 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -41,9 +41,9 @@
const DrivetrainConfig dt_config_;
- PolyDrivetrain dt_openloop_{dt_config_};
- DrivetrainMotorsSS dt_closedloop_{dt_config_};
StateFeedbackLoop<7, 2, 3> kf_;
+ PolyDrivetrain dt_openloop_;
+ DrivetrainMotorsSS dt_closedloop_;
double last_left_voltage_ = 0;
double last_right_voltage_ = 0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 7fa91b9..24b1d38 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -112,6 +112,10 @@
double uncapped_left_voltage;
double uncapped_right_voltage;
+ // The goal velocities for the polydrive controller.
+ double left_velocity_goal;
+ double right_velocity_goal;
+
// True if the output voltage was capped last cycle.
bool output_was_capped;
};
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 6e08b84..7287df1 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -20,8 +20,9 @@
using ::frc971::control_loops::CIMLogging;
using ::frc971::control_loops::CoerceGoal;
-PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config)
- : kf_(dt_config.make_kf_drivetrain_loop()),
+PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config,
+ StateFeedbackLoop<7, 2, 3> *kf)
+ : kf_(kf),
U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
/*[*/ -1, 0 /*]*/,
/*[*/ 0, 1 /*]*/,
@@ -212,7 +213,7 @@
}
}
-double PolyDrivetrain::FilterVelocity(double throttle) {
+double PolyDrivetrain::FilterVelocity(double throttle) const {
const Eigen::Matrix<double, 2, 2> FF =
loop_->B().inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
@@ -261,8 +262,8 @@
void PolyDrivetrain::Update() {
if (dt_config_.loop_type == LoopType::CLOSED_LOOP) {
- loop_->mutable_X_hat()(0, 0) = kf_.X_hat()(1, 0);
- loop_->mutable_X_hat()(1, 0) = kf_.X_hat()(3, 0);
+ loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
+ loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
}
// TODO(austin): Observer for the current velocity instead of difference
@@ -292,6 +293,8 @@
}
const double left_velocity = fvel - steering_velocity;
const double right_velocity = fvel + steering_velocity;
+ goal_left_velocity_ = left_velocity;
+ goal_right_velocity_ = right_velocity;
// Integrate velocity to get the position.
// This position is used to get integral control.
@@ -354,6 +357,8 @@
LOG_STRUCT(DEBUG, "currently", logging);
}
+ goal_left_velocity_ = current_left_velocity;
+ goal_right_velocity_ = current_right_velocity;
// Any motor is not in gear. Speed match.
::Eigen::Matrix<double, 1, 1> R_left;
@@ -384,6 +389,12 @@
}
}
+void PolyDrivetrain::PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) {
+ status->left_velocity_goal = goal_left_velocity_;
+ status->right_velocity_goal = goal_right_velocity_;
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index c482f77..689e76f 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -15,7 +15,8 @@
public:
enum Gear { HIGH, LOW, SHIFTING_UP, SHIFTING_DOWN };
- PolyDrivetrain(const DrivetrainConfig &dt_config);
+ PolyDrivetrain(const DrivetrainConfig &dt_config,
+ StateFeedbackLoop<7, 2, 3> *kf);
int controller_index() const { return loop_->controller_index(); }
@@ -39,7 +40,7 @@
void SetPosition(
const ::frc971::control_loops::DrivetrainQueue::Position *position);
- double FilterVelocity(double throttle);
+ double FilterVelocity(double throttle) const;
double MaxVelocity();
@@ -47,8 +48,10 @@
void SendMotors(::frc971::control_loops::DrivetrainQueue::Output *output);
+ void PopulateStatus(::frc971::control_loops::DrivetrainQueue::Status *status);
+
private:
- StateFeedbackLoop<7, 2, 3> kf_;
+ StateFeedbackLoop<7, 2, 3> *kf_;
const ::aos::controls::HPolytope<2> U_Poly_;
@@ -66,6 +69,9 @@
::frc971::control_loops::DrivetrainQueue::Position position_;
int counter_;
DrivetrainConfig dt_config_;
+
+ double goal_left_velocity_ = 0.0;
+ double goal_right_velocity_ = 0.0;
};
} // namespace drivetrain