diff --git a/comp/src/main/java/org/team100/frc2025/Elevator/Elevator.java b/comp/src/main/java/org/team100/frc2025/Elevator/Elevator.java index d72762a8..e82bc01f 100644 --- a/comp/src/main/java/org/team100/frc2025/Elevator/Elevator.java +++ b/comp/src/main/java/org/team100/frc2025/Elevator/Elevator.java @@ -8,29 +8,16 @@ import org.team100.lib.config.Identity; import org.team100.lib.config.PIDConstants; import org.team100.lib.dashboard.Glassy; -import org.team100.lib.encoder.AS5048RotaryPositionSensor; -import org.team100.lib.encoder.CombinedEncoder; -import org.team100.lib.encoder.EncoderDrive; -import org.team100.lib.encoder.IncrementalBareEncoder; -import org.team100.lib.encoder.RotaryPositionSensor; import org.team100.lib.encoder.SimulatedBareEncoder; -import org.team100.lib.encoder.SimulatedRotaryPositionSensor; import org.team100.lib.encoder.Talon6Encoder; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.motion.mechanism.LinearMechanism; -import org.team100.lib.motion.mechanism.RotaryMechanism; import org.team100.lib.motion.mechanism.SimpleLinearMechanism; -import org.team100.lib.motion.mechanism.SimpleRotaryMechanism; -import org.team100.lib.motion.servo.AngularPositionServo; -import org.team100.lib.motion.servo.GravityServoInterface; -import org.team100.lib.motion.servo.OutboardAngularPositionServo; -import org.team100.lib.motion.servo.OutboardGravityServo; import org.team100.lib.motion.servo.OutboardLinearPositionServo; import org.team100.lib.motor.Kraken6Motor; import org.team100.lib.motor.MotorPhase; import org.team100.lib.motor.SimulatedBareMotor; import org.team100.lib.profile.TrapezoidProfile100; -import org.team100.lib.state.Control100; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -71,7 +58,8 @@ public Elevator( PIDConstants elevatorPID = PIDConstants.makePositionPID(2); Feedforward100 elevatorFF = Feedforward100.makeKraken6Elevator(); - // TrapezoidProfile100 elevatorProfile = new TrapezoidProfile100(220, 220, 0.05); // TODO CHANGE THESE + // TrapezoidProfile100 elevatorProfile = new TrapezoidProfile100(220, 220, + // 0.05); // TODO CHANGE THESE TrapezoidProfile100 elevatorProfile = new TrapezoidProfile100(100, 100, 0.05); // TODO CHANGE THESE int wristSupplyLimit = 60; @@ -93,7 +81,6 @@ public Elevator( Kraken6Motor portMotor = new Kraken6Motor(portMotorLogger, portID, MotorPhase.REVERSE, elevatorSupplyLimit, elevatorStatorLimit, elevatorPID, elevatorFF); - LinearMechanism starboardMech = new SimpleLinearMechanism( starboardMotor, new Talon6Encoder(starboardLogger, starboardMotor), @@ -110,13 +97,6 @@ public Elevator( portServo = new OutboardLinearPositionServo(portLogger, portMech, elevatorProfile); - - - - - - - break; } default -> { @@ -158,8 +138,8 @@ public void resetWristProfile() { } public void setPosition(double x) { - starboardServo.setPosition(x, 1.3); //54 max - portServo.setPosition(x, 1.3); //54 max + starboardServo.setPosition(x, 1.3); // 54 max + portServo.setPosition(x, 1.3); // 54 max } public void setDutyCycle(double value) { @@ -173,6 +153,4 @@ public double getPosition() { return starboardServo.getPosition().orElse(0); } - - } diff --git a/comp/src/main/java/org/team100/frc2025/Elevator/ElevatorVisualization.java b/comp/src/main/java/org/team100/frc2025/Elevator/ElevatorVisualization.java index 1c822efd..180377ba 100644 --- a/comp/src/main/java/org/team100/frc2025/Elevator/ElevatorVisualization.java +++ b/comp/src/main/java/org/team100/frc2025/Elevator/ElevatorVisualization.java @@ -30,7 +30,6 @@ public ElevatorVisualization(Elevator elevator) { m_elevator = elevator; m_mechanism = new Mechanism2d(100, 100); MechanismRoot2d root = m_mechanism.getRoot("root", 50, 5); - double position = m_elevator.getPosition(); m_base = new MechanismLigament2d( "height", 35, diff --git a/comp/src/main/java/org/team100/frc2025/RobotContainer.java b/comp/src/main/java/org/team100/frc2025/RobotContainer.java index a4055318..a44b721a 100644 --- a/comp/src/main/java/org/team100/frc2025/RobotContainer.java +++ b/comp/src/main/java/org/team100/frc2025/RobotContainer.java @@ -5,24 +5,19 @@ import java.util.Optional; import java.util.function.BooleanSupplier; +import org.team100.frc2025.Climber.Climber; +import org.team100.frc2025.Climber.ClimberFactory; +import org.team100.frc2025.Climber.ClimberRotate; import org.team100.frc2025.Elevator.Elevator; import org.team100.frc2025.Elevator.ElevatorDown; import org.team100.frc2025.Elevator.SetElevator; import org.team100.frc2025.Elevator.SetWrist; import org.team100.frc2025.Elevator.SetWristSafe; -import org.team100.frc2025.Intake.AlgaeIntake; -import org.team100.frc2025.Intake.RunIntake; -import org.team100.frc2025.Intake.RunOuttake; -import org.team100.frc2025.Climber.Climber; -import org.team100.frc2025.Climber.ClimberFactory; -import org.team100.frc2025.Climber.ClimberRotate; - import org.team100.frc2025.Swerve.FullCycle; import org.team100.frc2025.Wrist.Wrist; import org.team100.lib.async.Async; import org.team100.lib.async.AsyncFactory; import org.team100.lib.commands.drivetrain.DriveToPoseSimple; -import org.team100.lib.commands.drivetrain.DriveToPoseWithProfile; import org.team100.lib.commands.drivetrain.DriveToPoseWithTrajectory; import org.team100.lib.commands.drivetrain.DriveToTranslationWithFront; import org.team100.lib.commands.drivetrain.FullCycle2; @@ -42,7 +37,6 @@ import org.team100.lib.controller.simple.PIDFeedback; import org.team100.lib.dashboard.Glassy; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.hid.DriverControl; import org.team100.lib.hid.DriverControlProxy; import org.team100.lib.hid.OperatorControl; @@ -150,7 +144,7 @@ public RobotContainer(TimedRobot100 robot) throws IOException { driveLog, gyro, m_modules.positions(), - GeometryUtil.kPoseZero, + Pose2d.kZero, Takt.get()); final AprilTagFieldLayoutWithCorrectOrientation m_layout = new AprilTagFieldLayoutWithCorrectOrientation(); @@ -287,7 +281,7 @@ public RobotContainer(TimedRobot100 robot) throws IOException { new Rotate(m_drive, holonomicController, swerveKinodynamics, Math.PI / 2)); onTrue(driverControl::resetRotation0, new ResetPose(m_drive, new Pose2d())); - onTrue(driverControl::resetRotation180, new SetRotation(m_drive, GeometryUtil.kRotation180)); + onTrue(driverControl::resetRotation180, new SetRotation(m_drive, Rotation2d.kPi)); // OPERATOR BUTTONS whileTrue(operatorControl::elevate, new SetWristSafe(m_wrist)); //x diff --git a/comp/src/main/java/org/team100/frc2025/RobotContainerParkingLot.java b/comp/src/main/java/org/team100/frc2025/RobotContainerParkingLot.java index 3c44aa0e..7c14fb7b 100644 --- a/comp/src/main/java/org/team100/frc2025/RobotContainerParkingLot.java +++ b/comp/src/main/java/org/team100/frc2025/RobotContainerParkingLot.java @@ -17,7 +17,6 @@ import org.team100.lib.controller.drivetrain.SwerveControllerFactory; import org.team100.lib.dashboard.Glassy; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.hid.DriverControl; import org.team100.lib.hid.DriverControlProxy; import org.team100.lib.hid.OperatorControl; @@ -41,6 +40,7 @@ import org.team100.lib.util.Takt; import org.team100.lib.visualization.TrajectoryVisualization; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -95,7 +95,7 @@ public class RobotContainerParkingLot implements Glassy { driveLogger, m_gyro, m_modules.positions(), - GeometryUtil.kPoseZero, + Pose2d.kZero, Takt.get()); final AprilTagFieldLayoutWithCorrectOrientation m_layout = new AprilTagFieldLayoutWithCorrectOrientation(); final VisionDataProvider24 visionDataProvider = new VisionDataProvider24( diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStationLeft.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStationLeft.java index e6c6abd6..5b518f7a 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStationLeft.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStationLeft.java @@ -4,16 +4,11 @@ import java.util.List; import org.team100.frc2025.FieldConstants; -import org.team100.frc2025.FieldConstants.FieldSector; -import org.team100.frc2025.FieldConstants.ReefAproach; -import org.team100.frc2025.FieldConstants.ReefDestination; import org.team100.frc2025.Swerve.SemiAuto.Navigator; -import org.team100.frc2025.Swerve.SemiAuto.ReefPath; import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; -import org.team100.lib.trajectory.PoseSet; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.visualization.TrajectoryVisualization; diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToIJLeft.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToIJLeft.java index 65036f7a..5c74e19e 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToIJLeft.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToIJLeft.java @@ -5,15 +5,12 @@ import org.team100.frc2025.FieldConstants; import org.team100.frc2025.FieldConstants.FieldSector; -import org.team100.frc2025.FieldConstants.ReefAproach; import org.team100.frc2025.FieldConstants.ReefDestination; import org.team100.frc2025.Swerve.SemiAuto.Navigator; -import org.team100.frc2025.Swerve.SemiAuto.ReefPath; import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; -import org.team100.lib.trajectory.PoseSet; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.visualization.TrajectoryVisualization; diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToReefDestination.java b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToReefDestination.java index 08f8b9a2..a0d756f6 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToReefDestination.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToReefDestination.java @@ -5,15 +5,12 @@ import org.team100.frc2025.FieldConstants; import org.team100.frc2025.FieldConstants.FieldSector; -import org.team100.frc2025.FieldConstants.ReefAproach; import org.team100.frc2025.FieldConstants.ReefDestination; import org.team100.frc2025.Swerve.SemiAuto.Navigator; -import org.team100.frc2025.Swerve.SemiAuto.ReefPath; import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; -import org.team100.lib.trajectory.PoseSet; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.visualization.TrajectoryVisualization; diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/FullCycle.java b/comp/src/main/java/org/team100/frc2025/Swerve/FullCycle.java index 8fe88750..b92589b3 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/FullCycle.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/FullCycle.java @@ -5,7 +5,6 @@ import org.team100.frc2025.FieldConstants; import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.dashboard.Glassy; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; @@ -14,6 +13,7 @@ import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -24,8 +24,8 @@ public class FullCycle extends SequentialCommandGroup implements Glassy { private static final double maxVelocityM_S = 2.0; private static final double maxAccelM_S_S = 2; - private static final Pose2d waypoint0 = new Pose2d(6, 2, GeometryUtil.kRotationZero); - private static final Pose2d waypoint1 = new Pose2d(2, 2, GeometryUtil.kRotationZero); + private static final Pose2d waypoint0 = new Pose2d(6, 2, Rotation2d.kZero); + private static final Pose2d waypoint1 = new Pose2d(2, 2, Rotation2d.kZero); public FullCycle( LoggerFactory parent, diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/Maker.java b/comp/src/main/java/org/team100/frc2025/Swerve/Maker.java index 1b749daa..d6044c78 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/Maker.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/Maker.java @@ -3,11 +3,7 @@ import org.team100.frc2025.FieldConstants.FieldSector; import org.team100.frc2025.FieldConstants.ReefDestination; import org.team100.frc2025.Swerve.Auto.GoToCoralStationLeft; -import org.team100.frc2025.Swerve.Auto.GoToIJLeft; import org.team100.frc2025.Swerve.Auto.GoToReefDestination; -import org.team100.frc2025.Swerve.SemiAuto.Hexagon_Nav.Generate120; -import org.team100.frc2025.Swerve.SemiAuto.Hexagon_Nav.Generate180; -import org.team100.frc2025.Swerve.SemiAuto.Hexagon_Nav.Generate60; import org.team100.frc2025.Swerve.SemiAuto.Profile_Nav.Embark; import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.controller.drivetrain.SwerveControllerFactory; @@ -48,10 +44,10 @@ public Maker( } - public Command test() { - final SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.get(); + public Command embark() { final SwerveController holonomicController = SwerveControllerFactory.ridiculous(m_logger); + final SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.get(); final HolonomicProfile profile = new HolonomicProfile( swerveKinodynamics.getMaxDriveVelocityM_S(), @@ -60,51 +56,50 @@ public Command test() { swerveKinodynamics.getMaxAngleSpeedRad_S(), swerveKinodynamics.getMaxAngleAccelRad_S2(), 0.1); // 5 degrees + return new Embark(m_drive, holonomicController, profile); + } + public Command test() { return new SequentialCommandGroup( - new GoToReefDestination( - m_logger, - m_drive, - SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger), - m_viz, - m_kinodynamics, - FieldSector.IJ, - ReefDestination.RIGHT), - new GoToCoralStationLeft( - m_logger, - m_drive, - SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger), - m_viz, - m_kinodynamics, - 0.5), - new GoToReefDestination( - m_logger, - m_drive, - SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger), - m_viz, - m_kinodynamics, - FieldSector.KL, - ReefDestination.LEFT), - new GoToCoralStationLeft( - m_logger, - m_drive, - SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger), - m_viz, - m_kinodynamics, - 0), - new GoToReefDestination( - m_logger, - m_drive, - SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger), - m_viz, - m_kinodynamics, - FieldSector.KL, - ReefDestination.RIGHT) - ); - - - // return new Embark(m_drive, holonomicController, profile); + new GoToReefDestination( + m_logger, + m_drive, + SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger), + m_viz, + m_kinodynamics, + FieldSector.IJ, + ReefDestination.RIGHT), + new GoToCoralStationLeft( + m_logger, + m_drive, + SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger), + m_viz, + m_kinodynamics, + 0.5), + new GoToReefDestination( + m_logger, + m_drive, + SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger), + m_viz, + m_kinodynamics, + FieldSector.KL, + ReefDestination.LEFT), + new GoToCoralStationLeft( + m_logger, + m_drive, + SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger), + m_viz, + m_kinodynamics, + 0), + new GoToReefDestination( + m_logger, + m_drive, + SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger), + m_viz, + m_kinodynamics, + FieldSector.KL, + ReefDestination.RIGHT)); } diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Hexagon_Nav/Generate60.java b/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Hexagon_Nav/Generate60.java index e1a58112..ed1d5d32 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Hexagon_Nav/Generate60.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Hexagon_Nav/Generate60.java @@ -81,7 +81,7 @@ public Trajectory100 trajectory(Pose2d currentPose) { double dotProduct = ((pointToAnchor.getX()/pointToAnchor.getNorm()) * (vectorToAnchorPoint.getX()/vectorToAnchorPoint.getNorm())) + ((pointToAnchor.getY()/pointToAnchor.getNorm()) * (vectorToAnchorPoint.getY()/vectorToAnchorPoint.getNorm())); - System.out.println(dotProduct); + // System.out.println(dotProduct); double distanceToReef = FieldConstants.getDistanceToReefCenter(currTranslation); diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Hexagon_Nav/LandingDestinationGroup.java b/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Hexagon_Nav/LandingDestinationGroup.java index f845ca5e..39130243 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Hexagon_Nav/LandingDestinationGroup.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Hexagon_Nav/LandingDestinationGroup.java @@ -5,7 +5,6 @@ package org.team100.frc2025.Swerve.SemiAuto.Hexagon_Nav; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; /** Add your docs here. */ public record LandingDestinationGroup(Rotation2d landingSpline, Rotation2d destinationSpline) {} diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Navigator.java b/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Navigator.java index 12f72081..119a8181 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Navigator.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Navigator.java @@ -7,12 +7,7 @@ import org.team100.frc2025.FieldConstants.ReefAproach; import org.team100.lib.controller.drivetrain.ReferenceController; import org.team100.lib.controller.drivetrain.SwerveController; -import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.logging.LoggerFactory.BooleanLogger; -import org.team100.lib.logging.LoggerFactory.DoubleLogger; -import org.team100.lib.logging.LoggerFactory.FieldRelativeVelocityLogger; -import org.team100.lib.logging.LoggerFactory.Pose2dLogger; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; import org.team100.lib.reference.TrajectoryReference; @@ -28,22 +23,7 @@ import edu.wpi.first.wpilibj2.command.Command; public abstract class Navigator extends Command implements Planner2025 { - public static class Log { - public final Pose2dLogger m_log_goal; - public final FieldRelativeVelocityLogger m_log_chassis_speeds; - public final DoubleLogger m_log_THETA_ERROR; - public final BooleanLogger m_log_FINSIHED; - - public Log(LoggerFactory parent) { - LoggerFactory log = parent.child("Navigator"); - m_log_goal = log.pose2dLogger(Level.TRACE, "goal"); - m_log_chassis_speeds = log.fieldRelativeVelocityLogger(Level.TRACE, "speed"); - m_log_THETA_ERROR = log.doubleLogger(Level.TRACE, "THETA ERROR"); - m_log_FINSIHED = log.booleanLogger(Level.TRACE, "FINSIHED"); - } - } - private final Log m_log; public final SwerveDriveSubsystem m_drive; private final SwerveController m_controller; private final TrajectoryVisualization m_viz; @@ -60,7 +40,6 @@ public Navigator( SwerveController controller, TrajectoryVisualization viz, SwerveKinodynamics kinodynamics) { - m_log = new Log(parent); m_drive = drive; m_controller = controller; m_viz = viz; @@ -159,9 +138,8 @@ public Rotation2d calculateInitialSpline(Translation2d targetPoint, Translation2 Translation2d tangentVector = vectorFromCenterToRobot.rotateBy(rotationAngle); - Translation2d tangentVectorAdjusted = tangentVector.times((1 / distanceToReef) * magicNumber); // MAGIC NUMBER - // is a MAGIC - // NUMBER + // MAGIC NUMBER is a MAGIC NUMBER + Translation2d tangentVectorAdjusted = tangentVector.times((1 / distanceToReef) * magicNumber); Translation2d finalVector = translationToTarget.plus(tangentVectorAdjusted); diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Profile_Nav/Embark.java b/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Profile_Nav/Embark.java index 6dda817d..53432fe3 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Profile_Nav/Embark.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Profile_Nav/Embark.java @@ -1,14 +1,10 @@ package org.team100.frc2025.Swerve.SemiAuto.Profile_Nav; -import java.util.Optional; -import java.util.function.Supplier; - import org.team100.frc2025.FieldConstants; import org.team100.frc2025.FieldConstants.FieldSector; import org.team100.lib.controller.drivetrain.ReferenceController; import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.dashboard.Glassy; -import org.team100.lib.logging.FieldLogger; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.SwerveModel; import org.team100.lib.profile.HolonomicProfile; diff --git a/comp/src/main/java/org/team100/frc2025/Wrist/Wrist.java b/comp/src/main/java/org/team100/frc2025/Wrist/Wrist.java index 64be98ed..d56e1df6 100644 --- a/comp/src/main/java/org/team100/frc2025/Wrist/Wrist.java +++ b/comp/src/main/java/org/team100/frc2025/Wrist/Wrist.java @@ -4,8 +4,6 @@ package org.team100.frc2025.Wrist; -import java.lang.Thread.State; -import java.util.ResourceBundle.Control; import org.team100.lib.config.Feedforward100; import org.team100.lib.config.Identity; import org.team100.lib.config.PIDConstants; @@ -14,23 +12,18 @@ import org.team100.lib.encoder.CombinedEncoder; import org.team100.lib.encoder.EncoderDrive; import org.team100.lib.encoder.IncrementalBareEncoder; -import org.team100.lib.encoder.ProxyRotaryPositionSensor; import org.team100.lib.encoder.RotaryPositionSensor; import org.team100.lib.encoder.SimulatedBareEncoder; import org.team100.lib.encoder.SimulatedRotaryPositionSensor; import org.team100.lib.encoder.Talon6Encoder; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.motion.mechanism.LinearMechanism; import org.team100.lib.motion.mechanism.RotaryMechanism; -import org.team100.lib.motion.mechanism.SimpleLinearMechanism; import org.team100.lib.motion.mechanism.SimpleRotaryMechanism; import org.team100.lib.motion.servo.AngularPositionServo; import org.team100.lib.motion.servo.GravityServoInterface; import org.team100.lib.motion.servo.OutboardAngularPositionServo; -import org.team100.lib.motion.servo.OutboardAngularPositionServoWithoutAbsolute; import org.team100.lib.motion.servo.OutboardAngularPositionServoWithoutWrap; import org.team100.lib.motion.servo.OutboardGravityServo; -import org.team100.lib.motion.servo.OutboardLinearPositionServo; import org.team100.lib.motor.Kraken6Motor; import org.team100.lib.motor.MotorPhase; import org.team100.lib.motor.SimulatedBareMotor; diff --git a/comp/src/test/java/org/team100/frc2025/motion/GravityServoTest.java b/comp/src/test/java/org/team100/frc2025/motion/GravityServoTest.java index b582351e..1d3c4694 100644 --- a/comp/src/test/java/org/team100/frc2025/motion/GravityServoTest.java +++ b/comp/src/test/java/org/team100/frc2025/motion/GravityServoTest.java @@ -45,7 +45,11 @@ void testSetPosition() { simMech); ProfiledController controller = new ProfiledController( - profile, pivotFeedback, MathUtil::angleModulus); + profile, + pivotFeedback, + MathUtil::angleModulus, + 0.05, + 0.05); AngularPositionServo servo = new OnboardAngularPositionServo( logger, simMech, diff --git a/lib/src/main/java/org/team100/lib/commands/arm/ArmTrajectoryCommand.java b/lib/src/main/java/org/team100/lib/commands/arm/ArmTrajectoryCommand.java index 93573f4e..e77096d5 100644 --- a/lib/src/main/java/org/team100/lib/commands/arm/ArmTrajectoryCommand.java +++ b/lib/src/main/java/org/team100/lib/commands/arm/ArmTrajectoryCommand.java @@ -159,7 +159,6 @@ public void execute() { final double u1 = ff1 + u1_pos + u1_vel; final double u2 = ff2 + u2_pos + u2_vel; - // TODO: u is v so add kV here m_armSubsystem.set(u1, u2); m_log_Lower_FF.log(() -> ff1); diff --git a/lib/src/main/java/org/team100/lib/commands/arm/CartesianManualPositionalArm.java b/lib/src/main/java/org/team100/lib/commands/arm/CartesianManualPositionalArm.java index a6a56198..38316472 100644 --- a/lib/src/main/java/org/team100/lib/commands/arm/CartesianManualPositionalArm.java +++ b/lib/src/main/java/org/team100/lib/commands/arm/CartesianManualPositionalArm.java @@ -90,8 +90,6 @@ public void execute() { final Translation2d cartesian_measurement = m_kinematics.forward(measurement.get()); - // TODO: there should be some sort of "kA" here rather than just consuming - // the accel value directly. final double u1 = MathUtil.clamp( m_lowerFeedback.calculate( new Model100(measurement.get().th1, 0), diff --git a/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveToPoseWithTrajectory.java b/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveToPoseWithTrajectory.java index 365b959e..d59e9c8d 100644 --- a/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveToPoseWithTrajectory.java +++ b/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveToPoseWithTrajectory.java @@ -52,7 +52,6 @@ public DriveToPoseWithTrajectory( @Override public void initialize() { m_trajectory = m_trajectories.apply(m_drive.getState(), m_goal.get()); - System.out.println(m_drive.getState()); if (m_trajectory.isEmpty()) { m_trajectory = null; return; diff --git a/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveToTranslationWithFront.java b/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveToTranslationWithFront.java index 1d89771e..e48f2662 100644 --- a/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveToTranslationWithFront.java +++ b/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveToTranslationWithFront.java @@ -9,7 +9,6 @@ import org.team100.lib.dashboard.Glassy; import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.FieldLogger; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.SwerveModel; @@ -103,7 +102,7 @@ private static Rotation2d goalTheta(Translation2d goal, Pose2d pose) { switch (Identity.instance) { case COMP_BOT: case BLANK: - return toGoal.plus(GeometryUtil.kRotation180); + return toGoal.plus(Rotation2d.kPi); default: return toGoal; } diff --git a/lib/src/main/java/org/team100/lib/commands/drivetrain/FullCycle2.java b/lib/src/main/java/org/team100/lib/commands/drivetrain/FullCycle2.java index 82d9f5cc..0121663a 100644 --- a/lib/src/main/java/org/team100/lib/commands/drivetrain/FullCycle2.java +++ b/lib/src/main/java/org/team100/lib/commands/drivetrain/FullCycle2.java @@ -4,7 +4,6 @@ import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.dashboard.Glassy; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; @@ -23,8 +22,8 @@ public class FullCycle2 extends SequentialCommandGroup implements Glassy { private static final double maxVelocityM_S = 2.0; private static final double maxAccelM_S_S = 2; - private static final Pose2d waypoint0 = new Pose2d(6, 2, GeometryUtil.kRotationZero); - private static final Pose2d waypoint1 = new Pose2d(2, 2, GeometryUtil.kRotationZero); + private static final Pose2d waypoint0 = new Pose2d(6, 2, Rotation2d.kZero); + private static final Pose2d waypoint1 = new Pose2d(2, 2, Rotation2d.kZero); public FullCycle2( LoggerFactory parent, diff --git a/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DrawSquare.java b/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DrawSquare.java index 1684a97a..c436627f 100644 --- a/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DrawSquare.java +++ b/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DrawSquare.java @@ -5,13 +5,13 @@ import org.team100.lib.commands.drivetrain.DriveToPoseWithTrajectory; import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.dashboard.Glassy; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.timing.ConstantConstraint; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -56,7 +56,7 @@ public DrawSquare( private Command go(double x, double y) { return new DriveToPoseWithTrajectory( - () -> new Pose2d(x, y, GeometryUtil.kRotationZero), + () -> new Pose2d(x, y, Rotation2d.kZero), m_drive, (start, end) -> m_planner.movingToRest(start, end), m_controller, diff --git a/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DriveInACircle.java b/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DriveInACircle.java index 276f0739..2fd8e709 100644 --- a/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DriveInACircle.java +++ b/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DriveInACircle.java @@ -6,7 +6,6 @@ import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.dashboard.Glassy; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; @@ -20,6 +19,7 @@ import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; @@ -132,7 +132,7 @@ public void execute() { } static Translation2d getCenter(Pose2d currentPose, double radiusM) { - return currentPose.transformBy(new Transform2d(0, radiusM, GeometryUtil.kRotationZero)).getTranslation(); + return currentPose.transformBy(new Transform2d(0, radiusM, Rotation2d.kZero)).getTranslation(); } static SwerveControl getReference( diff --git a/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DriveInALittleSquare.java b/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DriveInALittleSquare.java index 5de5429b..4cf6e52f 100644 --- a/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DriveInALittleSquare.java +++ b/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DriveInALittleSquare.java @@ -4,7 +4,6 @@ import org.team100.lib.dashboard.Glassy; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleState100; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleStates; @@ -64,7 +63,7 @@ public DriveInALittleSquare(SwerveDriveSubsystem swerve) { public void initialize() { // First get the wheels pointing the right way. m_state = DriveState.STEERING; - m_goal = GeometryUtil.kRotationZero; + m_goal = Rotation2d.kZero; m_setpoint = kStart; m_setpoint = m_driveProfile.calculate(0, m_setpoint.model(), kGoal); } @@ -77,7 +76,7 @@ public void execute() { && MathUtil.isNear(m_setpoint.v(), kGoal.v(), kVToleranceRad_S)) { // we were driving, but the timer elapsed, so switch to steering m_state = DriveState.STEERING; - m_goal = m_goal.plus(GeometryUtil.kRotation90); + m_goal = m_goal.plus(Rotation2d.kCCW_Pi_2); m_setpoint = kStart; } else { // keep going diff --git a/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/OscillateDirect.java b/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/OscillateDirect.java index 66ad658e..e4b8b36d 100644 --- a/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/OscillateDirect.java +++ b/lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/OscillateDirect.java @@ -3,7 +3,6 @@ import java.util.Optional; import org.team100.lib.dashboard.Glassy; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; @@ -108,37 +107,37 @@ public void execute() { void straight(double speedM_S) { m_drive.setRawModuleStates(new SwerveModuleStates ( - new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)), // FL - new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)), // FR - new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)), // RL - new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)) // RR + new SwerveModuleState100(speedM_S, Optional.of(Rotation2d.kZero)), // FL + new SwerveModuleState100(speedM_S, Optional.of(Rotation2d.kZero)), // FR + new SwerveModuleState100(speedM_S, Optional.of(Rotation2d.kZero)), // RL + new SwerveModuleState100(speedM_S, Optional.of(Rotation2d.kZero)) // RR )); } void sideways(double speedM_S) { m_drive.setRawModuleStates(new SwerveModuleStates ( - new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotation90)), // FL - new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotation90)), // FR - new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotation90)), // RL - new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotation90)) // RR + new SwerveModuleState100(speedM_S, Optional.of(Rotation2d.kCCW_Pi_2)), // FL + new SwerveModuleState100(speedM_S, Optional.of(Rotation2d.kCCW_Pi_2)), // FR + new SwerveModuleState100(speedM_S, Optional.of(Rotation2d.kCCW_Pi_2)), // RL + new SwerveModuleState100(speedM_S, Optional.of(Rotation2d.kCCW_Pi_2)) // RR )); } void back(double speedM_S) { m_drive.setRawModuleStates(new SwerveModuleStates ( - new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)), // FL - new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)), // FR - new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)), // RL - new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)) // RR + new SwerveModuleState100(-speedM_S, Optional.of(Rotation2d.kPi)), // FL + new SwerveModuleState100(-speedM_S, Optional.of(Rotation2d.kPi)), // FR + new SwerveModuleState100(-speedM_S, Optional.of(Rotation2d.kPi)), // RL + new SwerveModuleState100(-speedM_S, Optional.of(Rotation2d.kPi)) // RR )); } void skew(double speedM_S) { m_drive.setRawModuleStates(new SwerveModuleStates ( - new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)), // FL - new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)), // FR - new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)), // RL - new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)) // RR + new SwerveModuleState100(speedM_S, Optional.of(Rotation2d.kZero)), // FL + new SwerveModuleState100(-speedM_S, Optional.of(Rotation2d.kPi)), // FR + new SwerveModuleState100(speedM_S, Optional.of(Rotation2d.kZero)), // RL + new SwerveModuleState100(-speedM_S, Optional.of(Rotation2d.kPi)) // RR )); } diff --git a/lib/src/main/java/org/team100/lib/commands/drivetrain/manual/ManualWithOptionalTargetLock.java b/lib/src/main/java/org/team100/lib/commands/drivetrain/manual/ManualWithOptionalTargetLock.java index 22111f34..b8c75cae 100644 --- a/lib/src/main/java/org/team100/lib/commands/drivetrain/manual/ManualWithOptionalTargetLock.java +++ b/lib/src/main/java/org/team100/lib/commands/drivetrain/manual/ManualWithOptionalTargetLock.java @@ -5,7 +5,6 @@ import org.team100.lib.controller.simple.Feedback100; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.TargetUtil; import org.team100.lib.hid.DriverControl; import org.team100.lib.logging.FieldLogger; @@ -118,7 +117,7 @@ public FieldRelativeVelocity apply( } final Translation2d currentTranslation = state.pose().getTranslation(); - Rotation2d bearing = TargetUtil.bearing(currentTranslation, target.get()).plus(GeometryUtil.kRotation180); + Rotation2d bearing = TargetUtil.bearing(currentTranslation, target.get()).plus(Rotation2d.kPi); final double yaw = state.theta().x(); diff --git a/lib/src/main/java/org/team100/lib/controller/drivetrain/FullStateSwerveController.java b/lib/src/main/java/org/team100/lib/controller/drivetrain/FullStateSwerveController.java index 5a327d2d..183ba60a 100644 --- a/lib/src/main/java/org/team100/lib/controller/drivetrain/FullStateSwerveController.java +++ b/lib/src/main/java/org/team100/lib/controller/drivetrain/FullStateSwerveController.java @@ -1,6 +1,5 @@ package org.team100.lib.controller.drivetrain; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.FieldRelativeDeltaLogger; @@ -11,6 +10,8 @@ import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity; import org.team100.lib.util.Util; +import edu.wpi.first.math.geometry.Rotation2d; + /** * Velocity feedforward, proportional feedback on position and velocity. */ @@ -128,7 +129,7 @@ FieldRelativeVelocity positionFeedback(SwerveModel measurement, SwerveModel curr FieldRelativeVelocity velocityFeedback(SwerveModel currentPose, SwerveModel currentReference) { final FieldRelativeVelocity velocityError = velocityError(currentPose, currentReference); m_atReference &= velocityError.norm() < m_xDotTolerance - && Math.abs(velocityError.angle().orElse(GeometryUtil.kRotationZero).getRadians()) < m_omegaTolerance; + && Math.abs(velocityError.angle().orElse(Rotation2d.kZero).getRadians()) < m_omegaTolerance; final FieldRelativeVelocity u_VFB = new FieldRelativeVelocity( m_kPCartV * velocityError.x(), diff --git a/lib/src/main/java/org/team100/lib/controller/simple/ProfiledController.java b/lib/src/main/java/org/team100/lib/controller/simple/ProfiledController.java index b1420a91..823ed8fd 100644 --- a/lib/src/main/java/org/team100/lib/controller/simple/ProfiledController.java +++ b/lib/src/main/java/org/team100/lib/controller/simple/ProfiledController.java @@ -36,25 +36,26 @@ public record Result(Control100 feedforward, double feedback) { } private static final double kDt = TimedRobot100.LOOP_PERIOD_S; - // TODO: make these constructor args - private static final double kPositionTolerance = 0.05; - private static final double kVelocityTolerance = 0.05; + private final Profile100 m_profile; private final Feedback100 m_feedback; private final DoubleUnaryOperator m_modulus; + private final double m_positionTolerance; + private final double m_velocityTolerance; + private Model100 m_setpoint; - /** - * TODO: include modulus some other way: the profile and feedback need to both - * also be rotary, if this controller is rotary. - */ public ProfiledController( Profile100 profile, Feedback100 feedback, - DoubleUnaryOperator modulus) { + DoubleUnaryOperator modulus, + double positionTolerance, + double velocityTolerance) { m_profile = profile; m_feedback = feedback; m_modulus = modulus; + m_positionTolerance = positionTolerance; + m_velocityTolerance = velocityTolerance; } /** @@ -136,10 +137,10 @@ public boolean atGoal(Model100 goal) { && MathUtil.isNear( goal.x(), setpoint.x(), - kPositionTolerance) + m_positionTolerance) && MathUtil.isNear( goal.v(), setpoint.v(), - kVelocityTolerance); + m_velocityTolerance); } } diff --git a/lib/src/main/java/org/team100/lib/experiments/Experiment.java b/lib/src/main/java/org/team100/lib/experiments/Experiment.java index 518f2ca5..0f35ba51 100644 --- a/lib/src/main/java/org/team100/lib/experiments/Experiment.java +++ b/lib/src/main/java/org/team100/lib/experiments/Experiment.java @@ -66,5 +66,10 @@ public enum Experiment { * starting motion. * TODO(2/24/25) I think this isn't worth the coupling between layers */ - SteerAtRest + SteerAtRest, + /** + * Use pure outboard PID for steering control, rather than the usual profiled + * motion -- it's faster and less work for the RoboRIO. + */ + UnprofiledSteering } diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 9ce16a59..75678bc8 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.MathUtil; 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.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -24,16 +23,6 @@ public class GeometryUtil { private static final boolean DEBUG = false; - public static final Rotation2d kRotationZero = new Rotation2d(); - public static final Rotation2d kRotation90 = new Rotation2d(Math.PI / 2); - public static final Rotation2d kRotation180 = new Rotation2d(Math.PI); - public static final Pose2d kPoseZero = new Pose2d(); - public static final Pose3d kPose3dZero = new Pose3d(); - public static final Translation2d kTranslation2dIdentity = new Translation2d(); - public static final PoseWithCurvature kPose2dWithCurvatureIdentity = new PoseWithCurvature(); - public static final Twist2d kTwist2dIdentity = new Twist2d(0.0, 0.0, 0.0); - public static final Rotation3d kRotation3Zero = new Rotation3d(); - private GeometryUtil() { } @@ -91,11 +80,11 @@ public static Pose2d inverse(Pose2d a) { } public static Twist2d slog(final Pose2d transform) { - return GeometryUtil.kPoseZero.log(transform); + return Pose2d.kZero.log(transform); } public static Pose2d sexp(final Twist2d delta) { - return GeometryUtil.kPoseZero.exp(delta); + return Pose2d.kZero.exp(delta); } public static Pose2d fromRotation(final Rotation2d rotation) { @@ -103,7 +92,7 @@ public static Pose2d fromRotation(final Rotation2d rotation) { } public static Pose2d fromTranslation(final Translation2d translation) { - return new Pose2d(translation, GeometryUtil.kRotationZero); + return new Pose2d(translation, Rotation2d.kZero); } public static Rotation2d fromRadians(double angle_radians) { diff --git a/lib/src/main/java/org/team100/lib/localization/ObjectPosition24ArrayListener.java b/lib/src/main/java/org/team100/lib/localization/ObjectPosition24ArrayListener.java index ce567f51..0cebf496 100644 --- a/lib/src/main/java/org/team100/lib/localization/ObjectPosition24ArrayListener.java +++ b/lib/src/main/java/org/team100/lib/localization/ObjectPosition24ArrayListener.java @@ -29,10 +29,6 @@ /** * Listen for updates from the object-detector camera and remember them for * awhile. - * - * TODO: as described below, use a buffer here, don't just remember the very - * last thing the camera saw. Also use multiple sights to get a better idea of - * where the target is. */ public class ObjectPosition24ArrayListener { /** Ignore sights older than this. */ @@ -87,7 +83,6 @@ public void update() { } Transform3d cameraInRobotCoordinates = Camera.get(fields[1]).getOffset(); Pose2d robotPose = m_poseSupplier.get(v.getServerTime() / 1000000.0).pose(); - // TODO: this should accumulate sights, not replace the list every time. objects = TargetLocalizer.cameraRotsToFieldRelativeArray( robotPose, cameraInRobotCoordinates, diff --git a/lib/src/main/java/org/team100/lib/localization/PoseEstimationHelper.java b/lib/src/main/java/org/team100/lib/localization/PoseEstimationHelper.java index 93cdd37a..13741d6e 100644 --- a/lib/src/main/java/org/team100/lib/localization/PoseEstimationHelper.java +++ b/lib/src/main/java/org/team100/lib/localization/PoseEstimationHelper.java @@ -249,6 +249,6 @@ static Pose3d unapplyCameraOffsetCamera(Pose3d robotPose, Transform3d cameraInRo static Transform3d toTarget(Transform3d cameraInRobotCoordinates, Blip24 blip) { Translation3d t = PoseEstimationHelper.blipToTranslation(blip); return cameraInRobotCoordinates.plus( - new Transform3d(t, GeometryUtil.kRotation3Zero)); + new Transform3d(t, Rotation3d.kZero)); } } diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveControl.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveControl.java index 0889b213..6b9c8b0b 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveControl.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveControl.java @@ -2,7 +2,6 @@ import java.util.Optional; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeAcceleration; import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; @@ -133,7 +132,7 @@ public static SwerveControl fromTimedPose(TimedPose timedPose) { double velocityM_s = timedPose.velocityM_S(); Optional course = timedPose.state().getCourse(); - Rotation2d motion_direction = course.isPresent() ? course.get() : GeometryUtil.kRotationZero; + Rotation2d motion_direction = course.isPresent() ? course.get() : Rotation2d.kZero; double xv = motion_direction.getCos() * velocityM_s; double yv = motion_direction.getSin() * velocityM_s; double thetav = timedPose.state().getHeadingRate() * velocityM_s; diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java index cf17f519..b7cecfe5 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java @@ -4,7 +4,6 @@ import java.util.OptionalDouble; import org.team100.lib.dashboard.Glassy; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.ChassisSpeedsLogger; @@ -31,10 +30,10 @@ public class SwerveLocal implements Glassy, SwerveLocalObserver { private static final double kPositionToleranceRad = 0.05; // about 3 degrees private static final double kVelocityToleranceRad_S = 0.05; // 3 deg/s, slow! private static final SwerveModuleStates states0 = new SwerveModuleStates( - new SwerveModuleState100(0, Optional.of(GeometryUtil.kRotationZero)), - new SwerveModuleState100(0, Optional.of(GeometryUtil.kRotationZero)), - new SwerveModuleState100(0, Optional.of(GeometryUtil.kRotationZero)), - new SwerveModuleState100(0, Optional.of(GeometryUtil.kRotationZero))); + new SwerveModuleState100(0, Optional.of(Rotation2d.kZero)), + new SwerveModuleState100(0, Optional.of(Rotation2d.kZero)), + new SwerveModuleState100(0, Optional.of(Rotation2d.kZero)), + new SwerveModuleState100(0, Optional.of(Rotation2d.kZero))); private static final SwerveModuleStates states90 = new SwerveModuleStates( new SwerveModuleState100(0, Optional.of(new Rotation2d(Math.PI / 2))), new SwerveModuleState100(0, Optional.of(new Rotation2d(Math.PI / 2))), diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveModel.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveModel.java index bfe7cccb..62afafb1 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveModel.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveModel.java @@ -131,7 +131,6 @@ public static SwerveModel fromTimedPose(TimedPose timedPose) { // no course means no velocity. // this is one of the reasons that pure rotations don't work. - // TODO: make pure rotation work return new SwerveModel( new Model100(xx, 0), new Model100(yx, 0), diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveKinodynamics.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveKinodynamics.java index 91279d82..979a5106 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveKinodynamics.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveKinodynamics.java @@ -236,9 +236,6 @@ public static DiscreteSpeed discretize(ChassisSpeeds chassisSpeeds, double dt) { * instead of velocities, it is not needed. * * It performs inverse discretization and an extra correction. - * - * TODO: make sure the callers of this function are doing the right thing with - * the result. */ public ChassisSpeeds toChassisSpeedsWithDiscretization( SwerveModuleStates moduleStates, diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleState100.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleState100.java index fe4babcf..be0cab35 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleState100.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleState100.java @@ -7,11 +7,7 @@ import edu.wpi.first.math.kinematics.struct.SwerveModuleStateStruct; import edu.wpi.first.util.struct.StructSerializable; -/** - * The state of one swerve module. - * - * TODO: This would be better as (dx, dy). - */ +/** The state of one swerve module. */ public class SwerveModuleState100 implements Comparable, StructSerializable { private final double m_speedM_S; private final Optional m_angle; diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/FieldRelativeVelocityLimiter.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/FieldRelativeVelocityLimiter.java index 496ed1b8..eabcf992 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/FieldRelativeVelocityLimiter.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/FieldRelativeVelocityLimiter.java @@ -18,7 +18,6 @@ public FieldRelativeVelocityLimiter(BatterySagSpeedLimit limit) { } public FieldRelativeVelocity limit(FieldRelativeVelocity target) { - // TODO: allow the caller to force rotation preference. if (Experiments.instance.enabled(Experiment.LimitsPreferRotation)) return preferRotation(target); return proportional(target); diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/SwerveDeadband.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/SwerveDeadband.java index 707aec64..7f48d60b 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/SwerveDeadband.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/SwerveDeadband.java @@ -15,7 +15,6 @@ * For now, this is the simplest thing I could think of. */ public class SwerveDeadband { - private static final boolean DEBUG = false; /** 1 cm/s */ private final double m_translationLimit = 0.01; /** 0.01 rad/s */ diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SimulatedSwerveModule100.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SimulatedSwerveModule100.java index c8829854..01a14398 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SimulatedSwerveModule100.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SimulatedSwerveModule100.java @@ -96,7 +96,8 @@ private static AngularPositionServo simulatedTurningServo( Profile100 profile = kinodynamics.getSteeringProfile(); ProfiledController controller = new ProfiledController( - profile, turningPositionFeedback, MathUtil::angleModulus); + profile, turningPositionFeedback, MathUtil::angleModulus, + 0.05,0.05); OnboardAngularPositionServo turningServo = new OnboardAngularPositionServo( parent, turningMech, diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModule100.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModule100.java index 8ca46146..4d15cc65 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModule100.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModule100.java @@ -22,7 +22,6 @@ * Feedforward and feedback control of a single module. */ public abstract class SwerveModule100 implements Glassy { - private static final boolean DEBUG = false; private final LinearVelocityServo m_driveServo; private final AngularPositionServo m_turningServo; /** @@ -169,13 +168,7 @@ public void close() { // Package private for SwerveModuleCollection // - /** - * FOR TESTING ONLY - * - * TODO: remove this - * - * @return current measurements - */ + /** FOR TEST ONLY */ public SwerveModuleState100 getState() { OptionalDouble driveVelocity = m_driveServo.getVelocity(); OptionalDouble turningPosition = m_turningServo.getPosition(); diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModuleCollection.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModuleCollection.java index 8a90a71f..8dad82e9 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModuleCollection.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModuleCollection.java @@ -258,7 +258,7 @@ public OptionalDouble[] turningVelocity() { }; } - /** TODO: remove this */ + /** FOR TEST ONLY */ public SwerveModuleStates states() { return new SwerveModuleStates( m_frontLeft.getState(), diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/WCPSwerveModule100.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/WCPSwerveModule100.java index b4cbd062..dd8da48f 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/WCPSwerveModule100.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/WCPSwerveModule100.java @@ -9,6 +9,8 @@ import org.team100.lib.encoder.EncoderDrive; import org.team100.lib.encoder.RotaryPositionSensor; import org.team100.lib.encoder.Talon6Encoder; +import org.team100.lib.experiments.Experiment; +import org.team100.lib.experiments.Experiments; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; import org.team100.lib.motion.mechanism.LinearMechanism; @@ -19,6 +21,7 @@ import org.team100.lib.motion.servo.LinearVelocityServo; import org.team100.lib.motion.servo.OutboardAngularPositionServo; import org.team100.lib.motion.servo.OutboardLinearVelocityServo; +import org.team100.lib.motion.servo.UnprofiledOutboardAngularPositionServo; import org.team100.lib.motor.Falcon6Motor; import org.team100.lib.motor.Kraken6Motor; import org.team100.lib.motor.MotorPhase; @@ -221,8 +224,6 @@ private static AngularPositionServo turningServo( turningOffset, drive); - Profile100 profile = kinodynamics.getSteeringProfile(); - Talon6Encoder builtInEncoder = new Talon6Encoder( parent, turningMotor); @@ -233,31 +234,35 @@ private static AngularPositionServo turningServo( builtInEncoder, gearRatio); - AngularPositionServo turningServo = getOutboard( + CombinedEncoder combinedEncoder = new CombinedEncoder( parent, turningEncoder, - profile, - mech); + mech, + true); + + AngularPositionServo turningServo = outboardTurningServo( + parent, + kinodynamics, + mech, + combinedEncoder); turningServo.reset(); return turningServo; } - private static AngularPositionServo getOutboard( + private static AngularPositionServo outboardTurningServo( LoggerFactory parent, - RotaryPositionSensor turningEncoder, - Profile100 profile, - RotaryMechanism mech) { - CombinedEncoder combinedEncoder = new CombinedEncoder( - parent, - turningEncoder, - mech, - true); - AngularPositionServo servo = new OutboardAngularPositionServo( + SwerveKinodynamics kinodynamics, + RotaryMechanism mech, + CombinedEncoder combinedEncoder) { + if (Experiments.instance.enabled(Experiment.UnprofiledSteering)) { + return new UnprofiledOutboardAngularPositionServo(parent, mech, combinedEncoder); + } + Profile100 profile = kinodynamics.getSteeringProfile(); + return new OutboardAngularPositionServo( parent, mech, combinedEncoder, profile); - return servo; } private static RotaryPositionSensor turningEncoder( diff --git a/lib/src/main/java/org/team100/lib/motion/mechanism/LimitedLinearMechanism.java b/lib/src/main/java/org/team100/lib/motion/mechanism/LimitedLinearMechanism.java index 482193f8..6c8c315f 100644 --- a/lib/src/main/java/org/team100/lib/motion/mechanism/LimitedLinearMechanism.java +++ b/lib/src/main/java/org/team100/lib/motion/mechanism/LimitedLinearMechanism.java @@ -5,8 +5,6 @@ /** * Proxies a linear mechanism and imposes positional limits, like a software * "limit switch." This only makes sense if the "zero" is set correctly. - * - * TODO: make this class aware of whether it's been "homed" correctly. */ public class LimitedLinearMechanism implements LinearMechanism { private final SimpleLinearMechanism m_delegate; diff --git a/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleLinearMechanism.java b/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleLinearMechanism.java index 77e49100..50187ba7 100644 --- a/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleLinearMechanism.java +++ b/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleLinearMechanism.java @@ -63,8 +63,6 @@ public void setPosition( @Override public OptionalDouble getVelocityM_S() { OptionalDouble velocityRad_S = m_encoder.getVelocityRad_S(); - - //TODO for some reason this number is always zero if (velocityRad_S.isEmpty()) return OptionalDouble.empty(); return OptionalDouble.of(velocityRad_S.getAsDouble() * m_wheelRadiusM / m_gearRatio); diff --git a/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleRotaryMechanism.java b/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleRotaryMechanism.java index 8545995d..815c6d36 100644 --- a/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleRotaryMechanism.java +++ b/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleRotaryMechanism.java @@ -9,8 +9,6 @@ import org.team100.lib.logging.LoggerFactory.DoubleLogger; import org.team100.lib.motor.BareMotor; -import edu.wpi.first.math.MathUtil; - /** * Uses a motor and gears to produce rotational output, e.g. an arm joint. * diff --git a/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServo.java b/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServo.java index ef2eed7d..de1c0200 100644 --- a/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServo.java +++ b/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServo.java @@ -187,8 +187,6 @@ private boolean setpointAtGoal() { * It really makes the most sense to call this *before* updating the setpoint, * because the measurement comes from the recent-past Takt and the updated * setpoint will be aiming at the next one. - * - * TODO: clean this up. */ @Override public boolean atGoal() { diff --git a/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServoWithoutAbsolute.java b/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServoWithoutAbsolute.java index ebb29e5a..7960601c 100644 --- a/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServoWithoutAbsolute.java +++ b/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServoWithoutAbsolute.java @@ -2,7 +2,6 @@ import java.util.OptionalDouble; -import org.team100.lib.encoder.CombinedEncoder; import org.team100.lib.framework.TimedRobot100; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -24,8 +23,6 @@ * delegated to the mechanism. * * Must be used with a combined encoder, to "zero" the motor encoder. - * - * TODO: allow other zeroing strategies. */ public class OutboardAngularPositionServoWithoutAbsolute implements AngularPositionServo { private static final double kPositionTolerance = 0.05; @@ -177,8 +174,6 @@ private boolean setpointAtGoal() { * It really makes the most sense to call this *before* updating the setpoint, * because the measurement comes from the recent-past Takt and the updated * setpoint will be aiming at the next one. - * - * TODO: clean this up. */ @Override public boolean atGoal() { diff --git a/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServoWithoutWrap.java b/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServoWithoutWrap.java index 72ec8dae..62a9bcac 100644 --- a/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServoWithoutWrap.java +++ b/lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServoWithoutWrap.java @@ -187,8 +187,6 @@ private boolean setpointAtGoal() { * It really makes the most sense to call this *before* updating the setpoint, * because the measurement comes from the recent-past Takt and the updated * setpoint will be aiming at the next one. - * - * TODO: clean this up. */ @Override public boolean atGoal() { diff --git a/lib/src/main/java/org/team100/lib/motion/servo/ServoFactory.java b/lib/src/main/java/org/team100/lib/motion/servo/ServoFactory.java index fa36528d..d6997a51 100644 --- a/lib/src/main/java/org/team100/lib/motion/servo/ServoFactory.java +++ b/lib/src/main/java/org/team100/lib/motion/servo/ServoFactory.java @@ -115,7 +115,9 @@ public static AngularPositionServo neoVortexAngleServo( ProfiledController controller = new ProfiledController( new TrapezoidProfile100(maxVelocity, maxAccel, 0.05), feedback, - MathUtil::angleModulus); + MathUtil::angleModulus, + 0.05, + 0.05); return new OnboardAngularPositionServo( parent, mech, @@ -148,7 +150,9 @@ public static AngularPositionServo simulatedAngleServo( ProfiledController controller = new ProfiledController( new TrapezoidProfile100(maxVelocity, maxAccel, 0.05), feedback, - MathUtil::angleModulus); + MathUtil::angleModulus, + 0.05, + 0.05); return new OnboardAngularPositionServo( parent, mech, diff --git a/lib/src/main/java/org/team100/lib/motion/servo/UnprofiledOutboardAngularPositionServo.java b/lib/src/main/java/org/team100/lib/motion/servo/UnprofiledOutboardAngularPositionServo.java index 94f847c2..ce9c59f2 100644 --- a/lib/src/main/java/org/team100/lib/motion/servo/UnprofiledOutboardAngularPositionServo.java +++ b/lib/src/main/java/org/team100/lib/motion/servo/UnprofiledOutboardAngularPositionServo.java @@ -139,8 +139,6 @@ public boolean atSetpoint() { * It really makes the most sense to call this *before* updating the setpoint, * because the measurement comes from the recent-past Takt and the updated * setpoint will be aiming at the next one. - * - * TODO: clean this up. */ @Override public boolean atGoal() { diff --git a/lib/src/main/java/org/team100/lib/path/PathPlanner.java b/lib/src/main/java/org/team100/lib/path/PathPlanner.java index 79e9ead4..4eeb02e4 100644 --- a/lib/src/main/java/org/team100/lib/path/PathPlanner.java +++ b/lib/src/main/java/org/team100/lib/path/PathPlanner.java @@ -27,7 +27,6 @@ public static Path100 pathFromWaypointsAndHeadings( headings.get(i - 1), headings.get(i), mN.get(i))); } // does not force C1, theta responds too much - // TODO: find a way to make theta C1 // SplineUtil.forceC1(splines); SplineUtil.optimizeSpline(splines); return new Path100(SplineGenerator.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); @@ -46,7 +45,6 @@ public static Path100 pathFromWaypointsAndHeadings( headings.get(i - 1), headings.get(i))); } // does not force C1, theta responds too much - // TODO: find a way to make theta C1 // SplineUtil.forceC1(splines); SplineUtil.optimizeSpline(splines); return new Path100(SplineGenerator.parameterizeSplines(splines, maxDx, maxDy, maxDTheta)); diff --git a/lib/src/main/java/org/team100/lib/profile/HolonomicProfile.java b/lib/src/main/java/org/team100/lib/profile/HolonomicProfile.java index a4e45323..604c14d8 100644 --- a/lib/src/main/java/org/team100/lib/profile/HolonomicProfile.java +++ b/lib/src/main/java/org/team100/lib/profile/HolonomicProfile.java @@ -16,8 +16,6 @@ * * Note that because acceleration is adjusted, but not cruise velocity, the * resulting paths will not be straight, for rest-to-rest profiles. - * - * TODO: support other kinds of profiles, e.g. exponential */ public class HolonomicProfile { /** For testing */ diff --git a/lib/src/main/java/org/team100/lib/profile/Profile100.java b/lib/src/main/java/org/team100/lib/profile/Profile100.java index 9e5bd3e5..0942a050 100644 --- a/lib/src/main/java/org/team100/lib/profile/Profile100.java +++ b/lib/src/main/java/org/team100/lib/profile/Profile100.java @@ -17,8 +17,6 @@ public static record ResultWithETA(Control100 state, double etaS) { * Return the control for dt in the future. * * Note order here, initial first, goal second. - * - * TODO: remove dt, use TimedRobot100.LOOP_PERIOD_S */ Control100 calculate(double dt, Model100 initial, Model100 goal); @@ -26,9 +24,6 @@ public static record ResultWithETA(Control100 state, double etaS) { * Return the control for dt in the future. * * Note order here, initial first, goal second. - * - * TODO: remove dt, use TimedRobot100.LOOP_PERIOD_S - * */ ResultWithETA calculateWithETA(double dt, Model100 initial, Model100 goal); diff --git a/lib/src/main/java/org/team100/lib/profile/TrapezoidProfile100.java b/lib/src/main/java/org/team100/lib/profile/TrapezoidProfile100.java index 4081b479..baefbf98 100644 --- a/lib/src/main/java/org/team100/lib/profile/TrapezoidProfile100.java +++ b/lib/src/main/java/org/team100/lib/profile/TrapezoidProfile100.java @@ -98,9 +98,6 @@ public double solve(double dt, Model100 i, Model100 g, double eta, double etaTol * returns s < 0.01. * * It is very approximate, in order to not run too long. It's very primitive. - * - * TODO: this appears to work differently for positive and negative initial - * velocities. */ public static double solveForSlowerETA( double maxV, diff --git a/lib/src/main/java/org/team100/lib/sensors/SimulatedGyro.java b/lib/src/main/java/org/team100/lib/sensors/SimulatedGyro.java index bee52f8d..680f3678 100644 --- a/lib/src/main/java/org/team100/lib/sensors/SimulatedGyro.java +++ b/lib/src/main/java/org/team100/lib/sensors/SimulatedGyro.java @@ -1,6 +1,5 @@ package org.team100.lib.sensors; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleStates; import org.team100.lib.motion.drivetrain.module.SwerveModuleCollection; @@ -45,12 +44,12 @@ public double getYawRateNWU() { @Override public Rotation2d getPitchNWU() { - return GeometryUtil.kRotationZero; + return Rotation2d.kZero; } @Override public Rotation2d getRollNWU() { - return GeometryUtil.kRotationZero; + return Rotation2d.kZero; } @Override diff --git a/lib/src/main/java/org/team100/lib/spline/SplineGenerator.java b/lib/src/main/java/org/team100/lib/spline/SplineGenerator.java index 67b31c73..4921c455 100755 --- a/lib/src/main/java/org/team100/lib/spline/SplineGenerator.java +++ b/lib/src/main/java/org/team100/lib/spline/SplineGenerator.java @@ -64,9 +64,9 @@ private static void getSegmentArc( Pose2d p0 = s.getPose2d(t0); Pose2d phalf = s.getPose2d(t0 + (t1 - t0) * .5); Pose2d p1 = s.getPose2d(t1); - Twist2d twist_full = GeometryUtil.kPoseZero.log(GeometryUtil.transformBy(GeometryUtil.inverse(p0), p1)); + Twist2d twist_full = Pose2d.kZero.log(GeometryUtil.transformBy(GeometryUtil.inverse(p0), p1)); Pose2d phalf_predicted = GeometryUtil.transformBy(p0, - GeometryUtil.kPoseZero.exp(GeometryUtil.scale(twist_full, 0.5))); + Pose2d.kZero.exp(GeometryUtil.scale(twist_full, 0.5))); Pose2d error = GeometryUtil.transformBy(GeometryUtil.inverse(phalf), phalf_predicted); if (GeometryUtil.norm(twist_full) < 1e-6) { diff --git a/lib/src/main/java/org/team100/lib/spline/SplineUtil.java b/lib/src/main/java/org/team100/lib/spline/SplineUtil.java index 62ee660c..c6e0d714 100644 --- a/lib/src/main/java/org/team100/lib/spline/SplineUtil.java +++ b/lib/src/main/java/org/team100/lib/spline/SplineUtil.java @@ -14,7 +14,6 @@ public class SplineUtil { private static final boolean DEBUG = false; private static final double kEpsilon = 1e-5; - // TODO: find a good step size. private static final double kStepSize = 1.0; private static final double kMinDelta = 0.001; private static final int kMaxIterations = 100; @@ -371,9 +370,6 @@ static double getControlPoints( splines.set(i + 1, splines.get(i + 1).addToSecondDerivatives(0, 0, kEpsilon, 0)); controlPoints[i].ddy = (sumDCurvature2(splines) - original) / kEpsilon; - // TODO: ??? - splines.set(i, splines.get(i)); - splines.set(i + 1, splines.get(i + 1)); magnitude += controlPoints[i].ddx * controlPoints[i].ddx + controlPoints[i].ddy * controlPoints[i].ddy; } return magnitude; diff --git a/lib/src/main/java/org/team100/lib/state/Model100.java b/lib/src/main/java/org/team100/lib/state/Model100.java index c006f9fa..00b25a82 100644 --- a/lib/src/main/java/org/team100/lib/state/Model100.java +++ b/lib/src/main/java/org/team100/lib/state/Model100.java @@ -10,9 +10,6 @@ * * The usual state-space representation would be X = (x,v) and Xdot = (v,a). * Units are meters, radians, and seconds. - * - * TODO: include modulus here: the "x" is always either periodic (rotary) or - * not. */ public class Model100 { private final double m_x; diff --git a/lib/src/main/java/org/team100/lib/timing/SwerveDriveDynamicsConstraint.java b/lib/src/main/java/org/team100/lib/timing/SwerveDriveDynamicsConstraint.java index d4e6e8ba..d00a119a 100644 --- a/lib/src/main/java/org/team100/lib/timing/SwerveDriveDynamicsConstraint.java +++ b/lib/src/main/java/org/team100/lib/timing/SwerveDriveDynamicsConstraint.java @@ -4,7 +4,6 @@ import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleState100; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleStates; import org.team100.lib.motion.drivetrain.kinodynamics.limiter.SwerveUtil; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; @@ -37,7 +36,7 @@ public NonNegativeDouble getMaxVelocity(Pose2dWithMotion state) { // velocity. Optional course = state.getCourse(); Rotation2d course_local = state.getHeading().unaryMinus() - .rotateBy(course.isPresent() ? course.get() : GeometryUtil.kRotationZero); + .rotateBy(course.isPresent() ? course.get() : Rotation2d.kZero); double vx = course_local.getCos(); double vy = course_local.getSin(); // rad/m diff --git a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java index 2bc489d2..7ec4e831 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java +++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java @@ -3,7 +3,6 @@ import java.util.List; import java.util.function.Function; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.motion.drivetrain.SwerveModel; import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity; import org.team100.lib.path.Path100; @@ -38,9 +37,9 @@ public TrajectoryPlanner(List constraints) { /** A square counterclockwise starting with +x. */ public List square(Pose2d p0) { - Pose2d p1 = p0.plus(new Transform2d(1, 0, GeometryUtil.kRotationZero)); - Pose2d p2 = p0.plus(new Transform2d(1, 1, GeometryUtil.kRotationZero)); - Pose2d p3 = p0.plus(new Transform2d(0, 1, GeometryUtil.kRotationZero)); + Pose2d p1 = p0.plus(new Transform2d(1, 0, Rotation2d.kZero)); + Pose2d p2 = p0.plus(new Transform2d(1, 1, Rotation2d.kZero)); + Pose2d p3 = p0.plus(new Transform2d(0, 1, Rotation2d.kZero)); return List.of( restToRest(p0, p1), restToRest(p1, p2), @@ -51,17 +50,17 @@ public List square(Pose2d p0) { /** Make a square that gets a reset starting point at each corner. */ public List> permissiveSquare() { return List.of( - x -> restToRest(x, x.plus(new Transform2d(1, 0, GeometryUtil.kRotationZero))), - x -> restToRest(x, x.plus(new Transform2d(0, 1, GeometryUtil.kRotationZero))), - x -> restToRest(x, x.plus(new Transform2d(-1, 0, GeometryUtil.kRotationZero))), - x -> restToRest(x, x.plus(new Transform2d(0, -1, GeometryUtil.kRotationZero)))); + x -> restToRest(x, x.plus(new Transform2d(1, 0, Rotation2d.kZero))), + x -> restToRest(x, x.plus(new Transform2d(0, 1, Rotation2d.kZero))), + x -> restToRest(x, x.plus(new Transform2d(-1, 0, Rotation2d.kZero))), + x -> restToRest(x, x.plus(new Transform2d(0, -1, Rotation2d.kZero)))); } /** From current to x+1 */ public Trajectory100 line(Pose2d initial) { return restToRest( initial, - initial.plus(new Transform2d(1, 0, GeometryUtil.kRotationZero))); + initial.plus(new Transform2d(1, 0, Rotation2d.kZero))); } public Trajectory100 restToRest( @@ -102,9 +101,7 @@ public Trajectory100 movingToMoving(SwerveModel startState, SwerveModel endState Translation2d goalTranslation = endState.translation(); Translation2d translationToGoal = goalTranslation.minus(currentTranslation); Rotation2d angleToGoal = translationToGoal.getAngle(); - System.out.println(angleToGoal); Rotation2d startingAngle = currentSpeed.angle().orElse(angleToGoal); - System.out.println(startingAngle); try { return generateTrajectory( List.of( diff --git a/lib/src/main/java/org/team100/lib/util/Memo.java b/lib/src/main/java/org/team100/lib/util/Memo.java index 890a247f..ba973e4c 100644 --- a/lib/src/main/java/org/team100/lib/util/Memo.java +++ b/lib/src/main/java/org/team100/lib/util/Memo.java @@ -21,7 +21,6 @@ * doesn't need to apply its own cache layer. */ public class Memo { - // TODO: use weak references here private static final List> resetters = new ArrayList<>(); private static final List> updaters = new ArrayList<>(); private static final List doubleResetters = new ArrayList<>(); diff --git a/lib/src/main/java/org/team100/lib/util/Takt.java b/lib/src/main/java/org/team100/lib/util/Takt.java index 2c45e78c..e32f538f 100644 --- a/lib/src/main/java/org/team100/lib/util/Takt.java +++ b/lib/src/main/java/org/team100/lib/util/Takt.java @@ -37,7 +37,6 @@ public static void update() { /** * The current Takt time. Even though this is a double, it's ok to test * equality, because it is only incremented periodically. - * TODO: use different type. */ public static double get() { return now; diff --git a/lib/src/main/java/org/team100/lib/util/struct/OptionalRotation2dStruct.java b/lib/src/main/java/org/team100/lib/util/struct/OptionalRotation2dStruct.java index 343ce4a8..944ead2a 100644 --- a/lib/src/main/java/org/team100/lib/util/struct/OptionalRotation2dStruct.java +++ b/lib/src/main/java/org/team100/lib/util/struct/OptionalRotation2dStruct.java @@ -3,7 +3,6 @@ import java.nio.ByteBuffer; import java.util.Optional; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.util.OptionalRotation2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -55,7 +54,7 @@ public void pack(ByteBuffer bb, OptionalRotation2d value) { Rotation2d.struct.pack(bb, optional.get()); } else { bb.put((byte) 0x00); - Rotation2d.struct.pack(bb, GeometryUtil.kRotationZero); + Rotation2d.struct.pack(bb, Rotation2d.kZero); } } diff --git a/lib/src/test/java/org/team100/lib/commands/arm/ArmTrajectoryCommandTest.java b/lib/src/test/java/org/team100/lib/commands/arm/ArmTrajectoryCommandTest.java index b6e6d27d..c3971709 100644 --- a/lib/src/test/java/org/team100/lib/commands/arm/ArmTrajectoryCommandTest.java +++ b/lib/src/test/java/org/team100/lib/commands/arm/ArmTrajectoryCommandTest.java @@ -5,7 +5,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -16,6 +15,7 @@ import org.team100.lib.testing.Timeless; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; @@ -83,7 +83,7 @@ void testPosRefernce() { armKinematicsM, goal); Trajectory.State s = new Trajectory.State(); - s.poseMeters = new Pose2d(1, 1, GeometryUtil.kRotationZero); + s.poseMeters = new Pose2d(1, 1, Rotation2d.kZero); ArmAngles r = command.getThetaPosReference(s); assertEquals(0, r.th1, kDelta); assertEquals(Math.PI / 2, r.th2, kDelta); @@ -101,7 +101,7 @@ void testVelRefernce() { goal); Trajectory.State s = new Trajectory.State(); // zero rotation means path straight up - s.poseMeters = new Pose2d(1, 1, GeometryUtil.kRotationZero); + s.poseMeters = new Pose2d(1, 1, Rotation2d.kZero); s.velocityMetersPerSecond = 1; ArmAngles r = command.getThetaPosReference(s); // proximal straight up diff --git a/lib/src/test/java/org/team100/lib/commands/drivetrain/DriveToPoseWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/commands/drivetrain/DriveToPoseWithTrajectoryTest.java index c98b7f7f..2ad516df 100644 --- a/lib/src/test/java/org/team100/lib/commands/drivetrain/DriveToPoseWithTrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/commands/drivetrain/DriveToPoseWithTrajectoryTest.java @@ -30,6 +30,7 @@ import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -45,7 +46,7 @@ class DriveToPoseWithTrajectoryTest extends Fixtured implements Timeless { @Test void testSimple() { - Pose2d goal = GeometryUtil.kPoseZero; + Pose2d goal = Pose2d.kZero; SwerveDriveSubsystem drive = fixture.drive; SwerveController controller = SwerveControllerFactory.test(logger); @@ -54,7 +55,7 @@ void testSimple() { drive, (start, end) -> new Trajectory100( List.of(new TimedPose(new Pose2dWithMotion( - GeometryUtil.kPoseZero, new Pose2dWithMotion.MotionDirection(0, 0, 0), 0, 0), 0, 0, 0))), + Pose2d.kZero, new Pose2dWithMotion.MotionDirection(0, 0, 0), 0, 0), 0, 0, 0))), controller, viz); command.initialize(); @@ -71,7 +72,7 @@ void testAprilTag() throws IOException { Transform2d transform = new Transform2d( new Translation2d(-1, -1), - GeometryUtil.kRotationZero); + Rotation2d.kZero); Optional optGoal = Target.goal(layout, Alliance.Blue, 1, transform); assertTrue(optGoal.isPresent()); diff --git a/lib/src/test/java/org/team100/lib/commands/drivetrain/DriveWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/commands/drivetrain/DriveWithTrajectoryTest.java index 0891f4b5..f7484cc8 100644 --- a/lib/src/test/java/org/team100/lib/commands/drivetrain/DriveWithTrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/commands/drivetrain/DriveWithTrajectoryTest.java @@ -11,7 +11,6 @@ import org.team100.lib.controller.drivetrain.SwerveControllerFactory; import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -30,6 +29,7 @@ import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; public class DriveWithTrajectoryTest extends Fixtured implements Timeless { private static final double kDelta = 0.001; @@ -42,8 +42,8 @@ public class DriveWithTrajectoryTest extends Fixtured implements Timeless { @Test void testTrajectoryStart() { Trajectory100 t = planner.restToRest( - new Pose2d(0, 0, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotationZero)); + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, t.sample(0).velocityM_S(), kDelta); SwerveController controller = SwerveControllerFactory.test(logger); @@ -98,8 +98,8 @@ void testTrajectoryStart() { @Test void testTrajectoryDone() { Trajectory100 t = planner.restToRest( - new Pose2d(0, 0, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotationZero)); + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, t.sample(0).velocityM_S(), kDelta); SwerveController controller = SwerveControllerFactory.test(logger); @@ -132,8 +132,8 @@ void testRealDrive() { Experiments.instance.testOverride(Experiment.UseSetpointGenerator, true); // 1m along +x, no rotation. Trajectory100 trajectory = planner.restToRest( - new Pose2d(0, 0, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotationZero)); + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, trajectory.sample(0).velocityM_S(), kDelta); SwerveController controller = SwerveControllerFactory.test(logger); diff --git a/lib/src/test/java/org/team100/lib/commands/drivetrain/HeadingLatchTest.java b/lib/src/test/java/org/team100/lib/commands/drivetrain/HeadingLatchTest.java index fe766a31..7c233f3c 100644 --- a/lib/src/test/java/org/team100/lib/commands/drivetrain/HeadingLatchTest.java +++ b/lib/src/test/java/org/team100/lib/commands/drivetrain/HeadingLatchTest.java @@ -31,7 +31,7 @@ void testLatch() { Experiments.instance.testOverride(Experiment.StickyHeading, false); HeadingLatch l = new HeadingLatch(); Model100 s = new Model100(); - Rotation2d pov = GeometryUtil.kRotation90; + Rotation2d pov = Rotation2d.kCCW_Pi_2; DriverControl.Velocity input = new DriverControl.Velocity(0, 0, 0); Rotation2d desiredRotation = l.latchedRotation(10, s, pov, input.theta()); assertEquals(Math.PI / 2, desiredRotation.getRadians(), kDelta); @@ -45,7 +45,7 @@ void testUnLatch() { Experiments.instance.testOverride(Experiment.StickyHeading, false); HeadingLatch l = new HeadingLatch(); Model100 s = new Model100(); - Rotation2d pov = GeometryUtil.kRotation90; + Rotation2d pov = Rotation2d.kCCW_Pi_2; DriverControl.Velocity input = new DriverControl.Velocity(0, 0, 0); Rotation2d desiredRotation = l.latchedRotation(10, s, pov, input.theta()); assertEquals(Math.PI / 2, desiredRotation.getRadians(), kDelta); @@ -62,7 +62,7 @@ void testExplicitUnLatch() { Experiments.instance.testOverride(Experiment.StickyHeading, false); HeadingLatch l = new HeadingLatch(); Model100 s = new Model100(); - Rotation2d pov = GeometryUtil.kRotation90; + Rotation2d pov = Rotation2d.kCCW_Pi_2; DriverControl.Velocity input = new DriverControl.Velocity(0, 0, 0); Rotation2d desiredRotation = l.latchedRotation(10, s, pov, input.theta()); assertEquals(Math.PI / 2, desiredRotation.getRadians(), kDelta); diff --git a/lib/src/test/java/org/team100/lib/commands/drivetrain/manual/ManualWithFullStateHeadingTest.java b/lib/src/test/java/org/team100/lib/commands/drivetrain/manual/ManualWithFullStateHeadingTest.java index f830122d..dea77f4d 100644 --- a/lib/src/test/java/org/team100/lib/commands/drivetrain/manual/ManualWithFullStateHeadingTest.java +++ b/lib/src/test/java/org/team100/lib/commands/drivetrain/manual/ManualWithFullStateHeadingTest.java @@ -32,7 +32,7 @@ class ManualWithFullStateHeadingTest { private static final double kDelta = 0.01; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); - private Rotation2d desiredRotation = GeometryUtil.kRotationZero; + private Rotation2d desiredRotation = Rotation2d.kZero; @Test void testModeSwitching() { @@ -90,7 +90,7 @@ void testNotSnapMode() { twist1_1 = new DriverControl.Velocity(1, 0, 0); - twistM_S = m_manualWithHeading.apply(new SwerveModel(GeometryUtil.kPoseZero, twistM_S), twist1_1); + twistM_S = m_manualWithHeading.apply(new SwerveModel(Pose2d.kZero, twistM_S), twist1_1); assertNull(m_manualWithHeading.m_goal); verify(1, 0, 0, twistM_S); } @@ -113,7 +113,7 @@ void testSnapMode() { assertEquals(0, m_manualWithHeading.m_thetaSetpoint.v(), kDelta); // face towards +y - desiredRotation = GeometryUtil.kRotation90; + desiredRotation = Rotation2d.kCCW_Pi_2; // no user input final DriverControl.Velocity zeroVelocity = new DriverControl.Velocity(0, 0, 0); @@ -180,11 +180,11 @@ void testSnapHeld() { new double[] { 1.0, 0.1 }); // currently facing +x - Pose2d currentPose = GeometryUtil.kPoseZero; + Pose2d currentPose = Pose2d.kZero; m_manualWithHeading.reset(new SwerveModel()); // want to face towards +y - desiredRotation = GeometryUtil.kRotation90; + desiredRotation = Rotation2d.kCCW_Pi_2; // no dtheta // no stick input @@ -248,7 +248,7 @@ void testStickyHeading() { DriverControl.Velocity twist1_1 = new DriverControl.Velocity(0, 0, 1); SwerveModel currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 0)); // no POV desiredRotation = null; @@ -261,7 +261,7 @@ void testStickyHeading() { // already going full speed: currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 2.828)); // gyro indicates the correct speed gyro.rate = 2.828; @@ -273,7 +273,7 @@ void testStickyHeading() { // let go of the stick twist1_1 = new DriverControl.Velocity(0, 0, 0); currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 2.828)); // gyro rate is still full speed. gyro.rate = 2.828; @@ -304,7 +304,7 @@ void testStickyHeading2() { DriverControl.Velocity twist1_1 = new DriverControl.Velocity(0, 0, 1); SwerveModel currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 0)); // no POV desiredRotation = null; @@ -317,7 +317,7 @@ void testStickyHeading2() { // already going full speed: currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 2.828)); // gyro indicates the correct speed gyro.rate = 2.828; @@ -329,7 +329,7 @@ void testStickyHeading2() { // let go of the stick twist1_1 = new DriverControl.Velocity(0, 0, 0); currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 2.828)); // gyro rate is still full speed. gyro.rate = 2.828; diff --git a/lib/src/test/java/org/team100/lib/commands/drivetrain/manual/ManualWithProfiledHeadingTest.java b/lib/src/test/java/org/team100/lib/commands/drivetrain/manual/ManualWithProfiledHeadingTest.java index 103c5da0..a64d7478 100644 --- a/lib/src/test/java/org/team100/lib/commands/drivetrain/manual/ManualWithProfiledHeadingTest.java +++ b/lib/src/test/java/org/team100/lib/commands/drivetrain/manual/ManualWithProfiledHeadingTest.java @@ -33,7 +33,7 @@ class ManualWithProfiledHeadingTest { private static final double kDelta = 0.01; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); - private Rotation2d desiredRotation = GeometryUtil.kRotationZero; + private Rotation2d desiredRotation = Rotation2d.kZero; @Test void testModeSwitching() { @@ -95,7 +95,7 @@ void testNotSnapMode() { twist1_1 = new DriverControl.Velocity(1, 0, 0); - twistM_S = m_manualWithHeading.apply(new SwerveModel(GeometryUtil.kPoseZero, twistM_S), twist1_1); + twistM_S = m_manualWithHeading.apply(new SwerveModel(Pose2d.kZero, twistM_S), twist1_1); assertNull(m_manualWithHeading.m_goal); verify(1, 0, 0, twistM_S); } @@ -119,7 +119,7 @@ void testSnapMode() { assertEquals(0, m_manualWithHeading.m_thetaSetpoint.v(), kDelta); // face towards +y - desiredRotation = GeometryUtil.kRotation90; + desiredRotation = Rotation2d.kCCW_Pi_2; // no user input final DriverControl.Velocity twist1_1 = new DriverControl.Velocity(0, 0, 0); @@ -197,7 +197,7 @@ void testSnapHeld() { m_manualWithHeading.reset(new SwerveModel()); // want to face towards +y - desiredRotation = GeometryUtil.kRotation90; + desiredRotation = Rotation2d.kCCW_Pi_2; // no dtheta // no stick input @@ -269,7 +269,7 @@ void testStickyHeading() { DriverControl.Velocity control = new DriverControl.Velocity(0, 0, 1); SwerveModel currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 0)); // no POV desiredRotation = null; @@ -282,7 +282,7 @@ void testStickyHeading() { // already going full speed: currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 2.828)); // gyro indicates the correct speed gyro.rate = 2.828; @@ -294,7 +294,7 @@ void testStickyHeading() { // let go of the stick control = new DriverControl.Velocity(0, 0, 0); currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 2.828)); // gyro rate is still full speed. gyro.rate = 2.828; @@ -340,7 +340,7 @@ void testStickyHeading2() { DriverControl.Velocity twist1_1 = new DriverControl.Velocity(0, 0, 1); SwerveModel currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 0)); // no POV desiredRotation = null; @@ -353,7 +353,7 @@ void testStickyHeading2() { // already going full speed: currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 2.828)); // gyro indicates the correct speed gyro.rate = 2.828; @@ -365,7 +365,7 @@ void testStickyHeading2() { // let go of the stick twist1_1 = new DriverControl.Velocity(0, 0, 0); currentState = new SwerveModel( - GeometryUtil.kPoseZero, + Pose2d.kZero, new FieldRelativeVelocity(0, 0, 2.828)); // gyro rate is still full speed. gyro.rate = 2.828; diff --git a/lib/src/test/java/org/team100/lib/controller/drivetrain/ReferenceControllerTest.java b/lib/src/test/java/org/team100/lib/controller/drivetrain/ReferenceControllerTest.java index fd9d5e09..b15dd7c1 100644 --- a/lib/src/test/java/org/team100/lib/controller/drivetrain/ReferenceControllerTest.java +++ b/lib/src/test/java/org/team100/lib/controller/drivetrain/ReferenceControllerTest.java @@ -7,7 +7,6 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -26,6 +25,7 @@ import org.team100.lib.trajectory.TrajectoryPlanner; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; public class ReferenceControllerTest extends Fixtured implements Timeless { private static final double kDelta = 0.001; @@ -37,8 +37,8 @@ public class ReferenceControllerTest extends Fixtured implements Timeless { @Test void testTrajectoryStart() { Trajectory100 t = planner.restToRest( - new Pose2d(0, 0, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotationZero)); + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, t.sample(0).velocityM_S(), kDelta); SwerveController controller = SwerveControllerFactory.test(logger); @@ -96,8 +96,8 @@ void testTrajectoryStart() { @Test void testTrajectoryDone() { Trajectory100 t = planner.restToRest( - new Pose2d(0, 0, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotationZero)); + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, t.sample(0).velocityM_S(), kDelta); SwerveController controller = SwerveControllerFactory.test(logger); @@ -130,8 +130,8 @@ void testRealDrive() { fixture.collection.reset(); // 1m along +x, no rotation. Trajectory100 trajectory = planner.restToRest( - new Pose2d(0, 0, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotationZero)); + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, trajectory.sample(0).velocityM_S(), kDelta); SwerveController controller = SwerveControllerFactory.test(logger); diff --git a/lib/src/test/java/org/team100/lib/controller/drivetrain/SwerveControllerTest.java b/lib/src/test/java/org/team100/lib/controller/drivetrain/SwerveControllerTest.java index f94fcfb0..cacd01dc 100644 --- a/lib/src/test/java/org/team100/lib/controller/drivetrain/SwerveControllerTest.java +++ b/lib/src/test/java/org/team100/lib/controller/drivetrain/SwerveControllerTest.java @@ -251,12 +251,12 @@ void testErrorAhead() { void testErrorSideways() { FullStateSwerveController controller = new FullStateSwerveController(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, 0.01, 0.02); // measurement is at the origin, facing down y - SwerveModel measurement = new SwerveModel(new Pose2d(0, 0, GeometryUtil.kRotation90)); + SwerveModel measurement = new SwerveModel(new Pose2d(0, 0, Rotation2d.kCCW_Pi_2)); // motion is in a straight line, down the x axis MotionDirection motionDirection = new MotionDirection(1, 0, 0); // setpoint is +x, facing down y Pose2dWithMotion state = new Pose2dWithMotion( - new Pose2d(1, 0, GeometryUtil.kRotation90), + new Pose2d(1, 0, Rotation2d.kCCW_Pi_2), motionDirection, 0, // no curvature 0); // no change in curvature @@ -362,7 +362,7 @@ void testFeedForwardSideways() { MotionDirection motionDirection = new MotionDirection(1, 0, 0); // setpoint is the same Pose2dWithMotion state = new Pose2dWithMotion( - new Pose2d(0, 0, GeometryUtil.kRotation90), + new Pose2d(0, 0, Rotation2d.kCCW_Pi_2), motionDirection, 0, // no curvature 0); // no change in curvature @@ -443,7 +443,7 @@ void testFeedbackAhead() { void testFeedbackAheadPlusY() { FullStateSwerveController controller = new FullStateSwerveController(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // measurement is plus-Y, facing ahead - Pose2d currentState = new Pose2d(0, 1, GeometryUtil.kRotationZero); + Pose2d currentState = new Pose2d(0, 1, Rotation2d.kZero); // setpoint is at the origin Pose2d setpointPose = new Pose2d(); // motion is in a straight line, down the x axis @@ -516,9 +516,9 @@ void testFeedbackSideways() { FullStateSwerveController controller = new FullStateSwerveController(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // measurement is at the origin, facing down the y axis - Pose2d currentState = new Pose2d(0, 0, GeometryUtil.kRotation90); + Pose2d currentState = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // setpoint is the same - Pose2d setpointPose = new Pose2d(0, 0, GeometryUtil.kRotation90); + Pose2d setpointPose = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // motion is in a straight line, down the x axis MotionDirection motionDirection = new MotionDirection(1, 0, 0); // no curvature @@ -550,9 +550,9 @@ void testFeedbackSideways() { void testFeedbackSidewaysPlusY() { FullStateSwerveController controller = new FullStateSwerveController(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // measurement is plus-y, facing down the y axis - Pose2d currentState = new Pose2d(0, 1, GeometryUtil.kRotation90); + Pose2d currentState = new Pose2d(0, 1, Rotation2d.kCCW_Pi_2); // setpoint is parallel at the origin - Pose2d setpointPose = new Pose2d(0, 0, GeometryUtil.kRotation90); + Pose2d setpointPose = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // motion is in a straight line, down the x axis MotionDirection motionDirection = new MotionDirection(1, 0, 0); // no curvature @@ -620,9 +620,9 @@ void testFullFeedbackSideways() { FullStateSwerveController controller = new FullStateSwerveController(logger, 1, 1, 1, 1, 0.01, 0.02, 0.01, 0.02); // measurement is at the origin, facing +y - Pose2d currentPose = new Pose2d(0, 0, GeometryUtil.kRotation90); + Pose2d currentPose = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // setpoint postion is the same - Pose2d setpointPose = new Pose2d(0, 0, GeometryUtil.kRotation90); + Pose2d setpointPose = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // motion is in a straight line, down the x axis MotionDirection fieldRelativeMotionDirection = new MotionDirection(1, 0, 0); // no curvature @@ -659,11 +659,11 @@ void testFullFeedbackSidewaysWithRotation() { // measurement is too slow SwerveModel measurement = new SwerveModel( new Pose2d(0.1, 0.1, - GeometryUtil.kRotation90.plus(new Rotation2d(0.1))), + Rotation2d.kCCW_Pi_2.plus(new Rotation2d(0.1))), new FieldRelativeVelocity(0.5, 0, 0)); // setpoint postion is ahead in x and y and theta - Pose2d setpointPose = new Pose2d(0, 0, GeometryUtil.kRotation90); + Pose2d setpointPose = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2); // motion is in a straight line, down the x axis MotionDirection fieldRelativeMotionDirection = new MotionDirection(1, 0, 0); // no curvature diff --git a/lib/src/test/java/org/team100/lib/controller/simple/ProfiledControllerTest.java b/lib/src/test/java/org/team100/lib/controller/simple/ProfiledControllerTest.java index 7f29c8dd..69787364 100644 --- a/lib/src/test/java/org/team100/lib/controller/simple/ProfiledControllerTest.java +++ b/lib/src/test/java/org/team100/lib/controller/simple/ProfiledControllerTest.java @@ -54,7 +54,7 @@ void test2() { final double k2 = 1.0; Feedback100 f = new FullStateFeedback(logger, k1, k2, x -> x, 1, 1); - ProfiledController controller = new ProfiledController(p, f, x -> x); + ProfiledController controller = new ProfiledController(p, f, x -> x, 0.05, 0.05); Model100 measurement = new Model100(0, 0); controller.init(measurement); Model100 goal = new Model100(0.1, 0); @@ -79,7 +79,7 @@ void test1() { final double k2 = 1.0; Feedback100 f = new FullStateFeedback(logger, k1, k2, x -> x, 1, 1); - ProfiledController c = new ProfiledController(p, f, x -> x); + ProfiledController c = new ProfiledController(p, f, x -> x, 0.05, 0.05); final Model100 initial = new Model100(0, 0); final Model100 goal = new Model100(1, 0); diff --git a/lib/src/test/java/org/team100/lib/follower/DriveMotionPlannerTest.java b/lib/src/test/java/org/team100/lib/follower/DriveMotionPlannerTest.java index 78d98fcf..303cfc74 100644 --- a/lib/src/test/java/org/team100/lib/follower/DriveMotionPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/follower/DriveMotionPlannerTest.java @@ -97,7 +97,6 @@ void testFieldRelativeTrajectory() { } // this should be exactly right but it's not. - // TODO: it's because of the clock skew in the feedback, so fix that. assertEquals(195, pose.getTranslation().getX(), 1); assertEquals(13, pose.getTranslation().getY(), 0.4); assertEquals(0, pose.getRotation().getRadians(), 0.1); diff --git a/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java b/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java index ed2c2af4..41fe65e7 100644 --- a/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java +++ b/lib/src/test/java/org/team100/lib/geometry/TestSE2Math.java @@ -17,7 +17,7 @@ class TestSE2Math { @Test void testRotation2d() { // Test constructors - Rotation2d rot1 = GeometryUtil.kRotationZero; + Rotation2d rot1 = Rotation2d.kZero; assertEquals(1, rot1.getCos(), kTestEpsilon); assertEquals(0, rot1.getSin(), kTestEpsilon); assertEquals(0, rot1.getTan(), kTestEpsilon); @@ -82,7 +82,7 @@ void testRotation2d() { assertEquals(0, rot3.getRadians(), kTestEpsilon); // A rotation times its inverse should be the identity - Rotation2d identity = GeometryUtil.kRotationZero; + Rotation2d identity = Rotation2d.kZero; rot1 = Rotation2d.fromDegrees(21.45); rot2 = rot1.rotateBy(rot1.unaryMinus()); assertEquals(identity.getCos(), rot2.getCos(), kTestEpsilon); @@ -197,7 +197,7 @@ void testTranslation2d() { @Test void testPose2d() { // Test constructors - Pose2d pose1 = GeometryUtil.kPoseZero; + Pose2d pose1 = Pose2d.kZero; assertEquals(0, pose1.getTranslation().getX(), kTestEpsilon); assertEquals(0, pose1.getTranslation().getY(), kTestEpsilon); assertEquals(0, pose1.getRotation().getDegrees(), kTestEpsilon); @@ -223,7 +223,7 @@ void testPose2d() { assertEquals(0, pose3.getRotation().getDegrees(), kTestEpsilon); // A pose times its inverse should be the identity - Pose2d identity = GeometryUtil.kPoseZero; + Pose2d identity = Pose2d.kZero; pose1 = new Pose2d(new Translation2d(3.51512152, 4.23), Rotation2d.fromDegrees(91.6)); pose2 = GeometryUtil.transformBy(pose1, GeometryUtil.inverse(pose1)); assertEquals(identity.getTranslation().getX(), pose2.getTranslation().getX(), kTestEpsilon); @@ -254,27 +254,27 @@ void testPose2d() { void testTwist() { // Exponentiation (integrate twist to obtain a Pose2d) Twist2d twist = new Twist2d(1.0, 0.0, 0.0); - Pose2d pose = GeometryUtil.kPoseZero.exp(twist); + Pose2d pose = Pose2d.kZero.exp(twist); assertEquals(1.0, pose.getTranslation().getX(), kTestEpsilon); assertEquals(0.0, pose.getTranslation().getY(), kTestEpsilon); assertEquals(0.0, pose.getRotation().getDegrees(), kTestEpsilon); // Scaled. twist = new Twist2d(1.0, 0.0, 0.0); - pose = GeometryUtil.kPoseZero.exp(GeometryUtil.scale(twist, 2.5)); + pose = Pose2d.kZero.exp(GeometryUtil.scale(twist, 2.5)); assertEquals(2.5, pose.getTranslation().getX(), kTestEpsilon); assertEquals(0.0, pose.getTranslation().getY(), kTestEpsilon); assertEquals(0.0, pose.getRotation().getDegrees(), kTestEpsilon); // Logarithm (find the twist to apply to obtain a given Pose2d) pose = new Pose2d(new Translation2d(2.0, 2.0), Rotation2d.fromRadians(Math.PI / 2)); - twist = GeometryUtil.kPoseZero.log(pose); + twist = Pose2d.kZero.log(pose); assertEquals(Math.PI, twist.dx, kTestEpsilon); assertEquals(0.0, twist.dy, kTestEpsilon); assertEquals(Math.PI / 2, twist.dtheta, kTestEpsilon); // Logarithm is the inverse of exponentiation. - Pose2d new_pose = GeometryUtil.kPoseZero.exp(twist); + Pose2d new_pose = Pose2d.kZero.exp(twist); assertEquals(new_pose.getTranslation().getX(), pose.getTranslation().getX(), kTestEpsilon); assertEquals(new_pose.getTranslation().getY(), pose.getTranslation().getY(), kTestEpsilon); assertEquals(new_pose.getRotation().getDegrees(), pose.getRotation().getDegrees(), kTestEpsilon); diff --git a/lib/src/test/java/org/team100/lib/localization/PoseEstimationHelperTest.java b/lib/src/test/java/org/team100/lib/localization/PoseEstimationHelperTest.java index e4cfa2ed..5188c849 100644 --- a/lib/src/test/java/org/team100/lib/localization/PoseEstimationHelperTest.java +++ b/lib/src/test/java/org/team100/lib/localization/PoseEstimationHelperTest.java @@ -207,6 +207,7 @@ void posePerformance() { final int iterations = 10000000; long startTime = System.currentTimeMillis(); for (int i = 0; i < iterations; ++i) { + @SuppressWarnings("unused") Pose3d robotPoseInFieldCoords = helper.getRobotPoseInFieldCoords( cameraInRobotCoords, tagInFieldCoords, diff --git a/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100PerformanceTest.java b/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100PerformanceTest.java index 05294975..17dba0d0 100644 --- a/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100PerformanceTest.java +++ b/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100PerformanceTest.java @@ -16,16 +16,17 @@ import org.team100.lib.util.Util; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; public class SwerveDrivePoseEstimator100PerformanceTest { private static final boolean DEBUG = false; private static final double kDelta = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); - private final Pose2d visionRobotPoseMeters = new Pose2d(1, 0, GeometryUtil.kRotationZero); + private final Pose2d visionRobotPoseMeters = new Pose2d(1, 0, Rotation2d.kZero); static SwerveModulePositions p(double x) { - SwerveModulePosition100 m = new SwerveModulePosition100(x, Optional.of(GeometryUtil.kRotationZero)); + SwerveModulePosition100 m = new SwerveModulePosition100(x, Optional.of(Rotation2d.kZero)); return new SwerveModulePositions(m, m, m, m); } @@ -61,14 +62,14 @@ void test0() { logger, new MockGyro(), p(0), - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // fill the buffer with odometry double t = 0.0; double duration = SwerveDrivePoseEstimator100.kBufferDuration; while (t < duration) { - poseEstimator.put(t, GeometryUtil.kRotationZero, 0, p(t)); + poseEstimator.put(t, Rotation2d.kZero, 0, p(t)); t += 0.02; } assertEquals(11, poseEstimator.size()); diff --git a/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java b/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java index 0d23b104..44606aca 100644 --- a/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java +++ b/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java @@ -11,7 +11,6 @@ import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; @@ -43,12 +42,12 @@ class SwerveDrivePoseEstimator100Test { private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); private static final boolean DEBUG = false; - private final SwerveModulePosition100 p0 = new SwerveModulePosition100(0, Optional.of(GeometryUtil.kRotationZero)); + private final SwerveModulePosition100 p0 = new SwerveModulePosition100(0, Optional.of(Rotation2d.kZero)); private final SwerveModulePositions positionZero = new SwerveModulePositions(p0, p0, p0, p0); private final SwerveModulePosition100 p01 = new SwerveModulePosition100(0.1, - Optional.of(GeometryUtil.kRotationZero)); + Optional.of(Rotation2d.kZero)); private final SwerveModulePositions position01 = new SwerveModulePositions(p01, p01, p01, p01); - private final Pose2d visionRobotPoseMeters = new Pose2d(1, 0, GeometryUtil.kRotationZero); + private final Pose2d visionRobotPoseMeters = new Pose2d(1, 0, Rotation2d.kZero); private static void verify(double x, SwerveModel state) { Pose2d estimate = state.pose(); @@ -76,13 +75,13 @@ void odo1() { logger, new MockGyro(), positionZero, - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // zero initial time - poseEstimator.put(0.0, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0.0, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verifyVelocity(0.000, poseEstimator.get(0.00)); // 0.1 m - poseEstimator.put(0.02, GeometryUtil.kRotationZero, 0, position01); + poseEstimator.put(0.02, Rotation2d.kZero, 0, position01); verify(0.000, poseEstimator.get(0.00)); verifyVelocity(0.000, poseEstimator.get(0.00)); // velocity is the delta @@ -109,13 +108,13 @@ void odo2() { logger, new MockGyro(), positionZero, - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // zero initial time - poseEstimator.put(0.0, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0.0, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verifyVelocity(0.000, poseEstimator.get(0.00)); // 0.1 m - poseEstimator.put(0.02, GeometryUtil.kRotationZero, 0, position01); + poseEstimator.put(0.02, Rotation2d.kZero, 0, position01); verify(0.000, poseEstimator.get(0.00)); verifyVelocity(0.000, poseEstimator.get(0.00)); // velocity is the delta @@ -144,13 +143,13 @@ void odo3() { logger, new MockGyro(), positionZero, - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // zero initial time - poseEstimator.put(0.0, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0.0, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verifyVelocity(0.000, poseEstimator.get(0.00)); // 0.1 m - poseEstimator.put(0.02, GeometryUtil.kRotationZero, 0, position01); + poseEstimator.put(0.02, Rotation2d.kZero, 0, position01); verify(0.000, poseEstimator.get(0.00)); verifyVelocity(0.000, poseEstimator.get(0.00)); // velocity is the delta @@ -184,7 +183,7 @@ void outOfOrder() { logger, new MockGyro(), positionZero, - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // zero initial time // initial pose = 0 @@ -193,7 +192,7 @@ void outOfOrder() { // pose stays zero when updated at time zero // if we try to update zero, there's nothing to compare it to, // so we should just ignore this update. - poseEstimator.put(0.0, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0.0, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verify(0.000, poseEstimator.get(0.01)); verify(0.000, poseEstimator.get(0.02)); @@ -211,7 +210,7 @@ void outOfOrder() { // wheels haven't moved, so the "odometry opinion" should be zero // but it's not, it's applied relative to the vision update, so there's no // change. - poseEstimator.put(0.02, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0.02, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verify(0.167, poseEstimator.get(0.01)); @@ -222,7 +221,7 @@ void outOfOrder() { // 0, but instead odometry is applied relative to the latest estimate, which // was based on vision. so the actual odometry stddev is like *zero*. - poseEstimator.put(0.04, GeometryUtil.kRotationZero, 0, position01); + poseEstimator.put(0.04, Rotation2d.kZero, 0, position01); verify(0.000, poseEstimator.get(0.00)); verify(0.167, poseEstimator.get(0.02)); @@ -239,7 +238,7 @@ void outOfOrder() { verify(0.405, poseEstimator.get(0.04)); // wheels are in the same position as the previous iteration, - poseEstimator.put(0.06, GeometryUtil.kRotationZero, 0, position01); + poseEstimator.put(0.06, Rotation2d.kZero, 0, position01); verify(0.000, poseEstimator.get(0.00)); verify(0.305, poseEstimator.get(0.02)); verify(0.405, poseEstimator.get(0.04)); @@ -253,7 +252,7 @@ void outOfOrder() { verify(0.305, poseEstimator.get(0.014)); // but doesn't change this estimate since it's the same, and we're not moving, // we don't replay vision input - // TODO: this is wrong: two vision estimates should pull harder than one, + // it would be better if two vision estimates pulled harder than one, // even if they come in out-of-order. verify(0.305, poseEstimator.get(0.015)); verify(0.305, poseEstimator.get(0.02)); @@ -272,7 +271,7 @@ void outOfOrder() { verify(0.521, poseEstimator.get(0.06)); // wheels not moving -> no change, - poseEstimator.put(0.08, GeometryUtil.kRotationZero, 0, position01); + poseEstimator.put(0.08, Rotation2d.kZero, 0, position01); verify(0.000, poseEstimator.get(0.00)); verify(0.421, poseEstimator.get(0.02)); verify(0.521, poseEstimator.get(0.04)); @@ -291,7 +290,7 @@ void minorWeirdness() { logger, new MockGyro(), positionZero, - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // zero initial time // initial pose = 0 @@ -304,7 +303,7 @@ void minorWeirdness() { // pose stays zero when updated at time zero // if we try to update zero, there's nothing to compare it to, // so we should just ignore this update. - poseEstimator.put(0.0, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0.0, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verify(0.000, poseEstimator.get(0.02)); verify(0.000, poseEstimator.get(0.04)); @@ -327,7 +326,7 @@ void minorWeirdness() { // wheels haven't moved, so the "odometry opinion" should be zero // but it's not, it's applied relative to the vision update, so there's no // change. - poseEstimator.put(0.02, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0.02, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verify(0.167, poseEstimator.get(0.02)); verify(0.167, poseEstimator.get(0.04)); @@ -339,7 +338,7 @@ void minorWeirdness() { // 0, but instead odometry is applied relative to the latest estimate, which // was based on vision. so the actual odometry stddev is like *zero*. - poseEstimator.put(0.04, GeometryUtil.kRotationZero, 0, position01); + poseEstimator.put(0.04, Rotation2d.kZero, 0, position01); verify(0.000, poseEstimator.get(0.00)); verify(0.167, poseEstimator.get(0.02)); verify(0.267, poseEstimator.get(0.04)); @@ -356,7 +355,7 @@ void minorWeirdness() { verify(0.405, poseEstimator.get(0.08)); // wheels are in the same position as the previous iteration - poseEstimator.put(0.06, GeometryUtil.kRotationZero, 0, position01); + poseEstimator.put(0.06, Rotation2d.kZero, 0, position01); verify(0.000, poseEstimator.get(0.00)); verify(0.305, poseEstimator.get(0.02)); verify(0.405, poseEstimator.get(0.04)); @@ -364,7 +363,6 @@ void minorWeirdness() { verify(0.405, poseEstimator.get(0.08)); // a little earlier than the previous estimate does nothing. - // TODO: this is wrong poseEstimator.put(0.014, visionRobotPoseMeters, stateStdDevs, visionMeasurementStdDevs); verify(0.000, poseEstimator.get(0.00)); verify(0.305, poseEstimator.get(0.02)); @@ -381,7 +379,7 @@ void minorWeirdness() { verify(0.521, poseEstimator.get(0.08)); // wheels not moving -> no change - poseEstimator.put(0.08, GeometryUtil.kRotationZero, 0, position01); + poseEstimator.put(0.08, Rotation2d.kZero, 0, position01); verify(0.000, poseEstimator.get(0.00)); verify(0.421, poseEstimator.get(0.02)); verify(0.521, poseEstimator.get(0.04)); @@ -400,7 +398,7 @@ void test0105() { logger, new MockGyro(), positionZero, - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // zero initial time verify(0.000, poseEstimator.get(0.00)); @@ -409,7 +407,7 @@ void test0105() { verify(0.000, poseEstimator.get(0.06)); verify(0.000, poseEstimator.get(0.08)); - poseEstimator.put(0, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verify(0.000, poseEstimator.get(0.02)); verify(0.000, poseEstimator.get(0.04)); @@ -449,7 +447,7 @@ void test0110() { logger, new MockGyro(), positionZero, - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // zero initial time verify(0.000, poseEstimator.get(0.00)); @@ -458,7 +456,7 @@ void test0110() { verify(0.000, poseEstimator.get(0.06)); verify(0.000, poseEstimator.get(0.08)); - poseEstimator.put(0, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verify(0.000, poseEstimator.get(0.02)); verify(0.000, poseEstimator.get(0.04)); @@ -498,7 +496,7 @@ void test00505() { logger, new MockGyro(), positionZero, - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // zero initial time verify(0.000, poseEstimator.get(0.00)); @@ -507,7 +505,7 @@ void test00505() { verify(0.000, poseEstimator.get(0.06)); verify(0.000, poseEstimator.get(0.08)); - poseEstimator.put(0, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verify(0.000, poseEstimator.get(0.02)); verify(0.000, poseEstimator.get(0.04)); @@ -549,7 +547,7 @@ void reasonable() { logger, new MockGyro(), positionZero, - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // zero initial time verify(0.000, poseEstimator.get(0.00)); verify(0.000, poseEstimator.get(0.02)); @@ -557,7 +555,7 @@ void reasonable() { verify(0.000, poseEstimator.get(0.06)); verify(0.000, poseEstimator.get(0.08)); - poseEstimator.put(0, GeometryUtil.kRotationZero, 0, positionZero); + poseEstimator.put(0, Rotation2d.kZero, 0, positionZero); verify(0.000, poseEstimator.get(0.00)); verify(0.000, poseEstimator.get(0.02)); verify(0.000, poseEstimator.get(0.04)); diff --git a/lib/src/test/java/org/team100/lib/localization/VisionDataProviderTest.java b/lib/src/test/java/org/team100/lib/localization/VisionDataProviderTest.java index 084e32c3..89a3e8db 100644 --- a/lib/src/test/java/org/team100/lib/localization/VisionDataProviderTest.java +++ b/lib/src/test/java/org/team100/lib/localization/VisionDataProviderTest.java @@ -43,7 +43,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { @Override public SwerveModel get(double timestampSeconds) { - return new SwerveModel(GeometryUtil.kRotationZero); + return new SwerveModel(Rotation2d.kZero); } }; diff --git a/lib/src/test/java/org/team100/lib/motion/components/AnglePositionServoProfileTest.java b/lib/src/test/java/org/team100/lib/motion/components/AnglePositionServoProfileTest.java index 87666dc2..70daf16b 100644 --- a/lib/src/test/java/org/team100/lib/motion/components/AnglePositionServoProfileTest.java +++ b/lib/src/test/java/org/team100/lib/motion/components/AnglePositionServoProfileTest.java @@ -46,7 +46,9 @@ public AnglePositionServoProfileTest() { ProfiledController controller = new ProfiledController( profile, feedback2, - MathUtil::angleModulus); + MathUtil::angleModulus, + 0.05, + 0.05); servo = new OnboardAngularPositionServo( logger, mech, diff --git a/lib/src/test/java/org/team100/lib/motion/components/AngularPositionProfileTest.java b/lib/src/test/java/org/team100/lib/motion/components/AngularPositionProfileTest.java index 62905302..f3cdc137 100644 --- a/lib/src/test/java/org/team100/lib/motion/components/AngularPositionProfileTest.java +++ b/lib/src/test/java/org/team100/lib/motion/components/AngularPositionProfileTest.java @@ -58,7 +58,9 @@ void testTrapezoid() { ProfiledController controller = new ProfiledController( profile, feedback2, - MathUtil::angleModulus); + MathUtil::angleModulus, + 0.05, + 0.05); servo = new OnboardAngularPositionServo( logger, mech, @@ -75,7 +77,9 @@ void testProfile() { ProfiledController controller = new ProfiledController( profile, feedback2, - MathUtil::angleModulus); + MathUtil::angleModulus, + 0.05, + 0.05); servo = new OnboardAngularPositionServo( logger, mech, diff --git a/lib/src/test/java/org/team100/lib/motion/drivetrain/Fixture.java b/lib/src/test/java/org/team100/lib/motion/drivetrain/Fixture.java index ea8df91f..a5dcc025 100644 --- a/lib/src/test/java/org/team100/lib/motion/drivetrain/Fixture.java +++ b/lib/src/test/java/org/team100/lib/motion/drivetrain/Fixture.java @@ -2,7 +2,6 @@ import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.controller.drivetrain.SwerveControllerFactory; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.localization.SwerveDrivePoseEstimator100; import org.team100.lib.localization.VisionData; import org.team100.lib.logging.LoggerFactory; @@ -16,6 +15,8 @@ import org.team100.lib.sensors.SimulatedGyro; import org.team100.lib.testing.Timeless; +import edu.wpi.first.math.geometry.Pose2d; + /** * A real swerve subsystem populated with simulated motors and encoders, * for testing. @@ -46,7 +47,7 @@ public Fixture() { logger, gyro, collection.positions(), - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // initial time is zero here for testing VisionData v = new VisionData() { @Override diff --git a/lib/src/test/java/org/team100/lib/motion/drivetrain/RealisticFixture.java b/lib/src/test/java/org/team100/lib/motion/drivetrain/RealisticFixture.java index 45c9e8bd..4b101423 100644 --- a/lib/src/test/java/org/team100/lib/motion/drivetrain/RealisticFixture.java +++ b/lib/src/test/java/org/team100/lib/motion/drivetrain/RealisticFixture.java @@ -2,7 +2,6 @@ import org.team100.lib.controller.drivetrain.SwerveController; import org.team100.lib.controller.drivetrain.SwerveControllerFactory; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.localization.SwerveDrivePoseEstimator100; import org.team100.lib.localization.VisionData; import org.team100.lib.logging.LoggerFactory; @@ -16,6 +15,8 @@ import org.team100.lib.sensors.SimulatedGyro; import org.team100.lib.testing.Timeless; +import edu.wpi.first.math.geometry.Pose2d; + /** * A real swerve subsystem populated with simulated motors and encoders, * for testing. @@ -45,7 +46,7 @@ public RealisticFixture() { logger, gyro, collection.positions(), - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); // initial time is zero here for testing VisionData v = new VisionData() { @Override diff --git a/lib/src/test/java/org/team100/lib/motion/drivetrain/SwerveControlTest.java b/lib/src/test/java/org/team100/lib/motion/drivetrain/SwerveControlTest.java index a0f4eb4c..ae530ece 100644 --- a/lib/src/test/java/org/team100/lib/motion/drivetrain/SwerveControlTest.java +++ b/lib/src/test/java/org/team100/lib/motion/drivetrain/SwerveControlTest.java @@ -95,7 +95,7 @@ void testTimedPose4() { @Test void testChassisSpeeds0() { SwerveControl state = new SwerveControl( - new Pose2d(new Translation2d(0, 0), GeometryUtil.kRotation180), + new Pose2d(new Translation2d(0, 0), Rotation2d.kPi), new FieldRelativeVelocity(1, 0, 0)); ChassisSpeeds speeds = state.chassisSpeeds(); assertEquals(-1, speeds.vxMetersPerSecond, kDelta); @@ -106,7 +106,7 @@ void testChassisSpeeds0() { @Test void testChassisSpeeds1() { SwerveControl state = new SwerveControl( - new Pose2d(new Translation2d(0, 0), GeometryUtil.kRotation90), + new Pose2d(new Translation2d(0, 0), Rotation2d.kCCW_Pi_2), new FieldRelativeVelocity(1, 0, 1)); ChassisSpeeds speeds = state.chassisSpeeds(); assertEquals(0, speeds.vxMetersPerSecond, kDelta); diff --git a/lib/src/test/java/org/team100/lib/motion/drivetrain/SwerveModelTest.java b/lib/src/test/java/org/team100/lib/motion/drivetrain/SwerveModelTest.java index ec5c23ca..53bbb45c 100644 --- a/lib/src/test/java/org/team100/lib/motion/drivetrain/SwerveModelTest.java +++ b/lib/src/test/java/org/team100/lib/motion/drivetrain/SwerveModelTest.java @@ -94,7 +94,7 @@ void testTimedPose4() { @Test void testChassisSpeeds0() { SwerveModel state = new SwerveModel( - new Pose2d(new Translation2d(0, 0), GeometryUtil.kRotation180), + new Pose2d(new Translation2d(0, 0), Rotation2d.kPi), new FieldRelativeVelocity(1, 0, 0)); ChassisSpeeds speeds = state.chassisSpeeds(); assertEquals(-1, speeds.vxMetersPerSecond, kDelta); @@ -105,7 +105,7 @@ void testChassisSpeeds0() { @Test void testChassisSpeeds1() { SwerveModel state = new SwerveModel( - new Pose2d(new Translation2d(0, 0), GeometryUtil.kRotation90), + new Pose2d(new Translation2d(0, 0), Rotation2d.kCCW_Pi_2), new FieldRelativeVelocity(1, 0, 1)); ChassisSpeeds speeds = state.chassisSpeeds(); assertEquals(0, speeds.vxMetersPerSecond, kDelta); diff --git a/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100Test.java b/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100Test.java index d8c2742e..e0f1f4e9 100644 --- a/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100Test.java +++ b/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100Test.java @@ -67,7 +67,7 @@ void testCrab() { assertEquals(0, twist.dtheta, kDelta); // it transforms the starting pose correctly - Pose2d pStart = new Pose2d(0.5, 0.5, GeometryUtil.kRotationZero); + Pose2d pStart = new Pose2d(0.5, 0.5, Rotation2d.kZero); Pose2d pEnd = pStart.exp(twist); assertEquals(-0.5, pEnd.getX(), kDelta); assertEquals(0.5, pEnd.getY(), kDelta); @@ -83,8 +83,8 @@ void testCrabInverse() { new Translation2d(-0.5, 0.5), new Translation2d(-0.5, -0.5)); - Pose2d pStart = new Pose2d(0.5, 0.5, GeometryUtil.kRotationZero); - Pose2d pEnd = new Pose2d(-0.5, 1.5, GeometryUtil.kRotationZero); + Pose2d pStart = new Pose2d(0.5, 0.5, Rotation2d.kZero); + Pose2d pEnd = new Pose2d(-0.5, 1.5, Rotation2d.kZero); Twist2d t = pStart.log(pEnd); assertEquals(-1, t.dx, kDelta); assertEquals(1, t.dy, kDelta); @@ -210,7 +210,7 @@ void testRollInverse() { Math.PI / 2); // check that the exp is correct - Pose2d pStart = new Pose2d(0.5, 0.5, GeometryUtil.kRotationZero); + Pose2d pStart = new Pose2d(0.5, 0.5, Rotation2d.kZero); Pose2d pEnd = pStart.exp(t); assertEquals(-0.5, pEnd.getX(), kDelta); assertEquals(0.5, pEnd.getY(), kDelta); diff --git a/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModulePosition100Test.java b/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModulePosition100Test.java index 10597684..21109224 100644 --- a/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModulePosition100Test.java +++ b/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModulePosition100Test.java @@ -7,15 +7,16 @@ import java.util.Optional; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.motion.drivetrain.kinodynamics.struct.SwerveModulePosition100Struct; +import edu.wpi.first.math.geometry.Rotation2d; + class SwerveModulePosition100Test { @Test void testStruct() { SwerveModulePosition100Struct s = SwerveModulePosition100.struct; ByteBuffer bb = ByteBuffer.allocate(s.getSize()); - SwerveModulePosition100 p = new SwerveModulePosition100(1, Optional.of(GeometryUtil.kRotationZero)); + SwerveModulePosition100 p = new SwerveModulePosition100(1, Optional.of(Rotation2d.kZero)); s.pack(bb, p); assertEquals(0, bb.remaining()); // distance = 8 diff --git a/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/SimulatedDrivingTest.java b/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/SimulatedDrivingTest.java index 0071b70e..a00d153a 100644 --- a/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/SimulatedDrivingTest.java +++ b/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/limiter/SimulatedDrivingTest.java @@ -50,7 +50,7 @@ public class SimulatedDrivingTest implements Timeless { logger, gyro, collection.positions(), - GeometryUtil.kPoseZero, + Pose2d.kZero, 0); SwerveLocal swerveLocal = new SwerveLocal(logger, swerveKinodynamics, collection); VisionData visionData = new VisionData() { diff --git a/lib/src/test/java/org/team100/lib/motion/servo/OnboardAngularPositionServoTest.java b/lib/src/test/java/org/team100/lib/motion/servo/OnboardAngularPositionServoTest.java index e4b7ed13..9692e8c3 100644 --- a/lib/src/test/java/org/team100/lib/motion/servo/OnboardAngularPositionServoTest.java +++ b/lib/src/test/java/org/team100/lib/motion/servo/OnboardAngularPositionServoTest.java @@ -41,7 +41,9 @@ void testOnboard() { ProfiledController controller = new ProfiledController( profile, turningFeedback2, - MathUtil::angleModulus); + MathUtil::angleModulus, + 0.05, + 0.05); final AngularPositionServo servo = new OnboardAngularPositionServo( logger, mech, diff --git a/lib/src/test/java/org/team100/lib/motion/servo/OnboardLinearDutyCyclePositionServoTest.java b/lib/src/test/java/org/team100/lib/motion/servo/OnboardLinearDutyCyclePositionServoTest.java index 10cb0dd9..818fe03a 100644 --- a/lib/src/test/java/org/team100/lib/motion/servo/OnboardLinearDutyCyclePositionServoTest.java +++ b/lib/src/test/java/org/team100/lib/motion/servo/OnboardLinearDutyCyclePositionServoTest.java @@ -31,7 +31,7 @@ void test1() { final double k1 = 1.0; final double k2 = 0.01; Feedback100 f = new FullStateFeedback(logger, k1, k2, x -> x, 1, 1); - ProfiledController c = new ProfiledController(p, f, x -> x); + ProfiledController c = new ProfiledController(p, f, x -> x, 0.05, 0.05); OnboardLinearDutyCyclePositionServo s = new OnboardLinearDutyCyclePositionServo(logger, mech, c, 0.1); s.reset(); diff --git a/lib/src/test/java/org/team100/lib/motion/servo/UnprofiledOutboardAngularPositionServoTest.java b/lib/src/test/java/org/team100/lib/motion/servo/UnprofiledOutboardAngularPositionServoTest.java new file mode 100644 index 00000000..02d7d0fd --- /dev/null +++ b/lib/src/test/java/org/team100/lib/motion/servo/UnprofiledOutboardAngularPositionServoTest.java @@ -0,0 +1,64 @@ +package org.team100.lib.motion.servo; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; +import org.team100.lib.encoder.CombinedEncoder; +import org.team100.lib.encoder.SimulatedBareEncoder; +import org.team100.lib.encoder.SimulatedRotaryPositionSensor; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.TestLoggerFactory; +import org.team100.lib.logging.primitive.TestPrimitiveLogger; +import org.team100.lib.motion.mechanism.SimpleRotaryMechanism; +import org.team100.lib.motor.SimulatedBareMotor; +import org.team100.lib.testing.Timeless; + +public class UnprofiledOutboardAngularPositionServoTest implements Timeless { + private static final double kDelta = 0.001; + private final LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); + + @Test + void testSimple() { + SimulatedBareMotor motor = new SimulatedBareMotor(log, 100); + SimulatedBareEncoder encoder = new SimulatedBareEncoder(log, motor); + SimpleRotaryMechanism mech = new SimpleRotaryMechanism(log, motor, encoder, 1); + SimulatedRotaryPositionSensor sensor = new SimulatedRotaryPositionSensor(log, mech); + CombinedEncoder combinedEncoder = new CombinedEncoder(log, sensor, mech, false); + UnprofiledOutboardAngularPositionServo servo = new UnprofiledOutboardAngularPositionServo( + log, mech, combinedEncoder); + + servo.reset(); + servo.periodic(); + stepTime(); + + assertEquals(0, motor.getVelocityRad_S(), kDelta); + assertEquals(0, encoder.getPositionRad().getAsDouble(), kDelta); + assertEquals(0, encoder.getVelocityRad_S().getAsDouble(), kDelta); + assertEquals(0, sensor.getPositionRad().getAsDouble(), kDelta); + assertEquals(0, mech.getVelocityRad_S().getAsDouble(), kDelta); + + servo.periodic(); + servo.setPosition(1, 0); + stepTime(); + + // move 0 to 1 in 0.02 => v = 50 + assertEquals(50, motor.getVelocityRad_S(), kDelta); + assertEquals(1, encoder.getPositionRad().getAsDouble(), kDelta); + assertEquals(50, encoder.getVelocityRad_S().getAsDouble(), kDelta); + assertEquals(50, mech.getVelocityRad_S().getAsDouble(), kDelta); + // the sensor does trapezoid integration so it's halfway there after one cycle + assertEquals(0.5, sensor.getPositionRad().getAsDouble(), kDelta); + + servo.periodic(); + servo.setPosition(1, 0); + stepTime(); + + assertEquals(0, motor.getVelocityRad_S(), kDelta); + assertEquals(1, encoder.getPositionRad().getAsDouble(), kDelta); + assertEquals(0, encoder.getVelocityRad_S().getAsDouble(), kDelta); + assertEquals(0, mech.getVelocityRad_S().getAsDouble(), kDelta); + // all the way there now + assertEquals(1, sensor.getPositionRad().getAsDouble(), kDelta); + + } +} diff --git a/lib/src/test/java/org/team100/lib/path/Path100Test.java b/lib/src/test/java/org/team100/lib/path/Path100Test.java index 32e82d50..6254f1b5 100755 --- a/lib/src/test/java/org/team100/lib/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/path/Path100Test.java @@ -58,12 +58,12 @@ public Translation2d getPoint(double t) { @Override public Rotation2d getHeading(double t) { - return GeometryUtil.kRotationZero; + return Rotation2d.kZero; } @Override public Optional getCourse(double t) { - return Optional.of(GeometryUtil.kRotationZero); + return Optional.of(Rotation2d.kZero); } @Override diff --git a/lib/src/test/java/org/team100/lib/path/PathPlannerTest.java b/lib/src/test/java/org/team100/lib/path/PathPlannerTest.java index c6367bf5..a5a459e5 100644 --- a/lib/src/test/java/org/team100/lib/path/PathPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/path/PathPlannerTest.java @@ -74,10 +74,10 @@ void testForced() throws TimingException { void testBackingUp() { List waypoints = List.of( new Pose2d(0, 0, new Rotation2d(Math.PI)), - new Pose2d(1, 0, GeometryUtil.kRotationZero)); + new Pose2d(1, 0, Rotation2d.kZero)); List headings = List.of( - GeometryUtil.kRotationZero, - GeometryUtil.kRotationZero); + Rotation2d.kZero, + Rotation2d.kZero); Path100 path = PathPlanner.pathFromWaypointsAndHeadings( waypoints, headings, diff --git a/lib/src/test/java/org/team100/lib/profile/HolonomicProfileTest.java b/lib/src/test/java/org/team100/lib/profile/HolonomicProfileTest.java index ea2b0d03..88d37f19 100644 --- a/lib/src/test/java/org/team100/lib/profile/HolonomicProfileTest.java +++ b/lib/src/test/java/org/team100/lib/profile/HolonomicProfileTest.java @@ -1,7 +1,6 @@ package org.team100.lib.profile; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.motion.drivetrain.SwerveControl; import org.team100.lib.motion.drivetrain.SwerveModel; import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity; @@ -9,6 +8,7 @@ import org.team100.lib.util.Util; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; class HolonomicProfileTest { private static final boolean PRINT = false; @@ -17,7 +17,7 @@ class HolonomicProfileTest { void test2d() { HolonomicProfile hp = new HolonomicProfile(1, 1, 0.01, 1, 1, 0.01); SwerveModel i = new SwerveModel(); - SwerveModel g = new SwerveModel(new Pose2d(1, 5, GeometryUtil.kRotationZero)); + SwerveModel g = new SwerveModel(new Pose2d(1, 5, Rotation2d.kZero)); hp.solve(i, g); SwerveControl s = i.control(); for (double t = 0; t < 10; t += 0.02) { @@ -31,7 +31,7 @@ void test2d() { void test2dWithEntrySpeed() { HolonomicProfile hp = new HolonomicProfile(1, 1, 0.01, 1, 1, 0.01); SwerveModel i = new SwerveModel(new Pose2d(), new FieldRelativeVelocity(1, 0, 0)); - SwerveModel g = new SwerveModel(new Pose2d(0, 1, GeometryUtil.kRotationZero)); + SwerveModel g = new SwerveModel(new Pose2d(0, 1, Rotation2d.kZero)); hp.solve(i, g); SwerveControl s = i.control(); for (double t = 0; t < 10; t += 0.02) { @@ -49,7 +49,7 @@ void test2dWithEntrySpeed() { void testSolvePerformance() { HolonomicProfile hp = new HolonomicProfile(1, 1, 0.01, 1, 1, 0.01); SwerveModel i = new SwerveModel(new Pose2d(), new FieldRelativeVelocity(1, 0, 0)); - SwerveModel g = new SwerveModel(new Pose2d(0, 1, GeometryUtil.kRotationZero)); + SwerveModel g = new SwerveModel(new Pose2d(0, 1, Rotation2d.kZero)); int N = 1000000; double t0 = Takt.actual(); for (int ii = 0; ii < N; ++ii) { diff --git a/lib/src/test/java/org/team100/lib/reference/ProfileReferenceTest.java b/lib/src/test/java/org/team100/lib/reference/ProfileReferenceTest.java index 7d313e57..f03a6d4f 100644 --- a/lib/src/test/java/org/team100/lib/reference/ProfileReferenceTest.java +++ b/lib/src/test/java/org/team100/lib/reference/ProfileReferenceTest.java @@ -3,20 +3,20 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.motion.drivetrain.SwerveModel; import org.team100.lib.profile.HolonomicProfile; import org.team100.lib.testing.Timeless; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; public class ProfileReferenceTest implements Timeless { private static final double kDelta = 0.001; @Test void testSimple() { - SwerveModel measurement = new SwerveModel(new Pose2d(0, 0, GeometryUtil.kRotationZero)); - SwerveModel goal = new SwerveModel(new Pose2d(1, 0, GeometryUtil.kRotationZero)); + SwerveModel measurement = new SwerveModel(new Pose2d(0, 0, Rotation2d.kZero)); + SwerveModel goal = new SwerveModel(new Pose2d(1, 0, Rotation2d.kZero)); HolonomicProfile hp = new HolonomicProfile(1, 1, 0.01, 1, 1, 0.01); ProfileReference r = new ProfileReference(hp); r.setGoal(goal); diff --git a/lib/src/test/java/org/team100/lib/reference/TrajectoryReferenceTest.java b/lib/src/test/java/org/team100/lib/reference/TrajectoryReferenceTest.java index 59bcb93c..2ef65cb2 100644 --- a/lib/src/test/java/org/team100/lib/reference/TrajectoryReferenceTest.java +++ b/lib/src/test/java/org/team100/lib/reference/TrajectoryReferenceTest.java @@ -5,7 +5,6 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.motion.drivetrain.SwerveModel; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamicsFactory; @@ -16,6 +15,7 @@ import org.team100.lib.trajectory.TrajectoryPlanner; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; public class TrajectoryReferenceTest implements Timeless { private static final double kDelta = 0.001; @@ -26,8 +26,8 @@ public class TrajectoryReferenceTest implements Timeless { @Test void testSimple() { Trajectory100 t = planner.restToRest( - new Pose2d(0, 0, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotationZero)); + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)); TrajectoryReference r = new TrajectoryReference(t); // measurement is irrelevant r.initialize(new SwerveModel()); diff --git a/lib/src/test/java/org/team100/lib/sensors/MockGyro.java b/lib/src/test/java/org/team100/lib/sensors/MockGyro.java index aeb1352a..e7df0969 100644 --- a/lib/src/test/java/org/team100/lib/sensors/MockGyro.java +++ b/lib/src/test/java/org/team100/lib/sensors/MockGyro.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.geometry.Rotation2d; public class MockGyro implements Gyro { - public Rotation2d rotation = GeometryUtil.kRotationZero; + public Rotation2d rotation = Rotation2d.kZero; public double rate = 0; @Override @@ -20,12 +20,12 @@ public double getYawRateNWU() { @Override public Rotation2d getPitchNWU() { - return GeometryUtil.kRotationZero; + return Rotation2d.kZero; } @Override public Rotation2d getRollNWU() { - return GeometryUtil.kRotationZero; + return Rotation2d.kZero; } @Override diff --git a/lib/src/test/java/org/team100/lib/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/spline/HolonomicSplineTest.java index 477772be..bbda155f 100644 --- a/lib/src/test/java/org/team100/lib/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/spline/HolonomicSplineTest.java @@ -70,26 +70,26 @@ void testLinear() { void testBounds() { { // allow 0 degrees - Pose2d p0 = new Pose2d(0, 0, GeometryUtil.kRotationZero); - Pose2d p1 = new Pose2d(1, 0, GeometryUtil.kRotationZero); + Pose2d p0 = new Pose2d(0, 0, Rotation2d.kZero); + Pose2d p1 = new Pose2d(1, 0, Rotation2d.kZero); assertDoesNotThrow(() -> HolonomicSpline.checkBounds(p0, p1)); } { // allow 90 degrees Pose2d p0 = new Pose2d(0, 0, new Rotation2d(Math.PI / 2)); - Pose2d p1 = new Pose2d(1, 0, GeometryUtil.kRotationZero); + Pose2d p1 = new Pose2d(1, 0, Rotation2d.kZero); assertDoesNotThrow(() -> HolonomicSpline.checkBounds(p0, p1)); } { // allow 135 degrees Pose2d p0 = new Pose2d(0, 0, new Rotation2d(3 * Math.PI / 4)); - Pose2d p1 = new Pose2d(1, 0, GeometryUtil.kRotationZero); + Pose2d p1 = new Pose2d(1, 0, Rotation2d.kZero); assertDoesNotThrow(() -> HolonomicSpline.checkBounds(p0, p1)); } { // disallow u-turn; these are never what you want. Pose2d p0 = new Pose2d(0, 0, new Rotation2d(Math.PI)); - Pose2d p1 = new Pose2d(1, 0, GeometryUtil.kRotationZero); + Pose2d p1 = new Pose2d(1, 0, Rotation2d.kZero); assertDoesNotThrow(() -> HolonomicSpline.checkBounds(p0, p1)); } } @@ -343,12 +343,14 @@ void testMismatchedXYDerivatives() { } Path100 path = new Path100(SplineGenerator.parameterizeSplines(splines, 0.05, 0.05, 0.05)); - Util.printf("path %s\n", path); + if (DEBUG) + Util.printf("path %s\n", path); List constraints = new ArrayList<>(); ScheduleGenerator scheduleGenerator = new ScheduleGenerator(constraints); Trajectory100 trajectory = scheduleGenerator.timeParameterizeTrajectory(path, 0.05, 0, 0); - Util.printf("trajectory %s\n", trajectory); + if (DEBUG) + Util.printf("trajectory %s\n", trajectory); } diff --git a/lib/src/test/java/org/team100/lib/spline/QuinticHermiteOptimizerTest.java b/lib/src/test/java/org/team100/lib/spline/QuinticHermiteOptimizerTest.java index 14e327ca..927e6a33 100755 --- a/lib/src/test/java/org/team100/lib/spline/QuinticHermiteOptimizerTest.java +++ b/lib/src/test/java/org/team100/lib/spline/QuinticHermiteOptimizerTest.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.geometry.Translation2d; class QuinticHermiteOptimizerTest { - private static final boolean DEBUG = false; private static double kEpsilon = 1e-12; @Test diff --git a/lib/src/test/java/org/team100/lib/spline/SplineGeneratorTest.java b/lib/src/test/java/org/team100/lib/spline/SplineGeneratorTest.java index 65398fb6..a63aea46 100755 --- a/lib/src/test/java/org/team100/lib/spline/SplineGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/spline/SplineGeneratorTest.java @@ -16,7 +16,7 @@ class SplineGeneratorTest { @Test void test() { - Pose2d p1 = new Pose2d(new Translation2d(0, 0), GeometryUtil.kRotationZero); + Pose2d p1 = new Pose2d(new Translation2d(0, 0), Rotation2d.kZero); Pose2d p2 = new Pose2d(new Translation2d(15, 10), new Rotation2d(1, 5)); HolonomicSpline s = new HolonomicSpline( p1, p2, new Rotation2d(), new Rotation2d()); diff --git a/lib/src/test/java/org/team100/lib/timing/ScheduleGeneratorTest.java b/lib/src/test/java/org/team100/lib/timing/ScheduleGeneratorTest.java index 1e4154ae..4b5c47a5 100755 --- a/lib/src/test/java/org/team100/lib/timing/ScheduleGeneratorTest.java +++ b/lib/src/test/java/org/team100/lib/timing/ScheduleGeneratorTest.java @@ -28,10 +28,10 @@ public class ScheduleGeneratorTest { public static final double kTestEpsilon = 1e-12; public static final List kWaypoints = Arrays.asList( - new Pose2dWithMotion(new Pose2d(new Translation2d(0.0, 0.0), GeometryUtil.kRotationZero)), - new Pose2dWithMotion(new Pose2d(new Translation2d(24.0, 0.0), GeometryUtil.kRotationZero)), - new Pose2dWithMotion(new Pose2d(new Translation2d(36.0, 12.0), GeometryUtil.kRotationZero)), - new Pose2dWithMotion(new Pose2d(new Translation2d(60.0, 12.0), GeometryUtil.kRotationZero))); + new Pose2dWithMotion(new Pose2d(new Translation2d(0.0, 0.0), Rotation2d.kZero)), + new Pose2dWithMotion(new Pose2d(new Translation2d(24.0, 0.0), Rotation2d.kZero)), + new Pose2dWithMotion(new Pose2d(new Translation2d(36.0, 12.0), Rotation2d.kZero)), + new Pose2dWithMotion(new Pose2d(new Translation2d(60.0, 12.0), Rotation2d.kZero))); public static final List kHeadings = List.of( GeometryUtil.fromDegrees(0), @@ -100,12 +100,12 @@ void testJustTurningInPlace() { new Pose2dWithMotion( new Pose2d( new Translation2d(0.0, 0.0), - GeometryUtil.kRotationZero), + Rotation2d.kZero), new MotionDirection(0, 0, 1), 0, 0), new Pose2dWithMotion( new Pose2d( new Translation2d(0.0, 0.0), - GeometryUtil.kRotation180), + Rotation2d.kPi), new MotionDirection(0, 0, 1), 0, 0))); // Triangle profile. diff --git a/lib/src/test/java/org/team100/lib/timing/SwerveDriveDynamicsConstraintTest.java b/lib/src/test/java/org/team100/lib/timing/SwerveDriveDynamicsConstraintTest.java index a74b9eff..88bdcd5d 100644 --- a/lib/src/test/java/org/team100/lib/timing/SwerveDriveDynamicsConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/timing/SwerveDriveDynamicsConstraintTest.java @@ -15,10 +15,6 @@ class SwerveDriveDynamicsConstraintTest { private static final double kDelta = 0.001; - // the free speed of a module, which is also the free speed - // of the robot going in a straight line without rotating. - private static final double maxV = 4; - @Test void testVelocity() { SwerveKinodynamics l = SwerveKinodynamicsFactory.get(); @@ -26,7 +22,7 @@ void testVelocity() { // motionless double m = c.getMaxVelocity(new Pose2dWithMotion( - GeometryUtil.kPoseZero, new Pose2dWithMotion.MotionDirection(0, 0, 0), 0, 0)).getValue(); + Pose2d.kZero, new Pose2dWithMotion.MotionDirection(0, 0, 0), 0, 0)).getValue(); assertEquals(5, m, kDelta); // moving in +x, no curvature, no rotation @@ -52,7 +48,7 @@ void testAccel() { SwerveDriveDynamicsConstraint c = new SwerveDriveDynamicsConstraint(l); // this is constant MinMaxAcceleration m = c.getMinMaxAcceleration(new Pose2dWithMotion( - GeometryUtil.kPoseZero, new Pose2dWithMotion.MotionDirection(0, 0, 0), 0, 0), 0); + Pose2d.kZero, new Pose2dWithMotion.MotionDirection(0, 0, 0), 0, 0), 0); assertEquals(-20, m.getMinAccel(), kDelta); assertEquals(10, m.getMaxAccel(), kDelta); } diff --git a/lib/src/test/java/org/team100/lib/timing/YawRateConstraintTest.java b/lib/src/test/java/org/team100/lib/timing/YawRateConstraintTest.java index d9858bed..2b986bf1 100644 --- a/lib/src/test/java/org/team100/lib/timing/YawRateConstraintTest.java +++ b/lib/src/test/java/org/team100/lib/timing/YawRateConstraintTest.java @@ -24,7 +24,6 @@ void testSpin() { new Pose2d(), new MotionDirection(0, 0, 1), 0, 0); // the limiter is in the scheduler which only thinks about the along-the-path // dimension. - // TODO: do a real holonomic scheduler assertEquals(Double.NEGATIVE_INFINITY, c.getMinMaxAcceleration(p, 0).getMinAccel(), kDelta); assertEquals(Double.POSITIVE_INFINITY, diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java index 153e512a..693d75c0 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java @@ -25,8 +25,8 @@ class Trajectory100Test { @Test void testPreviewAndAdvance() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.get(); - Pose2d start = GeometryUtil.kPoseZero; - Pose2d end = start.plus(new Transform2d(1, 0, GeometryUtil.kRotationZero)); + Pose2d start = Pose2d.kZero; + Pose2d end = start.plus(new Transform2d(1, 0, Rotation2d.kZero)); Translation2d currentTranslation = start.getTranslation(); Translation2d goalTranslation = end.getTranslation(); @@ -61,8 +61,8 @@ void testPreviewAndAdvance() { @Test void testSample() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(); - Pose2d start = GeometryUtil.kPoseZero; - Pose2d end = start.plus(new Transform2d(1, 0, GeometryUtil.kRotationZero)); + Pose2d start = Pose2d.kZero; + Pose2d end = start.plus(new Transform2d(1, 0, Rotation2d.kZero)); Translation2d currentTranslation = start.getTranslation(); Translation2d goalTranslation = end.getTranslation(); @@ -97,12 +97,12 @@ void testSample() { void testSampleThoroughly() { List waypointsM = List.of( - new Pose2d(new Translation2d(), GeometryUtil.kRotationZero), - new Pose2d(new Translation2d(1, 0), GeometryUtil.kRotationZero)); + new Pose2d(new Translation2d(), Rotation2d.kZero), + new Pose2d(new Translation2d(1, 0), Rotation2d.kZero)); List headings = List.of( - GeometryUtil.kRotationZero, - GeometryUtil.kRotationZero); + Rotation2d.kZero, + Rotation2d.kZero); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(); List constraints = new TimingConstraintFactory(limits).fast(); @@ -133,12 +133,12 @@ void testSampleThoroughly() { void testSampleThoroughlyWithRotation() { List waypointsM = List.of( - new Pose2d(new Translation2d(), GeometryUtil.kRotationZero), - new Pose2d(new Translation2d(1, 0), GeometryUtil.kRotationZero)); + new Pose2d(new Translation2d(), Rotation2d.kZero), + new Pose2d(new Translation2d(1, 0), Rotation2d.kZero)); List headings = List.of( - GeometryUtil.kRotationZero, - GeometryUtil.kRotation90); + Rotation2d.kZero, + Rotation2d.kCCW_Pi_2); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(); List constraints = new TimingConstraintFactory(limits).fast(); @@ -173,14 +173,14 @@ void testSampleThoroughlyWithRotation() { // @Test void testSamplePerformance() { List waypointsM = List.of( - new Pose2d(new Translation2d(), GeometryUtil.kRotationZero), - new Pose2d(new Translation2d(10, 0), GeometryUtil.kRotationZero), - new Pose2d(new Translation2d(10, 10), GeometryUtil.kRotationZero)); + new Pose2d(new Translation2d(), Rotation2d.kZero), + new Pose2d(new Translation2d(10, 0), Rotation2d.kZero), + new Pose2d(new Translation2d(10, 10), Rotation2d.kZero)); List headings = List.of( - GeometryUtil.kRotationZero, - GeometryUtil.kRotation90, - GeometryUtil.kRotation180); + Rotation2d.kZero, + Rotation2d.kCCW_Pi_2, + Rotation2d.kPi); SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest3(); List constraints = new TimingConstraintFactory(limits).fast(); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index 1d842972..3dd92177 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -58,10 +58,10 @@ void testLinear() { void testBackingUp() { List waypoints = List.of( new Pose2d(0, 0, new Rotation2d(Math.PI)), - new Pose2d(1, 0, GeometryUtil.kRotationZero)); + new Pose2d(1, 0, Rotation2d.kZero)); List headings = List.of( - GeometryUtil.kRotationZero, - GeometryUtil.kRotationZero); + Rotation2d.kZero, + Rotation2d.kZero); SwerveKinodynamics limits = SwerveKinodynamicsFactory.get(); // these are the same as StraightLineTrajectoryTest. @@ -134,8 +134,8 @@ void testRestToRest() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.get(); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - SwerveModel start = new SwerveModel(GeometryUtil.kPoseZero, new FieldRelativeVelocity(0, 0, 0)); - Pose2d end = new Pose2d(1, 0, GeometryUtil.kRotationZero); + SwerveModel start = new SwerveModel(Pose2d.kZero, new FieldRelativeVelocity(0, 0, 0)); + Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 trajectory = planner.restToRest(start.pose(), end); assertEquals(0.904, trajectory.duration(), kDelta); @@ -162,8 +162,8 @@ void testMovingToRest() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.get(); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - SwerveModel start = new SwerveModel(GeometryUtil.kPoseZero, new FieldRelativeVelocity(1, 0, 0)); - Pose2d end = new Pose2d(1, 0, GeometryUtil.kRotationZero); + SwerveModel start = new SwerveModel(Pose2d.kZero, new FieldRelativeVelocity(1, 0, 0)); + Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); assertEquals(0.744, traj.duration(), kDelta); } @@ -173,8 +173,8 @@ void testBackingUp2() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.get(); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - SwerveModel start = new SwerveModel(GeometryUtil.kPoseZero, new FieldRelativeVelocity(-1, 0, 0)); - Pose2d end = new Pose2d(1, 0, GeometryUtil.kRotationZero); + SwerveModel start = new SwerveModel(Pose2d.kZero, new FieldRelativeVelocity(-1, 0, 0)); + Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); assertEquals(0.877, traj.duration(), kDelta); } @@ -184,8 +184,8 @@ void test2d() { SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.get(); List constraints = new TimingConstraintFactory(swerveKinodynamics).allGood(); TrajectoryPlanner planner = new TrajectoryPlanner(constraints); - SwerveModel start = new SwerveModel(GeometryUtil.kPoseZero, new FieldRelativeVelocity(0, 1, 0)); - Pose2d end = new Pose2d(1, 0, GeometryUtil.kRotationZero); + SwerveModel start = new SwerveModel(Pose2d.kZero, new FieldRelativeVelocity(0, 1, 0)); + Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); Trajectory100 traj = planner.movingToRest(start, end); assertEquals(1.247, traj.duration(), kDelta); } diff --git a/lib/src/test/java/org/team100/lib/util/DriveUtilTest.java b/lib/src/test/java/org/team100/lib/util/DriveUtilTest.java index 981e8185..1a505626 100644 --- a/lib/src/test/java/org/team100/lib/util/DriveUtilTest.java +++ b/lib/src/test/java/org/team100/lib/util/DriveUtilTest.java @@ -13,6 +13,7 @@ import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleDeltas; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePosition100; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; @@ -91,18 +92,18 @@ void testRoundTripModuleDeltas() { void testOneModule() { { SwerveModulePosition100 start = new SwerveModulePosition100( - 0, Optional.of(GeometryUtil.kRotationZero)); + 0, Optional.of(Rotation2d.kZero)); SwerveModulePosition100 end = new SwerveModulePosition100( - 0, Optional.of(GeometryUtil.kRotationZero)); + 0, Optional.of(Rotation2d.kZero)); SwerveModuleDelta delta = DriveUtil.delta(start, end); assertEquals(0, delta.distanceMeters, kDelta); assertEquals(0, delta.angle.get().getRadians(), kDelta); } { SwerveModulePosition100 start = new SwerveModulePosition100( - 0, Optional.of(GeometryUtil.kRotationZero)); + 0, Optional.of(Rotation2d.kZero)); SwerveModulePosition100 end = new SwerveModulePosition100( - 1, Optional.of(GeometryUtil.kRotationZero)); + 1, Optional.of(Rotation2d.kZero)); SwerveModuleDelta delta = DriveUtil.delta(start, end); assertEquals(1, delta.distanceMeters, kDelta); assertEquals(0, delta.angle.get().getRadians(), kDelta); @@ -111,9 +112,9 @@ void testOneModule() { // this ignores the initial zero rotation, and acts as if // the path is at 90 degrees the whole time. SwerveModulePosition100 start = new SwerveModulePosition100( - 0, Optional.of(GeometryUtil.kRotationZero)); + 0, Optional.of(Rotation2d.kZero)); SwerveModulePosition100 end = new SwerveModulePosition100( - 1, Optional.of(GeometryUtil.kRotation90)); + 1, Optional.of(Rotation2d.kCCW_Pi_2)); SwerveModuleDelta delta = DriveUtil.delta(start, end); assertEquals(1, delta.distanceMeters, kDelta); assertEquals(1.571, delta.angle.get().getRadians(), kDelta); diff --git a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java b/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java index 453286d5..787eb4cf 100644 --- a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java +++ b/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java @@ -72,14 +72,14 @@ void testProject() { @Test void testSlog() { Twist2d twist = GeometryUtil.slog( - new Pose2d(1, 0, GeometryUtil.kRotationZero)); + new Pose2d(1, 0, Rotation2d.kZero)); assertEquals(1, twist.dx, kDelta); assertEquals(0, twist.dy, kDelta); assertEquals(0, twist.dtheta, kDelta); // this twist represents an arc that ends up at the endpoint. twist = GeometryUtil.slog( - new Pose2d(1, 0, GeometryUtil.kRotation90)); + new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)); assertEquals(0.785, twist.dx, kDelta); assertEquals(-0.785, twist.dy, kDelta); assertEquals(1.571, twist.dtheta, kDelta); @@ -90,38 +90,38 @@ void testDistance() { // same pose => 0 assertEquals(0, GeometryUtil.distance( - new Pose2d(1, 0, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotationZero)), + new Pose2d(1, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)), kDelta); // 1d distance assertEquals(1, GeometryUtil.distance( - new Pose2d(0, 0, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotationZero)), + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)), kDelta); // 2d distance assertEquals(1.414, GeometryUtil.distance( - new Pose2d(0, 1, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotationZero)), + new Pose2d(0, 1, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kZero)), kDelta); // rotation means a little arc, so the path length is a little longer. assertEquals(1.111, GeometryUtil.distance( - new Pose2d(0, 0, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotation90)), + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)), kDelta); // the arc in this case is the entire quarter circle assertEquals(1.571, GeometryUtil.distance( - new Pose2d(0, 1, GeometryUtil.kRotationZero), - new Pose2d(1, 0, GeometryUtil.kRotation90)), + new Pose2d(0, 1, Rotation2d.kZero), + new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)), kDelta); // order doesn't matter assertEquals(1.571, GeometryUtil.distance( - new Pose2d(1, 0, GeometryUtil.kRotation90), - new Pose2d(0, 1, GeometryUtil.kRotationZero)), + new Pose2d(1, 0, Rotation2d.kCCW_Pi_2), + new Pose2d(0, 1, Rotation2d.kZero)), kDelta); } diff --git a/lib/src/test/java/org/team100/lib/util/TargetUtilTest.java b/lib/src/test/java/org/team100/lib/util/TargetUtilTest.java index 0815668e..d5a0433c 100644 --- a/lib/src/test/java/org/team100/lib/util/TargetUtilTest.java +++ b/lib/src/test/java/org/team100/lib/util/TargetUtilTest.java @@ -119,7 +119,7 @@ void testTargetMotionY() { void testTargetMotionYReversed() { // in front of the origin, facing back to it, moving 1m/s +y, SwerveModel state = new SwerveModel( - new Pose2d(1, 0, GeometryUtil.kRotation180), + new Pose2d(1, 0, Rotation2d.kPi), new FieldRelativeVelocity(0, 1, 0)); // target is dead ahead Translation2d target = new Translation2d(0, 0); diff --git a/lib/src/test/java/org/team100/lib/wpi_trajectory/TestMalformedSpline.java b/lib/src/test/java/org/team100/lib/wpi_trajectory/TestMalformedSpline.java index cdd33de1..cdde843c 100644 --- a/lib/src/test/java/org/team100/lib/wpi_trajectory/TestMalformedSpline.java +++ b/lib/src/test/java/org/team100/lib/wpi_trajectory/TestMalformedSpline.java @@ -7,21 +7,20 @@ import java.util.NoSuchElementException; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.GeometryUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; class TestMalformedSpline { @Test void malformedSplineTest() throws IOException { - // TODO: NoSuchElementException here probably indicates a bug assertThrows(NoSuchElementException.class, () -> TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, GeometryUtil.kRotationZero), + new Pose2d(0, 0, Rotation2d.kZero), List.of(), - new Pose2d(0, 0, GeometryUtil.kRotationZero), + new Pose2d(0, 0, Rotation2d.kZero), new TrajectoryConfig(6, 3))); }