diff --git a/wpilibc/src/main/native/cpp/simulation/Field2d.cpp b/wpilibc/src/main/native/cpp/simulation/Field2d.cpp deleted file mode 100644 index a6098b67b4..0000000000 --- a/wpilibc/src/main/native/cpp/simulation/Field2d.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 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. */ -/*----------------------------------------------------------------------------*/ - -#include "frc/simulation/Field2d.h" - -using namespace frc; - -Field2d::Field2d() : m_device{"Field2D"} { - if (m_device) { - m_x = m_device.CreateDouble("x", false, 0.0); - m_y = m_device.CreateDouble("y", false, 0.0); - m_rot = m_device.CreateDouble("rot", false, 0.0); - } -} - -void Field2d::SetRobotPose(const Pose2d& pose) { - if (m_device) { - auto& translation = pose.Translation(); - m_x.Set(translation.X().to()); - m_y.Set(translation.Y().to()); - m_rot.Set(pose.Rotation().Degrees().to()); - } else { - m_pose = pose; - } -} - -void Field2d::SetRobotPose(units::meter_t x, units::meter_t y, - Rotation2d rotation) { - if (m_device) { - m_x.Set(x.to()); - m_y.Set(y.to()); - m_rot.Set(rotation.Degrees().to()); - } else { - m_pose = Pose2d{x, y, rotation}; - } -} - -Pose2d Field2d::GetRobotPose() { - if (m_device) { - return Pose2d{units::meter_t{m_x.Get()}, units::meter_t{m_y.Get()}, - Rotation2d{units::degree_t{m_rot.Get()}}}; - } else { - return m_pose; - } -} diff --git a/wpilibc/src/main/native/cpp/smartdashboard/Field2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/Field2d.cpp new file mode 100644 index 0000000000..2911d282ed --- /dev/null +++ b/wpilibc/src/main/native/cpp/smartdashboard/Field2d.cpp @@ -0,0 +1,64 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/smartdashboard/Field2d.h" + +#include "frc/smartdashboard/SendableBuilder.h" + +using namespace frc; + +Field2d::Field2d() { + m_objects.emplace_back( + std::make_unique("Robot", FieldObject2d::private_init{})); + m_objects[0]->SetPose(Pose2d{}); +} + +void Field2d::SetRobotPose(const Pose2d& pose) { + std::scoped_lock lock(m_mutex); + m_objects[0]->SetPose(pose); +} + +void Field2d::SetRobotPose(units::meter_t x, units::meter_t y, + Rotation2d rotation) { + std::scoped_lock lock(m_mutex); + m_objects[0]->SetPose(x, y, rotation); +} + +Pose2d Field2d::GetRobotPose() const { + std::scoped_lock lock(m_mutex); + return m_objects[0]->GetPose(); +} + +FieldObject2d* Field2d::GetObject(const wpi::Twine& name) { + std::scoped_lock lock(m_mutex); + std::string nameStr = name.str(); + for (auto&& obj : m_objects) { + if (obj->m_name == nameStr) return obj.get(); + } + m_objects.emplace_back(std::make_unique( + std::move(nameStr), FieldObject2d::private_init{})); + auto obj = m_objects.back().get(); + if (m_table) obj->m_entry = m_table->GetEntry(obj->m_name); + return obj; +} + +FieldObject2d* Field2d::GetRobotObject() { + std::scoped_lock lock(m_mutex); + return m_objects[0].get(); +} + +void Field2d::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Field2d"); + m_table = builder.GetTable(); + + std::scoped_lock lock(m_mutex); + for (auto&& obj : m_objects) { + std::scoped_lock lock2(obj->m_mutex); + obj->m_entry = m_table->GetEntry(obj->m_name); + obj->UpdateEntry(true); + } +} diff --git a/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp new file mode 100644 index 0000000000..a3e55b907e --- /dev/null +++ b/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp @@ -0,0 +1,80 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/smartdashboard/FieldObject2d.h" + +using namespace frc; + +void FieldObject2d::SetPose(const Pose2d& pose) { + SetPoses(wpi::makeArrayRef(pose)); +} + +void FieldObject2d::SetPose(units::meter_t x, units::meter_t y, + Rotation2d rotation) { + SetPoses(wpi::makeArrayRef(Pose2d{x, y, rotation})); +} + +Pose2d FieldObject2d::GetPose() const { + std::scoped_lock lock(m_mutex); + UpdateFromEntry(); + if (m_poses.empty()) return {}; + return m_poses[0]; +} + +void FieldObject2d::SetPoses(wpi::ArrayRef poses) { + std::scoped_lock lock(m_mutex); + m_poses.assign(poses.begin(), poses.end()); + UpdateEntry(); +} + +void FieldObject2d::SetPoses(std::initializer_list poses) { + SetPoses(wpi::makeArrayRef(poses.begin(), poses.end())); +} + +std::vector FieldObject2d::GetPoses() const { + std::scoped_lock lock(m_mutex); + UpdateFromEntry(); + return std::vector(m_poses.begin(), m_poses.end()); +} + +wpi::ArrayRef FieldObject2d::GetPoses( + wpi::SmallVectorImpl& out) const { + std::scoped_lock lock(m_mutex); + UpdateFromEntry(); + out.assign(m_poses.begin(), m_poses.end()); + return out; +} + +void FieldObject2d::UpdateEntry(bool setDefault) { + if (!m_entry) return; + wpi::SmallVector arr; + for (auto&& pose : m_poses) { + auto& translation = pose.Translation(); + arr.push_back(translation.X().to()); + arr.push_back(translation.Y().to()); + arr.push_back(pose.Rotation().Degrees().to()); + } + if (setDefault) + m_entry.SetDefaultDoubleArray(arr); + else + m_entry.SetDoubleArray(arr); +} + +void FieldObject2d::UpdateFromEntry() const { + if (!m_entry) return; + auto val = m_entry.GetValue(); + if (!val || !val->IsDoubleArray()) return; + auto arr = val->GetDoubleArray(); + auto size = arr.size(); + if ((size % 3) != 0) return; + m_poses.resize(size / 3); + for (size_t i = 0; i < size / 3; ++i) { + m_poses[i] = + Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]}, + Rotation2d{units::degree_t{arr[i * 3 + 2]}}}; + } +} diff --git a/wpilibc/src/main/native/include/frc/simulation/Field2d.h b/wpilibc/src/main/native/include/frc/simulation/Field2d.h deleted file mode 100644 index de308474de..0000000000 --- a/wpilibc/src/main/native/include/frc/simulation/Field2d.h +++ /dev/null @@ -1,70 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 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. */ -/*----------------------------------------------------------------------------*/ - -#pragma once - -#include -#include - -#include "frc/geometry/Pose2d.h" -#include "frc/geometry/Rotation2d.h" - -namespace frc { - -/** - * 2D representation of game field (for simulation). - * - * In non-simulation mode this simply stores and returns the robot pose. - * - * The robot pose is the actual location shown on the simulation view. - * This may or may not match the robot's internal odometry. For example, if - * the robot is shown at a particular starting location, the pose in this - * class would represent the actual location on the field, but the robot's - * internal state might have a 0,0,0 pose (unless it's initialized to - * something different). - * - * As the user is able to edit the pose, code performing updates should get - * the robot pose, transform it as appropriate (e.g. based on simulated wheel - * velocity), and set the new pose. - */ -class Field2d { - public: - Field2d(); - - /** - * Set the robot pose from a Pose object. - * - * @param pose 2D pose - */ - void SetRobotPose(const Pose2d& pose); - - /** - * Set the robot pose from x, y, and rotation. - * - * @param x X location - * @param y Y location - * @param rotation rotation - */ - void SetRobotPose(units::meter_t x, units::meter_t y, Rotation2d rotation); - - /** - * Get the robot pose. - * - * @return 2D pose - */ - Pose2d GetRobotPose(); - - private: - Pose2d m_pose; - - hal::SimDevice m_device; - hal::SimDouble m_x; - hal::SimDouble m_y; - hal::SimDouble m_rot; -}; - -} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/Field2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/Field2d.h new file mode 100644 index 0000000000..d51ebed605 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/smartdashboard/Field2d.h @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/smartdashboard/FieldObject2d.h" +#include "frc/smartdashboard/Sendable.h" + +namespace frc { + +/** + * 2D representation of game field for dashboards. + * + * An object's pose is the location shown on the dashboard view. Note that + * for the robot, this may or may not match the internal odometry. For example, + * if the robot is shown at a particular starting location, the pose in this + * class would represent the actual location on the field, but the robot's + * internal state might have a 0,0,0 pose (unless it's initialized to + * something different). + * + * As the user is able to edit the pose, code performing updates should get + * the robot pose, transform it as appropriate (e.g. based on wheel odometry), + * and set the new pose. + * + * This class provides methods to set the robot pose, but other objects can + * also be shown by using the GetObject() function. Other objects can + * also have multiple poses (which will show the object at multiple locations). + */ +class Field2d : public Sendable { + public: + using Entry = size_t; + + Field2d(); + + /** + * Set the robot pose from a Pose object. + * + * @param pose 2D pose + */ + void SetRobotPose(const Pose2d& pose); + + /** + * Set the robot pose from x, y, and rotation. + * + * @param x X location + * @param y Y location + * @param rotation rotation + */ + void SetRobotPose(units::meter_t x, units::meter_t y, Rotation2d rotation); + + /** + * Get the robot pose. + * + * @return 2D pose + */ + Pose2d GetRobotPose() const; + + /** + * Get or create a field object. + * + * @return Field object + */ + FieldObject2d* GetObject(const wpi::Twine& name); + + /** + * Get the robot object. + * + * @return Field object for robot + */ + FieldObject2d* GetRobotObject(); + + void InitSendable(SendableBuilder& builder) override; + + private: + std::shared_ptr m_table; + + mutable wpi::mutex m_mutex; + std::vector> m_objects; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h new file mode 100644 index 0000000000..f6ad93b4ed --- /dev/null +++ b/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h @@ -0,0 +1,106 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" + +namespace frc { + +class Field2d; + +/** + * Game field object on a Field2d. + */ +class FieldObject2d { + friend class Field2d; + struct private_init {}; + + public: + FieldObject2d(std::string&& name, const private_init&) + : m_name{std::move(name)} {} + + /** + * Set the pose from a Pose object. + * + * @param pose 2D pose + */ + void SetPose(const Pose2d& pose); + + /** + * Set the pose from x, y, and rotation. + * + * @param x X location + * @param y Y location + * @param rotation rotation + */ + void SetPose(units::meter_t x, units::meter_t y, Rotation2d rotation); + + /** + * Get the pose. + * + * @return 2D pose, or 0,0,0 if unknown / does not exist + */ + Pose2d GetPose() const; + + /** + * Set multiple poses from an array of Pose objects. + * The total number of poses is limited to 85. + * + * @param poses array of 2D poses + */ + void SetPoses(wpi::ArrayRef poses); + + /** + * Set multiple poses from an array of Pose objects. + * The total number of poses is limited to 85. + * + * @param obj Object entry + * @param poses array of 2D poses + */ + void SetPoses(std::initializer_list poses); + + /** + * Get multiple poses. + * + * @param obj Object entry + * @return vector of 2D poses + */ + std::vector GetPoses() const; + + /** + * Get multiple poses. + * + * @param obj Object entry + * @param out output SmallVector to hold 2D poses + * @return ArrayRef referring to output SmallVector + */ + wpi::ArrayRef GetPoses(wpi::SmallVectorImpl& out) const; + + private: + void UpdateEntry(bool setDefault = false); + void UpdateFromEntry() const; + + mutable wpi::mutex m_mutex; + std::string m_name; + nt::NetworkTableEntry m_entry; + mutable wpi::SmallVector m_poses; +}; + +} // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp index d8cc462bac..1c44d39eff 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp @@ -12,6 +12,7 @@ #include #include #include +#include using namespace DriveConstants; @@ -53,6 +54,7 @@ void DriveSubsystem::SimulationPeriodic() { -m_drivetrainSimulator.GetHeading().Degrees().to()); m_fieldSim.SetRobotPose(m_odometry.GetPose()); + frc::SmartDashboard::PutData("Field", &m_fieldSim); } units::ampere_t DriveSubsystem::GetCurrentDraw() const { diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h index 2cbc9908a2..8b92bc0c12 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include @@ -169,7 +169,6 @@ class DriveSubsystem : public frc2::SubsystemBase { frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; frc::sim::AnalogGyroSim m_gyroAngleSim{m_gyro}; - // The Field2d class simulates the field in the sim GUI. Note that we can have - // only one instance! + // The Field2d class shows the field in the sim GUI. frc::Field2d m_fieldSim; }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java index ab3d003d1c..209282b616 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java @@ -24,7 +24,7 @@ import edu.wpi.first.wpiutil.math.numbers.N7; /** * This class simulates the state of the drivetrain. In simulationPeriodic, users should first set inputs from motors with * {@link #setInputs(double, double)}, call {@link #update(double)} to update the simulation, - * and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use {@link edu.wpi.first.wpilibj.simulation.Field2d} to + * and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to * visualize their robot on the Sim GUI's field. * *

