diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp index 3dc807f57c..522e64da53 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp @@ -6,6 +6,8 @@ #include #include #include +#include +#include #include #include @@ -17,14 +19,23 @@ class Robot : public frc::TimedRobot { // Start the timer. m_timer.Start(); + // Send Field2d to SmartDashboard. + frc::SmartDashboard::PutData(&m_field); + // Reset the drivetrain's odometry to the starting pose of the trajectory. m_drive.ResetOdometry(m_trajectory.InitialPose()); + + // Send our generated trajectory to Field2d. + m_field.GetObject("traj")->SetTrajectory(m_trajectory); } void AutonomousPeriodic() override { // Update odometry. m_drive.UpdateOdometry(); + // Update robot position on Field2d. + m_field.SetRobotPose(m_drive.GetPose()); + if (m_timer.Get() < m_trajectory.TotalTime()) { // Get the desired pose from the trajectory. auto desiredPose = m_trajectory.Sample(m_timer.Get()); @@ -79,6 +90,9 @@ class Robot : public frc::TimedRobot { // The timer to use during the autonomous period. frc2::Timer m_timer; + + // Create Field2d for robot and trajectory visualizations. + frc::Field2d m_field; }; #ifndef RUNNING_FRC_TESTS diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java index 14cb73ba1a..24db270833 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java @@ -17,6 +17,8 @@ import edu.wpi.first.wpilibj.SlewRateLimiter; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.List; public class Robot extends TimedRobot { @@ -36,6 +38,9 @@ public class Robot extends TimedRobot { // The timer to use during the autonomous period. private Timer m_timer; + // Create Field2d for robot and trajectory visualizations. + private Field2d m_field; + @Override public void robotInit() { // Create the trajectory to follow in autonomous. It is best to initialize @@ -46,6 +51,13 @@ public class Robot extends TimedRobot { List.of(new Translation2d(1, 1), new Translation2d(2, -1)), new Pose2d(3, 0, Rotation2d.fromDegrees(0)), new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0))); + + // Create and push Field2d to SmartDashboard. + m_field = new Field2d(); + SmartDashboard.putData(m_field); + + // Push the trajectory to Field2d. + m_field.getObject("traj").setTrajectory(m_trajectory); } @Override @@ -63,6 +75,9 @@ public class Robot extends TimedRobot { // Update odometry. m_drive.updateOdometry(); + // Update robot position on Field2d. + m_field.setRobotPose(m_drive.getPose()); + if (m_timer.get() < m_trajectory.getTotalTimeSeconds()) { // Get the desired pose from the trajectory. var desiredPose = m_trajectory.sample(m_timer.get());