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

[examples] Add Field2d to RamseteController example (#3371)

Browse Source
This commit is contained in:
Dalton Smith
2021-05-22 01:28:29 -04:00
committed by GitHub
parent 4c562a4457
commit 49b06beedf
2 changed files with 29 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java
View File

@@ -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());