Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix duplicated state when using quintic splines (#2307)

Browse Source 
Generating a trajectory using quintic splines caused a duplicated state at all knot points.
This commit is contained in:
Prateek Machiraju
2020-01-25 02:10:28 -05:00
committed by Peter Johnson
parent 7797da78f5
commit 558c020cca
2 changed files with 15 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -166,7 +166,12 @@ SplineHelper::QuinticControlVectorsFromWaypoints(
const auto scalar =
1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
vectors.push_back(QuinticControlVector(scalar, p0));
// Only add the first control vector if this is the first iteration. The
// last control vector of this iteration is the first control vector of
// the next iteration.
if (i == 0) {
vectors.push_back(QuinticControlVector(scalar, p0));
}
vectors.push_back(QuinticControlVector(scalar, p1));
}
return vectors;

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -69,7 +69,13 @@ public final class SplineHelper {
// This just makes the splines look better.
final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
vectors.add(getQuinticControlVector(scalar, p0));
// Only add the first control vector if this is the first iteration. The
// last control vector of this iteration is the first control vector of
// the next iteration.
if (i == 0) {
vectors.add(getQuinticControlVector(scalar, p0));
}
vectors.add(getQuinticControlVector(scalar, p1));
}
return vectors;