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 4c6e48259e..871017a0de 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 @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.controller.PIDController; -import edu.wpi.first.wpilibj.examples.swervesdriveposeestimator.ExampleGlobalMeasurementSensor; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java new file mode 100644 index 0000000000..eb83734f0a --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.math.StateSpaceUtil; +import edu.wpi.first.math.util.Units; + +/** This dummy class represents a global measurement sensor, such as a computer vision solution. */ +public final class ExampleGlobalMeasurementSensor { + private ExampleGlobalMeasurementSensor() { + // Utility class + } + + /** + * Get a "noisy" fake global pose reading. + * + * @param estimatedRobotPose The robot pose. + */ + public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) { + var rand = + StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); + return new Pose2d( + estimatedRobotPose.getX() + rand.get(0, 0), + estimatedRobotPose.getY() + rand.get(1, 0), + estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0)))); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index 547dc70dda..24f1d09007 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.controller.PIDController; -import edu.wpi.first.wpilibj.examples.swervesdriveposeestimator.ExampleGlobalMeasurementSensor; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java index 5e214d3ff4..7bed5cff05 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java @@ -33,15 +33,13 @@ public class Robot extends TimedRobot { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. final var xSpeed = - -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) - * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed; + -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) * Drivetrain.kMaxSpeed; // Get the y speed or sideways/strafe speed. We are inverting this because // we want a positive value when we pull to the left. Xbox controllers // return positive values when you pull to the right by default. final var ySpeed = - -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft)) - * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed; + -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft)) * Drivetrain.kMaxSpeed; // Get the rate of angular rotation. We are inverting this because we want a // positive value when we pull to the left (remember, CCW is positive in @@ -49,7 +47,7 @@ public class Robot extends TimedRobot { // the right by default. final var rot = -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) - * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed; + * Drivetrain.kMaxAngularSpeed; m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Robot.java index 70fe05c1f8..5c37aa4a9e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Robot.java @@ -33,15 +33,13 @@ public class Robot extends TimedRobot { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. final var xSpeed = - -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) - * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed; + -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) * Drivetrain.kMaxSpeed; // Get the y speed or sideways/strafe speed. We are inverting this because // we want a positive value when we pull to the left. Xbox controllers // return positive values when you pull to the right by default. final var ySpeed = - -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft)) - * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed; + -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft)) * Drivetrain.kMaxSpeed; // Get the rate of angular rotation. We are inverting this because we want a // positive value when we pull to the left (remember, CCW is positive in @@ -49,7 +47,7 @@ public class Robot extends TimedRobot { // the right by default. final var rot = -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) - * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed; + * Drivetrain.kMaxAngularSpeed; m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative); }