Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +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

14
wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp
View File

@@ -6,6 +6,8 @@
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/controller/RamseteController.h>
#include <frc/smartdashboard/Field2d.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc/trajectory/TrajectoryGenerator.h>
#include <frc2/Timer.h>
@@ -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

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