Clang-format drivetrain
Change-Id: I9e20a52ca97b968046969fa2c2e7f42b1fcbb1c3
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