Created a spline drivetrain.
Change-Id: Icfbfcec930d17d1a2ca075fc2eb67594d35113df
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 44eb262..18ed29e 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -188,3 +188,33 @@
// Maximum acceleration for the profile.
float max_acceleration;
};
+
+// Definition of a constraint on a trajectory
+struct Constraint {
+ // Type of constraint
+ // 0: Null constraint. Ignore and all following
+ // 1: longitual acceleration
+ // 2: lateral acceleration
+ // 3: voltage
+ // 4: velocity
+ uint8_t constraint_type;
+ float value;
+ // start and end distance are only checked for velocity limits.
+ float start_distance;
+ float end_distance;
+};
+
+// Parameters for computing a trajectory using a chain of splines and
+// constraints.
+struct MultiSpline {
+ // index of the spline. Zero indicates the spline should not be computed.
+ uint32_t spline_idx;
+ // Number of splines. The spline point arrays will be expected to have
+ // 6 + 5 * (n - 1) points in them. The endpoints are shared between
+ // neighboring splines.
+ uint8_t spline_count;
+ float[36] spline_x;
+ float[36] spline_y;
+
+ Constraint[6] constraints;
+};
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index aee3a37..d95e101 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -49,6 +49,23 @@
)
cc_library(
+ name = "splinedrivetrain",
+ srcs = [
+ "splinedrivetrain.cc",
+ ],
+ hdrs = [
+ "splinedrivetrain.h",
+ ],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_queue",
+ ":spline",
+ ":distance_spline",
+ ":trajectory",
+ ]
+)
+
+cc_library(
name = "ssdrivetrain",
srcs = [
"ssdrivetrain.cc",
@@ -170,6 +187,7 @@
":gear",
":polydrivetrain",
":ssdrivetrain",
+ ":splinedrivetrain",
"//aos/controls:control_loop",
"//aos/logging:matrix_logging",
"//aos/logging:queue_logging",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a50b429..ec9faaa 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -38,6 +38,7 @@
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+ dt_spline_(dt_config_),
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),
@@ -329,6 +330,7 @@
dt_closedloop_.SetGoal(*goal);
dt_openloop_.SetGoal(*goal);
+ dt_spline_.SetGoal(*goal);
}
dt_openloop_.Update(robot_state().voltage_battery);
@@ -342,6 +344,9 @@
case 1:
dt_closedloop_.SetOutput(output);
break;
+ case 2:
+ dt_spline_.SetOutput(output);
+ break;
}
// The output should now contain the shift request.
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a357eab..05bf711 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/gear.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
namespace frc971 {
namespace control_loops {
@@ -50,6 +51,7 @@
StateFeedbackLoop<7, 2, 4> kf_;
PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
+ SplineDrivetrain dt_spline_;
::aos::monotonic_clock::time_point last_gyro_time_ =
::aos::monotonic_clock::min_time;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index c66b03e..f63f685 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -72,6 +72,14 @@
// The control loop will profile if these are all non-zero.
.frc971.ProfileParameters linear;
.frc971.ProfileParameters angular;
+
+ // Parameters for a spline to follow. This just contains info on a spline to
+ // compute. Each time this is sent, spline drivetrain will compute a new
+ // spline.
+ .frc971.MultiSpline spline;
+
+ // Which spline to follow.
+ uint32_t spline_handle;
};
message Position {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
new file mode 100644
index 0000000..11a4f69
--- /dev/null
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -0,0 +1,90 @@
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
+
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+const int kMaxSplineConstraints = 6;
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
+ : dt_config_(dt_config) {}
+
+// TODO(alex): put in another thread to avoid malloc in RT.
+void SplineDrivetrain::SetGoal(
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+ current_spline_handle_ = goal.spline_handle;
+ const ::frc971::MultiSpline &multispline = goal.spline;
+ if (multispline.spline_idx) {
+ current_spline_idx_ = multispline.spline_idx;
+ auto x = multispline.spline_x;
+ auto y = multispline.spline_y;
+ ::std::vector<Spline> splines = ::std::vector<Spline>();
+ for (int i = 0; i < multispline.spline_count; ++i) {
+ ::Eigen::Matrix<double, 2, 6> points =
+ ::Eigen::Matrix<double, 2, 6>::Zero();
+ for (int j = 0; j < 6; ++j) {
+ points(0, j) = x[i * 5 + j];
+ points(1, j) = y[i * 5 + j];
+ }
+ splines.emplace_back(Spline(points));
+ }
+
+ distance_spline_ = ::std::unique_ptr<DistanceSpline>(
+ new DistanceSpline(::std::move(splines)));
+
+ current_trajectory_ = ::std::unique_ptr<Trajectory>(
+ new Trajectory(distance_spline_.get(), dt_config_));
+
+ for (int i = 0; i < kMaxSplineConstraints; ++i) {
+ const ::frc971::Constraint &constraint = multispline.constraints[i];
+ switch (constraint.constraint_type) {
+ case 0:
+ break;
+ case 1:
+ current_trajectory_->set_longitudal_acceleration(constraint.value);
+ break;
+ case 2:
+ current_trajectory_->set_lateral_acceleration(constraint.value);
+ break;
+ case 3:
+ current_trajectory_->set_voltage_limit(constraint.value);
+ break;
+ case 4:
+ current_trajectory_->LimitVelocity(constraint.start_distance,
+ constraint.end_distance,
+ constraint.value);
+ break;
+ }
+ }
+
+ current_trajectory_->Plan();
+ current_xva_plan_ = current_trajectory_->PlanXVA(dt_config_.dt);
+ }
+}
+
+// TODO(alex): Handle drift.
+void SplineDrivetrain::SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ if (!output) {
+ return;
+ }
+ if (current_spline_handle_ == current_spline_idx_) {
+ if (current_xva_idx_ < current_xva_plan_.size()) {
+ double current_distance = current_xva_plan_[current_xva_idx_](0);
+ ::Eigen::Matrix<double, 2, 1> FFVoltage =
+ current_trajectory_->FFVoltage(current_distance);
+ output->left_voltage = FFVoltage(0);
+ output->right_voltage = FFVoltage(1);
+ ++current_xva_idx_;
+ }
+ }
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
new file mode 100644
index 0000000..b311e14
--- /dev/null
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -0,0 +1,40 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
+
+#include "frc971/control_loops/drivetrain/distance_spline.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+class SplineDrivetrain {
+ public:
+ SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
+
+ void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+
+ void SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output);
+ // TODO(alex): What status do we need?
+ void PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ private:
+ const DrivetrainConfig<double> dt_config_;
+
+ uint32_t current_spline_handle_; // Current spline told to excecute.
+ uint32_t current_spline_idx_; // Current excecuting spline.
+ ::std::unique_ptr<DistanceSpline> distance_spline_;
+ ::std::unique_ptr<Trajectory> current_trajectory_;
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> current_xva_plan_;
+ size_t current_xva_idx_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_