Clang-format drivetrain
Change-Id: I9e20a52ca97b968046969fa2c2e7f42b1fcbb1c3
diff --git a/frc971/control_loops/drivetrain/camera.h b/frc971/control_loops/drivetrain/camera.h
index 02e7e65..e1d4b76 100644
--- a/frc971/control_loops/drivetrain/camera.h
+++ b/frc971/control_loops/drivetrain/camera.h
@@ -150,14 +150,14 @@
struct Reading {
// The heading as reported from the camera; zero = straight ahead,
// positive = target in the left half of the image.
- Scalar heading; // radians
+ Scalar heading; // radians
// The distance from the camera to the target.
Scalar distance; // meters
// Height of the target from the camera.
- Scalar height; // meters
+ Scalar height; // meters
// The angle of the target relative to line between the camera and
// the center of the target.
- Scalar skew; // radians
+ Scalar skew; // radians
};
Reading reading;
Reading noise;
@@ -283,14 +283,12 @@
apparent_width / noise_parameters_.max_viewable_distance;
view->noise.distance =
noise_parameters_.nominal_distance_noise / normalized_width;
- view->noise.skew =
- noise_parameters_.nominal_skew_noise / normalized_width;
+ view->noise.skew = noise_parameters_.nominal_skew_noise / normalized_width;
view->noise.height =
noise_parameters_.nominal_height_noise / normalized_width;
}
private:
-
// If the specified target is visible from the current camera Pose, adds it to
// the views array.
void AddTargetIfVisible(
@@ -316,7 +314,7 @@
// such a logical obstacle (e.g., the cargo ship) may consist of many
// obstacles in this list to account for all of its sides.
::std::array<LineSegment, num_obstacles> obstacles_;
-}; // class TypedCamera
+}; // class TypedCamera
template <int num_targets, int num_obstacles, typename Scalar>
void TypedCamera<num_targets, num_obstacles, Scalar>::AddTargetIfVisible(
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index cdb7fb9..113eb27 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -92,8 +92,9 @@
const double squared_norm = dspline_point.squaredNorm();
return ddspline_point / squared_norm -
- dspline_point * (dspline_point(0) * ddspline_point(0) +
- dspline_point(1) * ddspline_point(1)) /
+ dspline_point *
+ (dspline_point(0) * ddspline_point(0) +
+ dspline_point(1) * ddspline_point(1)) /
::std::pow(squared_norm, 2);
}
@@ -111,10 +112,10 @@
const double squared_norm = dspline_point.squaredNorm();
- return ddtheta / squared_norm -
- dtheta * (dspline_point(0) * ddspline_point(0) +
- dspline_point(1) * ddspline_point(1)) /
- ::std::pow(squared_norm, 2);
+ return ddtheta / squared_norm - dtheta *
+ (dspline_point(0) * ddspline_point(0) +
+ dspline_point(1) * ddspline_point(1)) /
+ ::std::pow(squared_norm, 2);
}
DistanceSpline::AlphaAndIndex DistanceSpline::DistanceToAlpha(
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
index 55b40dd..8189fe9 100644
--- a/frc971/control_loops/drivetrain/distance_spline_test.cc
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -61,10 +61,10 @@
idx_plot.push_back(dpoint(0));
idy_plot.push_back(dpoint(1));
- EXPECT_LT((point - expected_point).norm(), 1e-2) << ": At distance "
- << distance;
- EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2) << ": At distance "
- << distance;
+ EXPECT_LT((point - expected_point).norm(), 1e-2)
+ << ": At distance " << distance;
+ EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2)
+ << ": At distance " << distance;
// We need to record the starting state without integrating.
if (i == 0) {
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 13fee1a..5f2d3c4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -1,7 +1,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include <stdio.h>
#include <sched.h>
+#include <stdio.h>
#include <cmath>
#include <memory>
#include "Eigen/Dense"
@@ -114,7 +114,8 @@
down_estimator_.Reset();
// Just reset the localizer to the current state, except for the encoders.
LocalizerInterface::Ekf::State X_hat = localizer_->Xhat();
- X_hat(LocalizerInterface::StateIdx::kLeftEncoder) = position->left_encoder();
+ X_hat(LocalizerInterface::StateIdx::kLeftEncoder) =
+ position->left_encoder();
X_hat(LocalizerInterface::StateIdx::kRightEncoder) =
position->right_encoder();
localizer_->Reset(monotonic_now, X_hat);
@@ -264,9 +265,8 @@
VLOG(1) << "localizer_control "
<< aos::FlatbufferToJson(localizer_control_fetcher_.get());
localizer_->ResetPosition(
- monotonic_now,
- localizer_control_fetcher_->x(), localizer_control_fetcher_->y(),
- localizer_control_fetcher_->theta(),
+ monotonic_now, localizer_control_fetcher_->x(),
+ localizer_control_fetcher_->y(), localizer_control_fetcher_->theta(),
localizer_control_fetcher_->theta_uncertainty(),
!localizer_control_fetcher_->keep_current_theta());
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 3f553bb..b5990c3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -25,12 +25,12 @@
};
enum class GyroType : int32_t {
- SPARTAN_GYRO = 0, // Use the gyro on the spartan board.
- IMU_X_GYRO = 1, // Use the x-axis of the gyro on the IMU.
- IMU_Y_GYRO = 2, // Use the y-axis of the gyro on the IMU.
- IMU_Z_GYRO = 3, // Use the z-axis of the gyro on the IMU.
- FLIPPED_SPARTAN_GYRO = 4, // Use the gyro on the spartan board.
- FLIPPED_IMU_Z_GYRO = 5, // Use the flipped z-axis of the gyro on the IMU.
+ SPARTAN_GYRO = 0, // Use the gyro on the spartan board.
+ IMU_X_GYRO = 1, // Use the x-axis of the gyro on the IMU.
+ IMU_Y_GYRO = 2, // Use the y-axis of the gyro on the IMU.
+ IMU_Z_GYRO = 3, // Use the z-axis of the gyro on the IMU.
+ FLIPPED_SPARTAN_GYRO = 4, // Use the gyro on the spartan board.
+ FLIPPED_IMU_Z_GYRO = 5, // Use the flipped z-axis of the gyro on the IMU.
};
enum class IMUType : int32_t {
@@ -128,7 +128,8 @@
Eigen::Matrix<Scalar, 2, 2> Tlr_to_la() const {
return (::Eigen::Matrix<Scalar, 2, 2>() << 0.5, 0.5,
- -1.0 / (2 * robot_radius), 1.0 / (2 * robot_radius)).finished();
+ -1.0 / (2 * robot_radius), 1.0 / (2 * robot_radius))
+ .finished();
}
Eigen::Matrix<Scalar, 2, 2> Tla_to_lr() const {
@@ -140,8 +141,7 @@
Eigen::Matrix<Scalar, 4, 1> AngularLinearToLeftRight(
const Eigen::Matrix<Scalar, 2, 1> &linear,
const Eigen::Matrix<Scalar, 2, 1> &angular) const {
- Eigen::Matrix<Scalar, 2, 1> scaled_angle =
- angular * this->robot_radius;
+ Eigen::Matrix<Scalar, 2, 1> scaled_angle = angular * this->robot_radius;
Eigen::Matrix<Scalar, 4, 1> state;
state(0, 0) = linear(0, 0) - scaled_angle(0, 0);
state(1, 0) = linear(1, 0) - scaled_angle(1, 0);
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index c7ef9fd..4ff71d6 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -108,7 +108,8 @@
drivetrain_status_fetcher_(
event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
- imu_sender_(event_loop->MakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
+ imu_sender_(
+ event_loop->MakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
@@ -267,12 +268,11 @@
U(1, 0) += drivetrain_plant_.right_voltage_offset();
drivetrain_plant_.Update(U);
double dt_float = ::aos::time::DurationInSeconds(dt_config_.dt);
- const auto dynamics =
- [this](const ::Eigen::Matrix<double, 5, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- return ContinuousDynamics(velocity_drivetrain_->plant(),
- dt_config_.Tlr_to_la(), X, U);
- };
+ const auto dynamics = [this](const ::Eigen::Matrix<double, 5, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return ContinuousDynamics(velocity_drivetrain_->plant(),
+ dt_config_.Tlr_to_la(), X, U);
+ };
const Eigen::Matrix<double, 5, 1> last_state = state_;
state_ = RungeKuttaU(dynamics, state_, U, dt_float);
// Calculate Xdot from the actual state change rather than getting Xdot at the
@@ -284,8 +284,7 @@
const Eigen::Matrix<double, 5, 1> Xdot = (state_ - last_state) / dt_float;
const double yaw_rate = Xdot(2);
- const double longitudinal_velocity =
- (state_(4) + state_(3)) / 2.0;
+ const double longitudinal_velocity = (state_(4) + state_(3)) / 2.0;
const double centripetal_accel = yaw_rate * longitudinal_velocity;
// TODO(james): Allow inputting arbitrary calibrations, e.g., for testing
// situations where the IMU is not perfectly flat in the CG of the robot.
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index 800b758..e443ba1 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -68,7 +68,7 @@
Eigen::Matrix<double, 5, 1> *mutable_state() { return &state_; }
::Eigen::Matrix<double, 2, 1> GetPosition() const {
- return state_.block<2,1>(0,0);
+ return state_.block<2, 1>(0, 0);
}
void MaybePlot();
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.cc b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
index 4101993..5f8bce4 100644
--- a/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
@@ -38,10 +38,10 @@
left_goal = 0.0f;
right_goal = 0.0f;
max_ss_voltage = 0.0f;
- //linear.max_velocity = 0.0f;
- //linear.max_acceleration = 0.0f;
- //angular.max_velocity = 0.0f;
- //angular.max_acceleration = 0.0f;
+ // linear.max_velocity = 0.0f;
+ // linear.max_acceleration = 0.0f;
+ // angular.max_velocity = 0.0f;
+ // angular.max_acceleration = 0.0f;
}
DrivetrainQueue_Goal::DrivetrainQueue_Goal() { Zero(); }
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index de4f721..b2abaf9 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -231,14 +231,15 @@
// matrix for linear cases?
void Correct(
const Output &z, const Input *U,
- std::function<
- void(const State &, const StateSquare &,
- std::function<Output(const State &, const Input &)> *,
- std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)> make_h,
+ std::function<void(const State &, const StateSquare &,
+ std::function<Output(const State &, const Input &)> *,
+ std::function<Eigen::Matrix<
+ Scalar, kNOutputs, kNStates>(const State &)> *)>
+ make_h,
std::function<Output(const State &, const Input &)> h,
std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ dhdx,
+ const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t);
// A utility function for specifically updating with encoder and gyro
@@ -289,12 +290,11 @@
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
R.setZero();
R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_;
- Correct(
- z, &U, {},
- [this](const State &X, const Input &) {
- return H_encoders_and_gyro_ * X;
- },
- [this](const State &) { return H_encoders_and_gyro_; }, R, t);
+ Correct(z, &U, {},
+ [this](const State &X, const Input &) {
+ return H_encoders_and_gyro_ * X;
+ },
+ [this](const State &) { return H_encoders_and_gyro_; }, R, t);
}
// Sundry accessor:
@@ -359,7 +359,8 @@
std::function<void(const State &, const StateSquare &,
std::function<Output(const State &, const Input &)> *,
std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)> make_h;
+ const State &)> *)>
+ make_h;
// A function to calculate the expected output at a given state/input.
// TODO(james): For encoders/gyro, it is linear and the function call may
// be expensive. Potential source of optimization.
@@ -434,8 +435,7 @@
const Scalar diameter = 2.0 * dt_config_.robot_radius;
const Scalar yaw_rate = CalcYawRate(X);
// X and Y derivatives
- A_continuous(kX, kTheta) =
- -stheta * lng_vel - ctheta * lat_vel;
+ A_continuous(kX, kTheta) = -stheta * lng_vel - ctheta * lat_vel;
A_continuous(kX, kLeftVelocity) = ctheta / 2.0;
A_continuous(kX, kRightVelocity) = ctheta / 2.0;
A_continuous(kX, kLateralVelocity) = -stheta;
@@ -494,8 +494,7 @@
const Scalar expected_lat_accel = lng_vel * yaw_rate;
const Scalar expected_lng_accel =
CalcLongitudinalVelocity(Xdot) - yaw_rate * lat_vel;
- const Scalar lng_accel_offset =
- U(kLongitudinalAccel) - expected_lng_accel;
+ const Scalar lng_accel_offset = U(kLongitudinalAccel) - expected_lng_accel;
constexpr double kAccelWeight = 1.0;
if (!ignore_accel) {
Xdot(kLeftVelocity) += kAccelWeight * lng_accel_offset;
@@ -590,7 +589,6 @@
aos::PriorityQueue<Observation, kSaveSamples, std::less<Observation>>
observations_;
-
friend class testing::HybridEkfTest;
friend class y2019::control_loops::testing::ParameterizedLocalizerTest;
}; // class HybridEkf
@@ -601,10 +599,12 @@
std::function<void(const State &, const StateSquare &,
std::function<Output(const State &, const Input &)> *,
std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)> make_h,
+ const State &)> *)>
+ make_h,
std::function<Output(const State &, const Input &)> h,
std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ dhdx,
+ const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t) {
CHECK(!observations_.empty());
if (!observations_.full() && t < observations_.begin()->t) {
@@ -752,9 +752,9 @@
// noise on these numbers is particularly high, then we can end up with weird
// dynamics where a camera update both shifts our X/Y position and adjusts our
// velocity estimates substantially, causing the camera updates to create
- // "momentum" and if we don't trust the encoders enough, then we have no way of
- // determining that the velocity updates are bogus. This also interacts with
- // kVelocityOffsetTimeConstant.
+ // "momentum" and if we don't trust the encoders enough, then we have no way
+ // of determining that the velocity updates are bogus. This also interacts
+ // with kVelocityOffsetTimeConstant.
Q_continuous_(kLongitudinalVelocityOffset, kLongitudinalVelocityOffset) =
std::pow(1.1, 2.0);
Q_continuous_(kLateralVelocity, kLateralVelocity) = std::pow(0.1, 2.0);
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 48b8b5f..9d9517d 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -76,8 +76,7 @@
EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kLeftVelocity), expected_accel(0));
EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kRightVelocity), expected_accel(1));
EXPECT_EQ(Xdot_ekf(StateIdx::kLateralVelocity), 0.0);
- EXPECT_EQ(
- Xdot_ekf(StateIdx::kLongitudinalVelocityOffset), 0.0);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kLongitudinalVelocityOffset), 0.0);
} else {
const double lng_accel = U(InputIdx::kLongitudinalAccel) +
lat_vel * yaw_rate + ekf_.VelocityAccel(lng_vel);
@@ -121,7 +120,6 @@
std::normal_distribution<> normal_;
};
-
// Tests that if we provide a bunch of observations of the position
// with zero change in time, the state should approach the estimation.
struct DiffEqInputs {
@@ -152,7 +150,7 @@
const State Xdot_plus = DiffEq(GetParam().X + perturbation, GetParam().U,
GetParam().ignore_accel);
const State Xdot_minus = DiffEq(GetParam().X - perturbation, GetParam().U,
- GetParam().ignore_accel);
+ GetParam().ignore_accel);
const State numerical_dXdot_dX = (Xdot_plus - Xdot_minus) / (2.0 * kEps);
const State A_based_dXdot_dX = A * perturbation / kEps;
EXPECT_LT((A_based_dXdot_dX - numerical_dXdot_dX).norm(), 1e-8)
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index 8d2592b..56e7536 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -45,8 +45,8 @@
// States are X_hat_bar (position estimate) and P (Covariance)
void QuaternionUkf::DoPredict(const Eigen::Matrix<double, 3, 1> &U,
- const Eigen::Matrix<double, 3, 1> &measurement,
- const aos::monotonic_clock::duration dt) {
+ const Eigen::Matrix<double, 3, 1> &measurement,
+ const aos::monotonic_clock::duration dt) {
// Compute the sigma points.
// Our system is pretty linear. The traditional way of dealing with process
// noise is to augment your state vector with the mean of the process noise,
@@ -155,8 +155,7 @@
}
// Compute the kalman gain.
- const Eigen::Matrix<double, 3, kNumMeasurements> K =
- P_xz * P_vv.inverse();
+ const Eigen::Matrix<double, 3, kNumMeasurements> K = P_xz * P_vv.inverse();
// Update X_hat and the covariance P
X_hat_ = X_hat_ * Eigen::Quaternion<double>(
@@ -288,8 +287,7 @@
const Eigen::Vector3d robot_y_in_global_frame =
X_hat() * Eigen::Vector3d::UnitY();
const double xy_norm = robot_y_in_global_frame.block<2, 1>(0, 0).norm();
- builder.add_lateral_pitch(
- std::atan2(robot_y_in_global_frame.z(), xy_norm));
+ builder.add_lateral_pitch(std::atan2(robot_y_in_global_frame.z(), xy_norm));
}
builder.add_position_x(pos_vel()(0, 0));
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index 79e9b7f..548dba8 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -255,8 +255,7 @@
// determine that calibration routines would be unnecessary).
Eigen::Quaternion<double> Xquat(X);
Eigen::Matrix<double, 3, 1> gprime =
- Xquat.conjugate() * Eigen::Matrix<double, 3, 1>(0.0, 0.0, 1.0) *
- 1.0;
+ Xquat.conjugate() * Eigen::Matrix<double, 3, 1>(0.0, 0.0, 1.0) * 1.0;
return gprime;
}
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index c4643d9..9894f68 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -108,9 +108,10 @@
EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 0.01))
<< "Expected: " << expected.coeffs()
<< " Got: " << dtukf.X_hat().coeffs();
- EXPECT_NEAR(0.0,
- (Eigen::Vector3d(0.0, 1.0, 0.0) - dtukf.H(dtukf.X_hat().coeffs()))
- .norm(), 1e-10);
+ EXPECT_NEAR(
+ 0.0,
+ (Eigen::Vector3d(0.0, 1.0, 0.0) - dtukf.H(dtukf.X_hat().coeffs())).norm(),
+ 1e-10);
}
// Tests that the euler angles in the status message are correct.
@@ -166,7 +167,6 @@
<< " Got: " << dtukf.X_hat().coeffs();
}
-
// Tests that if the gyro indicates no movement but that the accelerometer shows
// that we are slightly rotated, that we eventually adjust our estimate to be
// correct.
@@ -193,9 +193,9 @@
<< " Got: " << dtukf.X_hat().coeffs();
}
-// Tests that if the accelerometer is reading values with a magnitude that isn't ~1g,
-// that we are slightly rotated, that we eventually adjust our estimate to be
-// correct.
+// Tests that if the accelerometer is reading values with a magnitude that isn't
+// ~1g, that we are slightly rotated, that we eventually adjust our estimate to
+// be correct.
TEST(DownEstimatorTest, UkfIgnoreBadAccel) {
drivetrain::DrivetrainUkf dtukf(
drivetrain::testing::GetTestDrivetrainConfig());
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 280171f..6ea23c6 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -1,5 +1,5 @@
-#include <vector>
#include <string>
+#include <vector>
#include "Eigen/Dense"
@@ -15,198 +15,192 @@
namespace drivetrain {
extern "C" {
- // Based on spline.h
- NSpline<6> *NewSpline(double x[6], double y[6]) {
- return new NSpline<6>((::Eigen::Matrix<double, 2, 6>() << x[0], x[1], x[2],
- x[3], x[4], x[5], y[0], y[1], y[2], y[3], y[4],
- y[5]).finished());
+// Based on spline.h
+NSpline<6> *NewSpline(double x[6], double y[6]) {
+ return new NSpline<6>((::Eigen::Matrix<double, 2, 6>() << x[0], x[1], x[2],
+ x[3], x[4], x[5], y[0], y[1], y[2], y[3], y[4], y[5])
+ .finished());
+}
+
+void deleteSpline(NSpline<6> *spline) { delete spline; }
+
+void SplinePoint(NSpline<6> *spline, double alpha, double *res) {
+ const Eigen::Vector2d xy = spline->Point(alpha);
+ res[0] = xy.x();
+ res[1] = xy.y();
+}
+
+void SplineDPoint(NSpline<6> *spline, double alpha, double *res) {
+ const Eigen::Vector2d dxy = spline->DPoint(alpha);
+ res[0] = dxy.x();
+ res[1] = dxy.y();
+}
+
+void SplineDDPoint(NSpline<6> *spline, double alpha, double *res) {
+ const Eigen::Vector2d ddxy = spline->DDPoint(alpha);
+ res[0] = ddxy.x();
+ res[1] = ddxy.y();
+}
+
+void SplineDDDPoint(NSpline<6> *spline, double alpha, double *res) {
+ const Eigen::Vector2d dddxy = spline->DDDPoint(alpha);
+ res[0] = dddxy.x();
+ res[1] = dddxy.y();
+}
+
+double SplineTheta(NSpline<6> *spline, double alpha) {
+ return spline->Theta(alpha);
+}
+
+double SplineDTheta(NSpline<6> *spline, double alpha) {
+ return spline->DTheta(alpha);
+}
+
+double SplineDDTheta(NSpline<6> *spline, double alpha) {
+ return spline->DDTheta(alpha);
+}
+
+void SplineControlPoints(NSpline<6> *spline, double *x, double *y) {
+ auto points = spline->control_points();
+ // Deal with incorrectly strided matrix.
+ for (int i = 0; i < 6; ++i) {
+ x[i] = points(0, i);
+ y[i] = points(1, i);
}
+}
- void deleteSpline(NSpline<6> *spline) { delete spline; }
-
- void SplinePoint(NSpline<6> *spline, double alpha, double *res) {
- const Eigen::Vector2d xy = spline->Point(alpha);
- res[0] = xy.x();
- res[1] = xy.y();
+// Based on distance_spline.h
+DistanceSpline *NewDistanceSpline(Spline **splines, int count) {
+ ::std::vector<Spline> splines_;
+ for (int i = 0; i < count; ++i) {
+ splines_.push_back(*splines[i]);
}
+ return new DistanceSpline(::std::vector<Spline>(splines_));
+}
- void SplineDPoint(NSpline<6> *spline, double alpha, double *res) {
- const Eigen::Vector2d dxy = spline->DPoint(alpha);
- res[0] = dxy.x();
- res[1] = dxy.y();
+void deleteDistanceSpline(DistanceSpline *spline) { delete spline; }
+
+void DistanceSplineXY(DistanceSpline *spline, double distance, double *res) {
+ const Eigen::Vector2d xy = spline->XY(distance);
+ res[0] = xy.x();
+ res[1] = xy.y();
+}
+
+void DistanceSplineDXY(DistanceSpline *spline, double distance, double *res) {
+ const Eigen::Vector2d dxy = spline->DXY(distance);
+ res[0] = dxy.x();
+ res[1] = dxy.y();
+}
+
+void DistanceSplineDDXY(DistanceSpline *spline, double distance, double *res) {
+ const Eigen::Vector2d ddxy = spline->DDXY(distance);
+ res[0] = ddxy.x();
+ res[1] = ddxy.y();
+}
+
+double DistanceSplineTheta(DistanceSpline *spline, double distance) {
+ return spline->Theta(distance);
+}
+
+double DistanceSplineDTheta(DistanceSpline *spline, double distance) {
+ return spline->DTheta(distance);
+}
+
+double DistanceSplineDThetaDt(DistanceSpline *spline, double distance,
+ double velocity) {
+ return spline->DThetaDt(distance, velocity);
+}
+
+double DistanceSplineDDTheta(DistanceSpline *spline, double distance) {
+ return spline->DDTheta(distance);
+}
+
+double DistanceSplineLength(DistanceSpline *spline) { return spline->length(); }
+
+// Based on trajectory.h
+Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
+ int num_distance) {
+ return new Trajectory(
+ spline, ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
+ num_distance);
+}
+
+void deleteTrajectory(Trajectory *t) { delete t; }
+
+void TrajectorySetLongitudinalAcceleration(Trajectory *t, double accel) {
+ t->set_longitudinal_acceleration(accel);
+}
+
+void TrajectorySetLateralAcceleration(Trajectory *t, double accel) {
+ t->set_lateral_acceleration(accel);
+}
+
+void TrajectorySetVoltageLimit(Trajectory *t, double limit) {
+ t->set_voltage_limit(limit);
+}
+
+void TrajectoryLimitVelocity(Trajectory *t, double start, double end,
+ double max) {
+ t->LimitVelocity(start, end, max);
+}
+
+void TrajectoryPlan(Trajectory *t) { t->Plan(); }
+
+void TrajectoryVoltage(Trajectory *t, double distance, double *res) {
+ const Eigen::Vector2d ff_voltage = t->FFVoltage(distance);
+ res[0] = ff_voltage.x();
+ res[1] = ff_voltage.y();
+}
+
+double TrajectoryLength(Trajectory *t) { return t->length(); }
+
+int TrajectoryGetPathLength(Trajectory *t) { return t->plan().size(); }
+
+// This assumes that res is created in python to be getPathLength() long.
+// Likely to SEGFAULT otherwise.
+void TrajectoryDistances(Trajectory *t, double *res) {
+ const ::std::vector<double> &distances = t->Distances();
+ ::std::memcpy(res, distances.data(), sizeof(double) * distances.size());
+}
+
+double TrajectoryDistance(Trajectory *t, int index) {
+ return t->Distance(index);
+}
+
+// This assumes that res is created in python to be getPathLength() long.
+// Likely to SEGFAULT otherwise.
+void TrajectoryGetPlan(Trajectory *t, double *res) {
+ const ::std::vector<double> &velocities = t->plan();
+ ::std::memcpy(res, velocities.data(), sizeof(double) * velocities.size());
+}
+
+// Time in in nanoseconds.
+::std::vector<::Eigen::Matrix<double, 3, 1>> *TrajectoryGetPlanXVAPtr(
+ Trajectory *t, int dt) {
+ return new ::std::vector<::Eigen::Matrix<double, 3, 1>>(
+ t->PlanXVA(::std::chrono::nanoseconds(dt)));
+}
+
+void TrajectoryDeleteVector(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
+ delete vec;
+}
+
+int TrajectoryGetVectorLength(
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
+ return vec->size();
+}
+
+void TrajectoryGetPlanXVA(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec,
+ double *X, double *V, double *A) {
+ for (size_t i = 0; i < vec->size(); ++i) {
+ X[i] = (*vec)[i][0];
+ V[i] = (*vec)[i][1];
+ A[i] = (*vec)[i][2];
}
+}
- void SplineDDPoint(NSpline<6> *spline, double alpha, double *res) {
- const Eigen::Vector2d ddxy = spline->DDPoint(alpha);
- res[0] = ddxy.x();
- res[1] = ddxy.y();
- }
-
- void SplineDDDPoint(NSpline<6> *spline, double alpha, double *res) {
- const Eigen::Vector2d dddxy = spline->DDDPoint(alpha);
- res[0] = dddxy.x();
- res[1] = dddxy.y();
- }
-
- double SplineTheta(NSpline<6> *spline, double alpha) {
- return spline->Theta(alpha);
- }
-
- double SplineDTheta(NSpline<6> *spline, double alpha) {
- return spline->DTheta(alpha);
- }
-
- double SplineDDTheta(NSpline<6> *spline, double alpha) {
- return spline->DDTheta(alpha);
- }
-
- void SplineControlPoints(NSpline<6> *spline, double *x, double *y) {
- auto points = spline->control_points();
- // Deal with incorrectly strided matrix.
- for (int i = 0; i < 6; ++i) {
- x[i] = points(0, i);
- y[i] = points(1, i);
- }
- }
-
- // Based on distance_spline.h
- DistanceSpline *NewDistanceSpline(Spline **splines, int count) {
- ::std::vector<Spline> splines_;
- for (int i = 0; i < count; ++i) {
- splines_.push_back(*splines[i]);
- }
- return new DistanceSpline(::std::vector<Spline>(splines_));
- }
-
- void deleteDistanceSpline(DistanceSpline *spline) { delete spline; }
-
- void DistanceSplineXY(DistanceSpline *spline, double distance, double *res) {
- const Eigen::Vector2d xy = spline->XY(distance);
- res[0] = xy.x();
- res[1] = xy.y();
- }
-
- void DistanceSplineDXY(DistanceSpline *spline, double distance, double *res) {
- const Eigen::Vector2d dxy = spline->DXY(distance);
- res[0] = dxy.x();
- res[1] = dxy.y();
- }
-
- void DistanceSplineDDXY(DistanceSpline *spline, double distance,
- double *res) {
- const Eigen::Vector2d ddxy = spline->DDXY(distance);
- res[0] = ddxy.x();
- res[1] = ddxy.y();
- }
-
- double DistanceSplineTheta(DistanceSpline *spline, double distance) {
- return spline->Theta(distance);
- }
-
- double DistanceSplineDTheta(DistanceSpline *spline, double distance) {
- return spline->DTheta(distance);
- }
-
- double DistanceSplineDThetaDt(DistanceSpline *spline, double distance,
- double velocity) {
- return spline->DThetaDt(distance, velocity);
- }
-
- double DistanceSplineDDTheta(DistanceSpline *spline, double distance) {
- return spline->DDTheta(distance);
- }
-
- double DistanceSplineLength(DistanceSpline *spline) {
- return spline->length();
- }
-
- // Based on trajectory.h
- Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
- int num_distance) {
- return new Trajectory(
- spline, ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
- num_distance);
- }
-
- void deleteTrajectory(Trajectory *t) { delete t; }
-
- void TrajectorySetLongitudinalAcceleration(Trajectory *t, double accel) {
- t->set_longitudinal_acceleration(accel);
- }
-
- void TrajectorySetLateralAcceleration(Trajectory *t, double accel) {
- t->set_lateral_acceleration(accel);
- }
-
- void TrajectorySetVoltageLimit(Trajectory *t, double limit) {
- t->set_voltage_limit(limit);
- }
-
- void TrajectoryLimitVelocity(Trajectory *t, double start, double end,
- double max) {
- t->LimitVelocity(start, end, max);
- }
-
- void TrajectoryPlan(Trajectory *t) { t->Plan(); }
-
- void TrajectoryVoltage(Trajectory *t, double distance, double* res) {
- const Eigen::Vector2d ff_voltage = t->FFVoltage(distance);
- res[0] = ff_voltage.x();
- res[1] = ff_voltage.y();
- }
-
- double TrajectoryLength(Trajectory *t) { return t->length(); }
-
- int TrajectoryGetPathLength(Trajectory *t) { return t->plan().size(); }
-
- // This assumes that res is created in python to be getPathLength() long.
- // Likely to SEGFAULT otherwise.
- void TrajectoryDistances(Trajectory *t, double *res) {
- const ::std::vector<double> &distances = t->Distances();
- ::std::memcpy(res, distances.data(), sizeof(double) * distances.size());
- }
-
- double TrajectoryDistance(Trajectory *t, int index) {
- return t->Distance(index);
- }
-
- // This assumes that res is created in python to be getPathLength() long.
- // Likely to SEGFAULT otherwise.
- void TrajectoryGetPlan(Trajectory *t, double *res) {
- const ::std::vector<double> &velocities = t->plan();
- ::std::memcpy(res, velocities.data(), sizeof(double) * velocities.size());
- }
-
- // Time in in nanoseconds.
- ::std::vector<::Eigen::Matrix<double, 3, 1>> *TrajectoryGetPlanXVAPtr(
- Trajectory *t, int dt) {
- return new ::std::vector<::Eigen::Matrix<double, 3, 1>>(
- t->PlanXVA(::std::chrono::nanoseconds(dt)));
- }
-
- void TrajectoryDeleteVector(
- ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
- delete vec;
- }
-
- int TrajectoryGetVectorLength(
- ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
- return vec->size();
- }
-
- void TrajectoryGetPlanXVA(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec,
- double *X, double *V, double *A) {
- for (size_t i = 0; i < vec->size(); ++i) {
- X[i] = (*vec)[i][0];
- V[i] = (*vec)[i][1];
- A[i] = (*vec)[i][2];
- }
- }
-
- // Util
- void SetUpLogging() {
- ::aos::network::OverrideTeamNumber(971);
- }
+// Util
+void SetUpLogging() { ::aos::network::OverrideTeamNumber(971); }
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 1e2c76f..a53538b 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -35,11 +35,11 @@
const DrivetrainConfig<double> &dt_config) {
::Eigen::Matrix<double, 3, 2> result;
result.setZero();
- result.block<2, 2>(1, 0) = dt_config.Tlr_to_la() *
- dt_config.make_hybrid_drivetrain_velocity_loop()
- .plant()
- .coefficients()
- .B_continuous;
+ result.block<2, 2>(1, 0) =
+ dt_config.Tlr_to_la() * dt_config.make_hybrid_drivetrain_velocity_loop()
+ .plant()
+ .coefficients()
+ .B_continuous;
return result;
}
void AB(const DrivetrainConfig<double> &dt_config,
@@ -64,11 +64,10 @@
return BDiscrete;
}
-::Eigen::Matrix<double, 2, 3> CalcK(
- const ::Eigen::Matrix<double, 3, 3> & A,
- const ::Eigen::Matrix<double, 3, 2> & B,
- const ::Eigen::Matrix<double, 3, 3> & Q,
- const ::Eigen::Matrix<double, 2, 2> & R) {
+::Eigen::Matrix<double, 2, 3> CalcK(const ::Eigen::Matrix<double, 3, 3> &A,
+ const ::Eigen::Matrix<double, 3, 2> &B,
+ const ::Eigen::Matrix<double, 3, 3> &Q,
+ const ::Eigen::Matrix<double, 2, 2> &R) {
Eigen::Matrix<double, 2, 3> K;
Eigen::Matrix<double, 3, 3> S;
int info = ::frc971::controls::dlqr<3, 2>(A, B, Q, R, &K, &S);
@@ -229,8 +228,9 @@
const ::Eigen::Matrix<double, 5, 1> rel_state =
(::Eigen::Matrix<double, 5, 1>() << relative_x,
relative_pose_.rel_pos().y(), relative_pose_.rel_theta(),
- abs_state(3, 0), abs_state(4, 0)).finished();
- if (velocity_sign_ * goal_velocity_ < 0) {
+ abs_state(3, 0), abs_state(4, 0))
+ .finished();
+ if (velocity_sign_ * goal_velocity_ < 0) {
goal_theta = rel_state(2, 0);
}
controls_goal_ << goal_theta, goal_velocity_, 0.0;
@@ -269,7 +269,8 @@
line_follow_logging_builder.add_y(target_pose_.abs_pos().y());
line_follow_logging_builder.add_theta(target_pose_.abs_theta());
line_follow_logging_builder.add_offset(relative_pose_.rel_pos().y());
- line_follow_logging_builder.add_distance_to_target(-relative_pose_.rel_pos().x());
+ line_follow_logging_builder.add_distance_to_target(
+ -relative_pose_.rel_pos().x());
line_follow_logging_builder.add_goal_theta(controls_goal_(0, 0));
line_follow_logging_builder.add_rel_theta(relative_pose_.rel_theta());
return line_follow_logging_builder.Finish();
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index 1ebf64d..24f8cd4 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -41,8 +41,7 @@
// Returns whether we set the output. If false, that implies that we do not
// yet have a target to track into and so some other drivetrain should take
// over.
- bool SetOutput(
- ::frc971::control_loops::drivetrain::OutputT *output);
+ bool SetOutput(::frc971::control_loops::drivetrain::OutputT *output);
flatbuffers::Offset<LineFollowLogging> PopulateStatus(
aos::Sender<drivetrain::Status>::Builder *line_follow_logging_builder)
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index a0d374e..b52b67f 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -2,8 +2,6 @@
namespace frc971 {
namespace control_loops {
-namespace drivetrain {
-
-} // namespace drivetrain
+namespace drivetrain {} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/spline_test.cc b/frc971/control_loops/drivetrain/spline_test.cc
index 3ba856d..4fa2001 100644
--- a/frc971/control_loops/drivetrain/spline_test.cc
+++ b/frc971/control_loops/drivetrain/spline_test.cc
@@ -50,7 +50,8 @@
const double alpha =
1.0 * static_cast<double>(i) / static_cast<double>(num_points - 1);
const ::Eigen::Matrix<double, 2, 1> expected_point = spline6_.Point(alpha);
- const ::Eigen::Matrix<double, 2, 1> expected_dpoint = spline6_.DPoint(alpha);
+ const ::Eigen::Matrix<double, 2, 1> expected_dpoint =
+ spline6_.DPoint(alpha);
const ::Eigen::Matrix<double, 2, 1> expected_ddpoint =
spline6_.DDPoint(alpha);
@@ -65,10 +66,10 @@
idy_plot.push_back(dpoint(1));
EXPECT_LT((point - expected_point).norm(), 1e-2) << ": At alpha " << alpha;
- EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2) << ": At alpha "
- << alpha;
- EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-2) << ": At alpha "
- << alpha;
+ EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2)
+ << ": At alpha " << alpha;
+ EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-2)
+ << ": At alpha " << alpha;
// We need to record the starting state without integrating.
if (i == 0) {
@@ -182,12 +183,12 @@
y_plot.push_back(point(1));
EXPECT_LT((point - expected_point).norm(), 1e-9) << ": At alpha " << alpha;
- EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-9) << ": At alpha "
- << alpha;
- EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-9) << ": At alpha "
- << alpha;
- EXPECT_LT((dddpoint - expected_dddpoint).norm(), 1e-9) << ": At alpha "
- << alpha;
+ EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-9)
+ << ": At alpha " << alpha;
+ EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-9)
+ << ": At alpha " << alpha;
+ EXPECT_LT((dddpoint - expected_dddpoint).norm(), 1e-9)
+ << ": At alpha " << alpha;
}
// Conditionally plot the functions and their integrals to aid debugging.
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 05d0d9e..423465a 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -183,8 +183,7 @@
enable_ = enable;
if (enable && current_trajectory_) {
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
- if (!IsAtEnd() &&
- current_spline_handle_ == current_spline_idx_) {
+ if (!IsAtEnd() && current_spline_handle_ == current_spline_idx_) {
has_started_execution_ = true;
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
@@ -213,7 +212,7 @@
state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
- ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2,1>(0,0);
+ ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2, 1>(0, 0);
next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, &xv_state);
next_U_ = U_ff + U_fb - voltage_error;
uncapped_U_ = next_U_;
@@ -237,9 +236,8 @@
output->right_voltage = next_U_(1);
}
-
void SplineDrivetrain::PopulateStatus(
- drivetrain::Status::Builder *builder) const {
+ drivetrain::Status::Builder *builder) const {
if (builder && enable_) {
builder->add_uncapped_left_voltage(uncapped_U_(0));
builder->add_uncapped_right_voltage(uncapped_U_(1));
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 145060d..2ceaac6 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -45,22 +45,20 @@
aos::Sender<drivetrain::Status>::Builder *builder) const;
flatbuffers::Offset<TrajectoryLogging> MakeTrajectoryLogging(
flatbuffers::FlatBufferBuilder *builder) const;
- void PopulateStatus(
- drivetrain::Status::Builder *status) const;
+ void PopulateStatus(drivetrain::Status::Builder *status) const;
// Accessor for the current goal state, pretty much only present for debugging
// purposes.
::Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
- return current_trajectory_
- ? current_trajectory_->GoalState(current_xva_(0),
- current_xva_(1))
- : ::Eigen::Matrix<double, 5, 1>::Zero();
+ return current_trajectory_ ? current_trajectory_->GoalState(current_xva_(0),
+ current_xva_(1))
+ : ::Eigen::Matrix<double, 5, 1>::Zero();
}
bool IsAtEnd() const {
return current_trajectory_
- ? current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0)) :
- true;
+ ? current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0))
+ : true;
}
// Returns true if the splinedrivetrain is enabled.
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 12a6bcc..82c83d2 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -113,8 +113,8 @@
bool is_inside_h, is_inside_45;
const auto adjusted_pos_error_h =
DoCoerceGoal<double>(pos_poly_hv, LH, wh, drive_error, &is_inside_h);
- const auto adjusted_pos_error_45 =
- DoCoerceGoal<double>(pos_poly_hv, L45, w45, intersection, &is_inside_45);
+ const auto adjusted_pos_error_45 = DoCoerceGoal<double>(
+ pos_poly_hv, L45, w45, intersection, &is_inside_45);
if (pos_poly_hv.IsInside(intersection)) {
adjusted_pos_error = adjusted_pos_error_h;
} else {
@@ -131,8 +131,8 @@
}
}
- *U = -U_integral + velocity_K *velocity_error +
- position_K *T_ *adjusted_pos_error + kf_->ff_U();
+ *U = -U_integral + velocity_K * velocity_error +
+ position_K * T_ * adjusted_pos_error + kf_->ff_U();
if (!output_was_capped_) {
if ((*U - kf_->U_uncapped()).norm() > 0.0001) {
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 729415c..ff60b4c 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -45,7 +45,7 @@
void Trajectory::LateralAccelPass() {
for (size_t i = 0; i < plan_.size(); ++i) {
const double distance = Distance(i);
- const double velocity_limit = LateralVelocityCurvature(distance);
+ const double velocity_limit = LateralVelocityCurvature(distance);
if (velocity_limit < plan_[i]) {
plan_[i] = velocity_limit;
plan_segment_type_[i] = CURVATURE_LIMITED;
@@ -102,8 +102,8 @@
}
if (best_accel < min_voltage_accel || best_accel > max_voltage_accel) {
LOG(WARNING) << "Viable friction limits and viable voltage limits do not "
- "overlap (x: " << x << ", v: " << v
- << ", backwards: " << backwards
+ "overlap (x: "
+ << x << ", v: " << v << ", backwards: " << backwards
<< ") best_accel = " << best_accel << ", min voltage "
<< min_voltage_accel << ", max voltage " << max_voltage_accel
<< " min friction " << min_friction_accel << " max friction "
@@ -414,7 +414,7 @@
// by the acceleration/deceleration limits. This may not always be true;
// if we ever encounter this error, we just need to back out what the
// accelerations would be in this case.
- LOG(FATAL) << "Unexpectedly got VOLTAGE_LIMITED plan.";
+ LOG(FATAL) << "Unexpectedly got VOLTAGE_LIMITED plan.";
break;
case SegmentType::ACCELERATION_LIMITED:
// TODO(james): The integration done here and in the DECELERATION_LIMITED
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 0801541..ef02db5 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -225,7 +225,8 @@
const ::Eigen::Matrix<double, 2, 1> K1(double current_ddtheta) const {
return (::Eigen::Matrix<double, 2, 1>()
<< -robot_radius_l_ * current_ddtheta,
- robot_radius_r_ * current_ddtheta).finished();
+ robot_radius_r_ * current_ddtheta)
+ .finished();
}
const ::Eigen::Matrix<double, 2, 1> K2(double current_dtheta) const {
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index f301e9e..2770f63 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -67,16 +67,14 @@
::std::unique_ptr<Trajectory> trajectory_;
::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva_;
- ParameterizedSplineTest()
- : dt_config_(GetTestDrivetrainConfig()) {}
+ ParameterizedSplineTest() : dt_config_(GetTestDrivetrainConfig()) {}
void SetUp() {
distance_spline_ = ::std::unique_ptr<DistanceSpline>(
new DistanceSpline(Spline(GetParam().control_points)));
// Run lots of steps to make the feedforwards terms more accurate.
- trajectory_ = ::std::unique_ptr<Trajectory>(
- new Trajectory(distance_spline_.get(), dt_config_,
- GetParam().velocity_limit));
+ trajectory_ = ::std::unique_ptr<Trajectory>(new Trajectory(
+ distance_spline_.get(), dt_config_, GetParam().velocity_limit));
trajectory_->set_lateral_acceleration(GetParam().lateral_acceleration);
trajectory_->set_longitudinal_acceleration(
GetParam().longitudinal_acceleration);
@@ -85,8 +83,7 @@
GetParam().trajectory_modification_fn(trajectory_.get());
initial_plan_ = trajectory_->plan();
- trajectory_->VoltageFeasibilityPass(
- Trajectory::VoltageLimit::kAggressive);
+ trajectory_->VoltageFeasibilityPass(Trajectory::VoltageLimit::kAggressive);
aggressive_voltage_plan_ = trajectory_->plan();
trajectory_->VoltageFeasibilityPass(
Trajectory::VoltageLimit::kConservative);
@@ -136,7 +133,8 @@
matplotlibcpp::plot(distances, backward_plan_, {{"label", "backward"}});
matplotlibcpp::plot(distances, forward_plan_, {{"label", "forward"}});
matplotlibcpp::plot(distances, curvature_plan_, {{"label", "lateral"}});
- matplotlibcpp::plot(distances, aggressive_voltage_plan_, {{"label", "aggressive voltage"}});
+ matplotlibcpp::plot(distances, aggressive_voltage_plan_,
+ {{"label", "aggressive voltage"}});
matplotlibcpp::plot(distances, voltage_plan_, {{"label", "voltage"}});
matplotlibcpp::plot(distances, initial_plan_, {{"label", "initial"}});
matplotlibcpp::legend();
@@ -187,7 +185,6 @@
::std::vector<double> length_plan_a_;
::std::vector<double> length_plan_vl_;
::std::vector<double> length_plan_vr_;
-
};
// Tests that the VoltageVelocityLimit function produces correct results by
@@ -206,7 +203,7 @@
const ::Eigen::Matrix<double, 2, 2> B =
trajectory_->velocity_drivetrain().plant().coefficients().B_continuous;
const double conservative_limit = trajectory_->VoltageVelocityLimit(
- distance, Trajectory::VoltageLimit::kConservative, &U);
+ distance, Trajectory::VoltageLimit::kConservative, &U);
ASSERT_LT(0.0, conservative_limit)
<< "Voltage limit should be strictly positive.";
const bool on_straight_line = distance_spline_->DTheta(distance) == 0 &&
@@ -245,8 +242,7 @@
const double aggressive_limit = trajectory_->VoltageVelocityLimit(
distance, Trajectory::VoltageLimit::kAggressive, &U);
if (on_straight_line) {
- EXPECT_EQ(::std::numeric_limits<double>::infinity(),
- aggressive_limit);
+ EXPECT_EQ(::std::numeric_limits<double>::infinity(), aggressive_limit);
continue;
}
EXPECT_TRUE((GetParam().voltage_limit == U.array().abs()).all()) << U;
@@ -264,14 +260,12 @@
const ::Eigen::Matrix<double, 2, 1> perturbed_wheel_accels =
K2 * perturbed_accel + K1 * aggressive_limit * aggressive_limit;
const ::Eigen::Matrix<double, 2, 1> perturbed_voltages =
- B.inverse() *
- (perturbed_wheel_accels - A * K2 * aggressive_limit);
+ B.inverse() * (perturbed_wheel_accels - A * K2 * aggressive_limit);
EXPECT_GT(perturbed_voltages.lpNorm<::Eigen::Infinity>(),
GetParam().voltage_limit)
<< "We were able to perturb the voltage!!.";
}
}
-
}
}
@@ -446,7 +440,7 @@
Eigen::Matrix<double, 5, 5> A_continuous;
Eigen::Matrix<double, 5, 2> B_continuous;
trajectory_->PathRelativeContinuousSystem(distance, &A_continuous,
- &B_continuous);
+ &B_continuous);
Eigen::Matrix<double, 5, 5> A_discrete;
Eigen::Matrix<double, 5, 2> B_discrete;
controls::C2D(A_continuous, B_continuous, dt_config_.dt, &A_discrete,
@@ -541,7 +535,8 @@
trajectory->LimitVelocity(1.0, 2.0, 0.5);
}
-void ShortLimitMiddleOfPathTrajectoryModificationFunction(Trajectory *trajectory) {
+void ShortLimitMiddleOfPathTrajectoryModificationFunction(
+ Trajectory *trajectory) {
trajectory->LimitVelocity(1.5, 1.5, 0.5);
}