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

Fix DifferentialDriveVoltageConstraint for tight turns w/ zero velocity (#2341)

Browse Source
This commit is contained in:
Oblarg
2020-02-15 15:36:16 -05:00
committed by GitHub
parent a226ad8509
commit 65eab93527
4 changed files with 130 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.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. */
@@ -7,16 +7,21 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.Collections;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertTrue;
class DifferentialDriveVoltageConstraintTest {
@@ -67,4 +72,32 @@ class DifferentialDriveVoltageConstraintTest {
);
}
}
@Test
void testEndpointHighCurvature() {
var feedforward = new SimpleMotorFeedforward(1, 1, 3);
// Large trackwidth - need to test with radius of curvature less than half of trackwidth
var kinematics = new DifferentialDriveKinematics(3);
double maxVoltage = 10;
var constraint = new DifferentialDriveVoltageConstraint(feedforward,
kinematics,
maxVoltage);
var config = new TrajectoryConfig(12, 12).addConstraint(constraint);
// Radius of curvature should be ~1 meter.
assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new ArrayList<Translation2d>(),
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
config));
assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new ArrayList<Translation2d>(),
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
config.setReversed(true)));
}
}