Our state-space system is: diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java deleted file mode 100644 index bbda95a6ce..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java +++ /dev/null @@ -1,100 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 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. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.simulation; - -import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj.geometry.Rotation2d; -import edu.wpi.first.wpilibj.geometry.Translation2d; - -/** - * 2D representation of game field (for simulation). - * - *

In non-simulation mode this simply stores and returns the robot pose. - * - *

The robot pose is the actual location shown on the simulation view. - * This may or may not match the robot's internal odometry. For example, if - * the robot is shown at a particular starting location, the pose in this - * class would represent the actual location on the field, but the robot's - * internal state might have a 0,0,0 pose (unless it's initialized to - * something different). - * - *

As the user is able to edit the pose, code performing updates should get - * the robot pose, transform it as appropriate (e.g. based on simulated wheel - * velocity), and set the new pose. - */ -public class Field2d { - public Field2d() { - m_device = SimDevice.create("Field2D"); - if (m_device != null) { - m_x = m_device.createDouble("x", false, 0.0); - m_y = m_device.createDouble("y", false, 0.0); - m_rot = m_device.createDouble("rot", false, 0.0); - } - } - - /** - * Set the robot pose from a Pose object. - * - * @param pose 2D pose - */ - public void setRobotPose(Pose2d pose) { - if (m_device != null) { - Translation2d translation = pose.getTranslation(); - m_x.set(translation.getX()); - m_y.set(translation.getY()); - m_rot.set(pose.getRotation().getDegrees()); - } else { - m_pose = pose; - } - } - - /** - * Set the robot pose from x, y, and rotation. - * - * @param xMeters X location, in meters - * @param yMeters Y location, in meters - * @param rotation rotation - */ - public void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) { - if (m_device != null) { - m_x.set(xMeters); - m_y.set(yMeters); - m_rot.set(rotation.getDegrees()); - } else { - m_pose = new Pose2d(xMeters, yMeters, rotation); - } - } - - /** - * Get the robot pose. - * - * @return 2D pose - */ - public Pose2d getRobotPose() { - if (m_device != null) { - return new Pose2d(m_x.get(), m_y.get(), Rotation2d.fromDegrees(m_rot.get())); - } else { - return m_pose; - } - } - - private Pose2d m_pose; - - @SuppressWarnings("MemberName") - private final SimDevice m_device; - - @SuppressWarnings("MemberName") - private SimDouble m_x; - - @SuppressWarnings("MemberName") - private SimDouble m_y; - - private SimDouble m_rot; -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java new file mode 100644 index 0000000000..8e15b772e9 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java @@ -0,0 +1,123 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.smartdashboard; + +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +/** + * 2D representation of game field for dashboards. + * + *

An object's pose is the location shown on the dashboard view. Note that + * for the robot, this may or may not match the internal odometry. For example, + * the robot is shown at a particular starting location, the pose in this + * class would represent the actual location on the field, but the robot's + * internal state might have a 0,0,0 pose (unless it's initialized to + * something different). + * + *

As the user is able to edit the pose, code performing updates should get + * the robot pose, transform it as appropriate (e.g. based on wheel odometry), + * and set the new pose. + * + *

This class provides methods to set the robot pose, but other objects can + * also be shown by using the getObject() function. Other objects can + * also have multiple poses (which will show the object at multiple locations). + */ +public class Field2d implements Sendable { + /** + * Constructor. + */ + public Field2d() { + FieldObject2d obj = new FieldObject2d("Robot"); + obj.setPose(new Pose2d()); + m_objects.add(obj); + } + + /** + * Set the robot pose from a Pose object. + * + * @param pose 2D pose + */ + public synchronized void setRobotPose(Pose2d pose) { + m_objects.get(0).setPose(pose); + } + + /** + * Set the robot pose from x, y, and rotation. + * + * @param xMeters X location, in meters + * @param yMeters Y location, in meters + * @param rotation rotation + */ + @SuppressWarnings("ParameterName") + public synchronized void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) { + m_objects.get(0).setPose(xMeters, yMeters, rotation); + } + + /** + * Get the robot pose. + * + * @return 2D pose + */ + public synchronized Pose2d getRobotPose() { + return m_objects.get(0).getPose(); + } + + /** + * Get or create a field object. + * + * @return Field object + */ + public synchronized FieldObject2d getObject(String name) { + for (FieldObject2d obj : m_objects) { + if (obj.m_name == name) { + return obj; + } + } + FieldObject2d obj = new FieldObject2d(name); + m_objects.add(obj); + if (m_table != null) { + synchronized (obj) { + obj.m_entry = m_table.getEntry(name); + } + } + return obj; + } + + /** + * Get the robot object. + * + * @return Field object for robot + */ + public synchronized FieldObject2d getRobotObject() { + return m_objects.get(0); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Field2d"); + m_table = builder.getTable(); + + synchronized (this) { + for (FieldObject2d obj : m_objects) { + synchronized (obj) { + obj.m_entry = m_table.getEntry(obj.m_name); + obj.updateEntry(true); + } + } + } + } + + private NetworkTable m_table; + private final List m_objects = new ArrayList<>(); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java new file mode 100644 index 0000000000..74d9928821 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java @@ -0,0 +1,153 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.smartdashboard; + +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +/** + * Game field object on a Field2d. + */ +public class FieldObject2d { + /** + * Package-local constructor. + * + * @param name name + */ + FieldObject2d(String name) { + m_name = name; + } + + /** + * Set the pose from a Pose object. + * + * @param pose 2D pose + */ + public synchronized void setPose(Pose2d pose) { + setPoses(pose); + } + + /** + * Set the pose from x, y, and rotation. + * + * @param xMeters X location, in meters + * @param yMeters Y location, in meters + * @param rotation rotation + */ + @SuppressWarnings("ParameterName") + public synchronized void setPose(double xMeters, double yMeters, Rotation2d rotation) { + setPose(new Pose2d(xMeters, yMeters, rotation)); + } + + /** + * Get the pose. + * + * @return 2D pose + */ + public synchronized Pose2d getPose() { + updateFromEntry(); + if (m_poses.isEmpty()) { + return new Pose2d(); + } + return m_poses.get(0); + } + + /** + * Set multiple poses from an list of Pose objects. + * The total number of poses is limited to 85. + * + * @param poses list of 2D poses + */ + public synchronized void setPoses(List poses) { + m_poses.clear(); + for (Pose2d pose : poses) { + m_poses.add(pose); + } + updateEntry(); + } + + /** + * Set multiple poses from an list of Pose objects. + * The total number of poses is limited to 85. + * + * @param poses list of 2D poses + */ + public synchronized void setPoses(Pose2d... poses) { + m_poses.clear(); + for (Pose2d pose : poses) { + m_poses.add(pose); + } + updateEntry(); + } + + /** + * Get multiple poses. + * + * @return list of 2D poses + */ + public synchronized List getPoses() { + updateFromEntry(); + return new ArrayList(m_poses); + } + + void updateEntry() { + updateEntry(false); + } + + synchronized void updateEntry(boolean setDefault) { + if (m_entry == null) { + return; + } + + double[] arr = new double[m_poses.size() * 3]; + int ndx = 0; + for (Pose2d pose : m_poses) { + Translation2d translation = pose.getTranslation(); + arr[ndx + 0] = translation.getX(); + arr[ndx + 1] = translation.getY(); + arr[ndx + 2] = pose.getRotation().getDegrees(); + ndx += 3; + } + + if (setDefault) { + m_entry.setDefaultDoubleArray(arr); + } else { + m_entry.setDoubleArray(arr); + } + } + + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") + private synchronized void updateFromEntry() { + if (m_entry == null) { + return; + } + + double[] arr = m_entry.getDoubleArray((double[]) null); + if (arr == null) { + return; + } + + if ((arr.length % 3) != 0) { + return; + } + + m_poses.clear(); + for (int i = 0; i < arr.length; i += 3) { + m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2]))); + } + } + + String m_name; + NetworkTableEntry m_entry; + private List m_poses; +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java index aaa0d8c29c..3a0d651c73 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java @@ -25,7 +25,8 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; -import edu.wpi.first.wpilibj.simulation.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class DriveSubsystem extends SubsystemBase { @@ -64,8 +65,7 @@ public class DriveSubsystem extends SubsystemBase { public DifferentialDrivetrainSim m_drivetrainSimulator; private EncoderSim m_leftEncoderSim; private EncoderSim m_rightEncoderSim; - // The Field2d class simulates the field in the sim GUI. Note that we can have only one - // instance! + // The Field2d class shows the field in the sim GUI private Field2d m_fieldSim; private SimDouble m_gyroAngleSim; @@ -125,6 +125,7 @@ public class DriveSubsystem extends SubsystemBase { m_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees()); m_fieldSim.setRobotPose(getPose()); + SmartDashboard.putData("Field", m_fieldSim); } /**