| Austin Schuh | c2b0877 | 2018-12-19 18:05:06 +1100 | [diff] [blame] | 1 | #include "frc971/control_loops/drivetrain/spline.h" |
| 2 | |
| Stephan Pleines | f63bde8 | 2024-01-13 15:59:33 -0800 | [diff] [blame] | 3 | namespace frc971::control_loops::drivetrain { |
| Austin Schuh | c2b0877 | 2018-12-19 18:05:06 +1100 | [diff] [blame] | 4 | |
| Austin Schuh | b23f525 | 2019-01-13 21:16:23 -0800 | [diff] [blame] | 5 | ::Eigen::Matrix<double, 2, 6> Spline4To6( |
| 6 | const ::Eigen::Matrix<double, 2, 4> &control_points) { |
| 7 | ::Eigen::Matrix<double, 2, 6> new_control_points; |
| 8 | // a' = a |
| 9 | // b' = (2a + 3b) / 5 |
| 10 | // c' = (a + 6b + 3c) / 10 |
| 11 | // d' = (d + 6c + 3b) / 10 |
| 12 | // e' = (2d + 3c) / 5 |
| 13 | // f' = d |
| 14 | new_control_points.block<2, 1>(0, 0) = control_points.block<2, 1>(0, 0); |
| 15 | new_control_points.block<2, 1>(0, 1) = |
| 16 | (2.0 * control_points.block<2, 1>(0, 0) + |
| 17 | 3.0 * control_points.block<2, 1>(0, 1)) / |
| 18 | 5.0; |
| 19 | new_control_points.block<2, 1>(0, 2) = |
| 20 | (control_points.block<2, 1>(0, 0) + |
| 21 | 6.0 * control_points.block<2, 1>(0, 1) + |
| 22 | 3.0 * control_points.block<2, 1>(0, 2)) / |
| 23 | 10.0; |
| 24 | new_control_points.block<2, 1>(0, 3) = |
| 25 | (control_points.block<2, 1>(0, 3) + |
| 26 | 6.0 * control_points.block<2, 1>(0, 2) + |
| 27 | 3.0 * control_points.block<2, 1>(0, 1)) / |
| 28 | 10.0; |
| 29 | new_control_points.block<2, 1>(0, 4) = |
| 30 | (2.0 * control_points.block<2, 1>(0, 3) + |
| 31 | 3.0 * control_points.block<2, 1>(0, 2)) / |
| 32 | 5.0; |
| 33 | new_control_points.block<2, 1>(0, 5) = control_points.block<2, 1>(0, 3); |
| 34 | return new_control_points; |
| 35 | } |
| 36 | |
| Stephan Pleines | f63bde8 | 2024-01-13 15:59:33 -0800 | [diff] [blame] | 37 | } // namespace frc971::control_loops::drivetrain |