diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp index 866d62d8fd..725074a71a 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp @@ -4,10 +4,31 @@ #include "Drivetrain.h" -#include - #include "ExampleGlobalMeasurementSensor.h" +Drivetrain::Drivetrain() { + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightGroup.SetInverted(true); + + m_gyro.Reset(); + + // Set the distance per pulse for the drive encoders. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_leftEncoder.SetDistancePerPulse( + (2 * std::numbers::pi * kWheelRadius / kEncoderResolution).value()); + m_rightEncoder.SetDistancePerPulse( + (2 * std::numbers::pi * kWheelRadius / kEncoderResolution).value()); + + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); + + frc::SmartDashboard::PutData("FieldSim", &m_fieldSim); + frc::SmartDashboard::PutData("Approximation", &m_fieldApproximation); +} + void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { const auto leftFeedforward = m_feedforward.Calculate(speeds.left); const auto rightFeedforward = m_feedforward.Calculate(speeds.right); @@ -25,16 +46,86 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot})); } +void Drivetrain::PublishCameraToObject( + frc::Pose3d objectInField, frc::Transform3d robotToCamera, + nt::DoubleArrayEntry& cameraToObjectEntry, + frc::sim::DifferentialDrivetrainSim drivetrainSimulator) { + frc::Pose3d robotInField{drivetrainSimulator.GetPose()}; + frc::Pose3d cameraInField = robotInField + robotToCamera; + frc::Transform3d cameraToObject{cameraInField, objectInField}; + + // Publishes double array with Translation3D elements {x, y, z} and Rotation3D + // elements {w, x, y, z} which describe the cameraToObject transformation. + std::array val{cameraToObject.X().value(), + cameraToObject.Y().value(), + cameraToObject.Z().value(), + cameraToObject.Rotation().GetQuaternion().W(), + cameraToObject.Rotation().GetQuaternion().X(), + cameraToObject.Rotation().GetQuaternion().Y(), + cameraToObject.Rotation().GetQuaternion().Z()}; + cameraToObjectEntry.Set(val); +} + +frc::Pose3d Drivetrain::ObjectToRobotPose( + frc::Pose3d objectInField, frc::Transform3d robotToCamera, + nt::DoubleArrayEntry& cameraToObjectEntry) { + std::vector val{cameraToObjectEntry.Get()}; + + // Reconstruct cameraToObject Transform3D from networktables. + frc::Translation3d translation{units::meter_t{val[0]}, units::meter_t{val[1]}, + units::meter_t{val[2]}}; + frc::Rotation3d rotation{frc::Quaternion{val[3], val[4], val[5], val[6]}}; + frc::Transform3d cameraToObject{translation, rotation}; + + return frc::ObjectToRobotPose(objectInField, cameraToObject, robotToCamera); +} + void Drivetrain::UpdateOdometry() { m_poseEstimator.Update(m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()}, units::meter_t{m_rightEncoder.GetDistance()}); - // Also apply vision measurements. We use 0.3 seconds in the past as an - // example -- on a real robot, this must be calculated based either on latency - // or timestamps. - m_poseEstimator.AddVisionMeasurement( - ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose( - m_poseEstimator.GetEstimatedPosition()), - frc::Timer::GetFPGATimestamp() - 0.3_s); + // Publish cameraToObject transformation to networktables --this would + // normally be handled by the computer vision solution. + PublishCameraToObject(m_objectInField, m_robotToCamera, + m_cameraToObjectEntryRef, m_drivetrainSimulator); + + // Compute the robot's field-relative position exclusively from vision + // measurements. + frc::Pose3d visionMeasurement3d = ObjectToRobotPose( + m_objectInField, m_robotToCamera, m_cameraToObjectEntryRef); + + // Convert robot's pose from Pose3d to Pose2d needed to apply vision + // measurements. + frc::Pose2d visionMeasurement2d = visionMeasurement3d.ToPose2d(); + + // Apply vision measurements. For simulation purposes only, we don't input a + // latency delay -- on a real robot, this must be calculated based either on + // known latency or timestamps. + m_poseEstimator.AddVisionMeasurement(visionMeasurement2d, + frc::Timer::GetFPGATimestamp()); +} + +void Drivetrain::SimulationPeriodic() { + // To update our simulation, we set motor voltage inputs, update the + // simulation, and write the simulated positions and velocities to our + // simulated encoder and gyro. + m_drivetrainSimulator.SetInputs(units::volt_t{m_leftGroup.Get()} * + frc::RobotController::GetInputVoltage(), + units::volt_t{m_rightGroup.Get()} * + frc::RobotController::GetInputVoltage()); + m_drivetrainSimulator.Update(20_ms); + + m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value()); + m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value()); + m_rightEncoderSim.SetDistance( + m_drivetrainSimulator.GetRightPosition().value()); + m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value()); + m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value()); +} + +void Drivetrain::Periodic() { + UpdateOdometry(); + m_fieldSim.SetRobotPose(m_drivetrainSimulator.GetPose()); + m_fieldApproximation.SetRobotPose(m_poseEstimator.GetEstimatedPosition()); } diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp index e9119c81ca..324e03f78c 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp @@ -15,6 +15,8 @@ class Robot : public frc::TimedRobot { m_drive.UpdateOdometry(); } + void RobotPeriodic() override { m_drive.Periodic(); } + void TeleopPeriodic() override { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. @@ -31,6 +33,8 @@ class Robot : public frc::TimedRobot { m_drive.Drive(xSpeed, rot); } + void SimulationPeriodic() override { m_drive.SimulationPeriodic(); } + private: frc::XboxController m_controller{0}; diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h index 3001763099..124cdb6a5d 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h @@ -7,13 +7,30 @@ #include #include +#include #include +#include +#include +#include +#include #include #include #include +#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -24,40 +41,100 @@ */ class Drivetrain { public: - Drivetrain() { - // We need to invert one side of the drivetrain so that positive voltages - // result in both sides moving forward. Depending on how your robot's - // gearbox is constructed, you might have to invert the left side instead. - m_rightGroup.SetInverted(true); - - m_gyro.Reset(); - // Set the distance per pulse for the drive encoders. We can simply use the - // distance traveled for one rotation of the wheel divided by the encoder - // resolution. - m_leftEncoder.SetDistancePerPulse( - 2 * std::numbers::pi * kWheelRadius.value() / kEncoderResolution); - m_rightEncoder.SetDistancePerPulse( - 2 * std::numbers::pi * kWheelRadius.value() / kEncoderResolution); - - m_leftEncoder.Reset(); - m_rightEncoder.Reset(); - } + Drivetrain(); static constexpr units::meters_per_second_t kMaxSpeed = 3.0_mps; // 3 meters per second static constexpr units::radians_per_second_t kMaxAngularSpeed{ std::numbers::pi}; // 1/2 rotation per second + /** + * Sets the desired wheel speeds. + * + * @param speeds The desired wheel speeds. + */ void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); + + /** Drives the robot with the given linear velocity and angular velocity. + * + * @param xSpeed Linear velocity. + * @param rot Angular Velocity. + */ void Drive(units::meters_per_second_t xSpeed, units::radians_per_second_t rot); + + /** + * Updates the field-relative position. + */ void UpdateOdometry(); + /** + * This function is called periodically during simulation. */ + void SimulationPeriodic(); + + /** This function is called periodically, regardless of mode. */ + void Periodic(); + + /** + * Computes and publishes to a networktables topic the transformation from + * the camera's pose to the object's pose. This function exists solely for the + * purposes of simulation, and this would normally be handled by computer + * vision. + * + *

