diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp index f1a6512bf2..cde0574a2f 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp @@ -69,12 +69,12 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { // An example trajectory to follow. All units in meters. auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory( - // Start at the origin facing the +X direction - frc::Pose2d(), + // Start at (1, 2) facing the +X direction + frc::Pose2d(1_m, 2_m, 0_deg), // Pass through these two interior waypoints, making an 's' curve path - {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)}, + {frc::Translation2d(2_m, 3_m), frc::Translation2d(3_m, 1_m)}, // End 3 meters straight ahead of where we started, facing forward - frc::Pose2d(3_m, 0_m, 0_deg), + frc::Pose2d(4_m, 2_m, 0_deg), // Pass the config config); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java index a0e0d04372..2c8036a122 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java @@ -70,7 +70,7 @@ public final class Constants { public static final double kDriveGearing = 8; // Example value only - as above, this must be tuned for your drive! - public static final double kPDriveVel = 0.1; + public static final double kPDriveVel = 8.5; } public static final class OIConstants { @@ -79,7 +79,7 @@ public final class Constants { public static final class AutoConstants { public static final double kMaxSpeedMetersPerSecond = 3; - public static final double kMaxAccelerationMetersPerSecondSquared = 6; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; // Reasonable baseline values for a RAMSETE follower in units of meters and seconds public static final double kRamseteB = 2; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java index e5e0266163..3bcc444e85 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems.DriveSubsystem; 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.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; @@ -108,12 +109,12 @@ public class RobotContainer { // An example trajectory to follow. All units in meters. Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( - // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(0)), + // Start at (1, 2) facing the +X direction + new Pose2d(1, 2, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path - List.of(), + List.of(new Translation2d(2, 3), new Translation2d(3, 1)), // End 3 meters straight ahead of where we started, facing forward - new Pose2d(6, 6, new Rotation2d(0)), + new Pose2d(4, 2, new Rotation2d(0)), // Pass config config);