diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp index 1a005898eb..5bf054c82e 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp @@ -60,6 +60,9 @@ void Drivetrain::SimulationPeriodic() { m_drivetrainSimulator.GetRightVelocity().to()); m_gyroSim.SetAngle( -m_drivetrainSimulator.GetHeading().Degrees().to()); +} +void Drivetrain::Periodic() { + UpdateOdometry(); m_fieldSim.SetRobotPose(m_odometry.GetPose()); } diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp index c205896370..26abb27d1d 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp @@ -22,6 +22,8 @@ class Robot : public frc::TimedRobot { frc::TrajectoryConfig(2_mps, 2_mps_sq)); } + void RobotPeriodic() override { m_drive.Periodic(); } + void AutonomousInit() override { m_timer.Reset(); m_timer.Start(); @@ -33,8 +35,6 @@ class Robot : public frc::TimedRobot { auto reference = m_trajectory.Sample(elapsed); auto speeds = m_ramsete.Calculate(m_drive.GetPose(), reference); m_drive.Drive(speeds.vx, speeds.omega); - - m_drive.UpdateOdometry(); } void TeleopPeriodic() override { diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h index e01fd823d0..e54a3748c3 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h @@ -64,6 +64,7 @@ class Drivetrain { frc::Pose2d GetPose() const { return m_odometry.GetPose(); } void SimulationPeriodic(); + void Periodic(); private: static constexpr units::meter_t kTrackWidth = 0.381_m * 2; 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 b68b785ac6..1835100f43 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp @@ -22,6 +22,7 @@ DriveSubsystem::DriveSubsystem() { m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); ResetEncoders(); + frc::SmartDashboard::PutData("Field", &m_fieldSim); } void DriveSubsystem::Periodic() { @@ -29,6 +30,7 @@ void DriveSubsystem::Periodic() { m_odometry.Update(m_gyro.GetRotation2d(), units::meter_t(m_leftEncoder.GetDistance()), units::meter_t(m_rightEncoder.GetDistance())); + m_fieldSim.SetRobotPose(m_odometry.GetPose()); } void DriveSubsystem::SimulationPeriodic() { @@ -52,9 +54,6 @@ void DriveSubsystem::SimulationPeriodic() { m_drivetrainSimulator.GetRightVelocity().to()); m_gyroAngleSim.SetAngle( -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/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java index f3ddcf0216..dd32a99cfb 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -138,7 +138,10 @@ public class Drivetrain { m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); + } + public void periodic() { + updateOdometry(); m_fieldSim.setRobotPose(m_odometry.getPoseMeters()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java index f218fcd5b2..1934474973 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java @@ -43,6 +43,11 @@ public class Robot extends TimedRobot { ); } + @Override + public void robotPeriodic() { + m_drive.periodic(); + } + @Override public void autonomousInit() { m_timer.reset(); @@ -56,7 +61,6 @@ public class Robot extends TimedRobot { Trajectory.State reference = m_trajectory.sample(elapsed); ChassisSpeeds speeds = m_ramsete.calculate(m_drive.getPose(), reference); m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); - m_drive.updateOdometry(); } @Override 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 d1f4d22dbc..d014ca35ae 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 @@ -100,6 +100,7 @@ public class DriveSubsystem extends SubsystemBase { // the Field2d class lets us visualize our robot in the simulation GUI. m_fieldSim = new Field2d(); + SmartDashboard.putData("Field", m_fieldSim); } } @@ -108,6 +109,7 @@ public class DriveSubsystem extends SubsystemBase { // Update the odometry in the periodic block m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); + m_fieldSim.setRobotPose(getPose()); } @Override @@ -125,9 +127,6 @@ public class DriveSubsystem extends SubsystemBase { m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); m_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees()); - - m_fieldSim.setRobotPose(getPose()); - SmartDashboard.putData("Field", m_fieldSim); } /**