The object could be a target or a fiducial marker. + * + * @param objectInField The object's field-relative position. + * @param robotToCamera The transformation from the robot's pose to the + * camera's pose. + * @param cameraToObjectEntry The networktables entry publishing and querying + * example computer vision measurements. + * @param drivetrainSimulation A DifferentialDrivetrainSim modeling the + * robot's drivetrain. + */ + void PublishCameraToObject( + frc::Pose3d objectInField, frc::Transform3d robotToCamera, + nt::DoubleArrayEntry& cameraToObjectEntry, + frc::sim::DifferentialDrivetrainSim drivetrainSimulator); + + /** + * Queries the camera-to-object transformation from networktables to compute + * the robot's field-relative pose from vision measurements. + * + *

The object could be a target or a fiducial marker. + * + * @param objectInField The object's field-relative position. + * @param robotToCamera The transformation from the robot's pose to the + * camera's pose. + * @param cameraToObjectEntry The networktables entry publishing and querying + * example computer vision measurements. + */ + frc::Pose3d ObjectToRobotPose(frc::Pose3d objectInField, + frc::Transform3d robotToCamera, + nt::DoubleArrayEntry& cameraToObjectEntry); + private: static constexpr units::meter_t kTrackWidth = 0.381_m * 2; static constexpr units::meter_t kWheelRadius = 0.0508_m; static constexpr int kEncoderResolution = 4096; + static constexpr std::array kDefaultVal{0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0}; + + frc::Transform3d m_robotToCamera{ + frc::Translation3d{1_m, 1_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}}}; + + nt::NetworkTableInstance m_inst{nt::NetworkTableInstance::GetDefault()}; + nt::DoubleArrayTopic m_cameraToObjectTopic{ + m_inst.GetDoubleArrayTopic("m_cameraToObjectTopic")}; + nt::DoubleArrayEntry m_cameraToObjectEntry = + m_cameraToObjectTopic.GetEntry(kDefaultVal); + nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry; + + frc::AprilTagFieldLayout m_aprilTagFieldLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2022RapidReact)}; + frc::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()}; + frc::PWMSparkMax m_leftLeader{1}; frc::PWMSparkMax m_leftFollower{2}; frc::PWMSparkMax m_rightLeader{3}; @@ -90,4 +167,16 @@ class Drivetrain { // Gains are for example purposes only - must be determined for your own // robot! frc::SimpleMotorFeedforward m_feedforward{1_V, 3_V / 1_mps}; + + // Simulation classes + frc::sim::AnalogGyroSim m_gyroSim{m_gyro}; + frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder}; + frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; + frc::Field2d m_fieldSim; + frc::Field2d m_fieldApproximation; + frc::LinearSystem<2, 2, 2> m_drivetrainSystem = + frc::LinearSystemId::IdentifyDrivetrainSystem( + 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq); + frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ + m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in}; }; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index 1b1808ecdb..e1b36fc067 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -4,21 +4,42 @@ package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.ComputerVisionUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.DoubleArrayTopic; import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.io.IOException; /** Represents a differential drive style drivetrain. */ public class Drivetrain { @@ -50,6 +71,18 @@ public class Drivetrain { private final DifferentialDriveKinematics m_kinematics = new DifferentialDriveKinematics(kTrackWidth); + private final Pose3d m_objectInField; + + private final Transform3d m_robotToCamera = + new Transform3d(new Translation3d(1, 1, 1), new Rotation3d(0, 0, Math.PI / 2)); + + private final DoubleArrayEntry m_cameraToObjectEntry; + + private final double[] m_defaultVal = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + private final Field2d m_fieldSim = new Field2d(); + private final Field2d m_fieldApproximation = new Field2d(); + /* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The numbers used below are robot specific, and should be tuned. */ private final DifferentialDrivePoseEstimator m_poseEstimator = @@ -65,11 +98,21 @@ public class Drivetrain { // Gains are for example purposes only - must be determined for your own robot! private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); + // Simulation classes + private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro); + private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder); + private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder); + private final LinearSystem m_drivetrainSystem = + LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + private final DifferentialDrivetrainSim m_drivetrainSimulator = + new DifferentialDrivetrainSim( + m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null); + /** * Constructs a differential drive object. Sets the encoder distance per pulse and resets the * gyro. */ - public Drivetrain() { + public Drivetrain(DoubleArrayTopic cameraToObjectTopic) { m_gyro.reset(); // We need to invert one side of the drivetrain so that positive voltages @@ -85,6 +128,21 @@ public class Drivetrain { m_leftEncoder.reset(); m_rightEncoder.reset(); + + m_cameraToObjectEntry = cameraToObjectTopic.getEntry(m_defaultVal); + + try { + m_objectInField = + AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile) + .getTagPose(0) + .get(); + } catch (IOException e) { + e.printStackTrace(); + throw new RuntimeException(); + } + + SmartDashboard.putData("Field", m_fieldSim); + SmartDashboard.putData("FieldEstimation", m_fieldApproximation); } /** @@ -115,16 +173,109 @@ public class Drivetrain { setSpeeds(wheelSpeeds); } + /** + * Computes and publishes to a network tables topic the transformation from the camera's pose to + * the object's pose. This function exists solely for the purposes of simulation, and this would + * normally be handled by computer vision. + * + *

The object could be a target or a fiducial marker. + * + * @param objectInField The object's field-relative position. + * @param robotToCamera The transformation from the robot's pose to the camera's pose. + * @param cameraToObjectEntry The networktables entry publishing and querying example computer + * vision measurements. + */ + public void publishCameraToObject( + Pose3d objectInField, + Transform3d robotToCamera, + DoubleArrayEntry cameraToObjectEntry, + DifferentialDrivetrainSim drivetrainSimulator) { + Pose3d robotInField = new Pose3d(drivetrainSimulator.getPose()); + Pose3d cameraInField = robotInField.plus(robotToCamera); + Transform3d cameraToObject = new Transform3d(cameraInField, objectInField); + + // Publishes double array with Translation3D elements {x, y, z} and Rotation3D elements {w, x, + // y, z} which describe + // the cameraToObject transformation. + double[] val = { + cameraToObject.getX(), + cameraToObject.getY(), + cameraToObject.getZ(), + cameraToObject.getRotation().getQuaternion().getW(), + cameraToObject.getRotation().getQuaternion().getX(), + cameraToObject.getRotation().getQuaternion().getY(), + cameraToObject.getRotation().getQuaternion().getZ() + }; + cameraToObjectEntry.set(val); + } + + /** + * Queries the camera-to-object transformation from networktables to compute the robot's + * field-relative pose from vision measurements. + * + *

The object could be a target or a fiducial marker. + * + * @param objectInField The object's field-relative pose. + * @param robotToCamera The transformation from the robot's pose to the camera's pose. + * @param cameraToObjectEntry The networktables entry publishing and querying example computer + * vision measurements. + */ + public Pose3d objectToRobotPose( + Pose3d objectInField, Transform3d robotToCamera, DoubleArrayEntry cameraToObjectEntry) { + double[] val = cameraToObjectEntry.get(); + + // Reconstruct cameraToObject Transform3D from networktables. + Translation3d translation = new Translation3d(val[0], val[1], val[2]); + Rotation3d rotation = new Rotation3d(new Quaternion(val[3], val[4], val[5], val[6])); + Transform3d cameraToObject = new Transform3d(translation, rotation); + + return ComputerVisionUtil.objectToRobotPose(objectInField, cameraToObject, robotToCamera); + } + /** Updates the field-relative position. */ public void updateOdometry() { m_poseEstimator.update( m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); - // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on - // a real robot, this must be calculated based either on latency or timestamps. - m_poseEstimator.addVisionMeasurement( - ExampleGlobalMeasurementSensor.getEstimatedGlobalPose( - m_poseEstimator.getEstimatedPosition()), - Timer.getFPGATimestamp() - 0.3); + // Publish cameraToObject transformation to networktables --this would normally be handled by + // the + // computer vision solution. + publishCameraToObject( + m_objectInField, m_robotToCamera, m_cameraToObjectEntry, m_drivetrainSimulator); + + // Compute the robot's field-relative position exclusively from vision measurements. + Pose3d visionMeasurement3d = + objectToRobotPose(m_objectInField, m_robotToCamera, m_cameraToObjectEntry); + + // Convert robot pose from Pose3d to Pose2d needed to apply vision measurements. + Pose2d visionMeasurement2d = visionMeasurement3d.toPose2d(); + + // Apply vision measurements. For simulation purposes only, we don't input a latency delay -- on + // a real robot, this must be calculated based either on known latency or timestamps. + m_poseEstimator.addVisionMeasurement(visionMeasurement2d, Timer.getFPGATimestamp()); + } + + /** This function is called periodically during simulation. */ + public void simulationPeriodic() { + // To update our simulation, we set motor voltage inputs, update the + // simulation, and write the simulated positions and velocities to our + // simulated encoder and gyro. + m_drivetrainSimulator.setInputs( + m_leftGroup.get() * RobotController.getInputVoltage(), + m_rightGroup.get() * RobotController.getInputVoltage()); + m_drivetrainSimulator.update(0.02); + + m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); + m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); + m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); + m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); + m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); + } + + /** This function is called periodically, no matter the mode. */ + public void periodic() { + updateOdometry(); + m_fieldSim.setRobotPose(m_drivetrainSimulator.getPose()); + m_fieldApproximation.setRobotPose(m_poseEstimator.getEstimatedPosition()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java index 10117244f9..56b13bd04e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java @@ -5,12 +5,18 @@ package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.networktables.DoubleArrayTopic; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; public class Robot extends TimedRobot { + private final NetworkTableInstance m_inst = NetworkTableInstance.getDefault(); + private final DoubleArrayTopic m_doubleArrayTopic = + m_inst.getDoubleArrayTopic("m_doubleArrayTopic"); + private final XboxController m_controller = new XboxController(0); - private final Drivetrain m_drive = new Drivetrain(); + private final Drivetrain m_drive = new Drivetrain(m_doubleArrayTopic); // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3); @@ -22,6 +28,16 @@ public class Robot extends TimedRobot { m_drive.updateOdometry(); } + @Override + public void simulationPeriodic() { + m_drive.simulationPeriodic(); + } + + @Override + public void robotPeriodic() { + m_drive.periodic(); + } + @Override public void teleopPeriodic() { // Get the x speed. We are inverting this because Xbox controllers return