Skip to content

Commit

Permalink
Fix unit tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@viggy96
viggy96 committed Jul 31, 2024
1 parent 199fbe3 commit 21c3576
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 21 deletions.
37 changes: 37 additions & 0 deletions src/test/java/frc/robot/AngleMatcher.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import org.apache.commons.math3.util.Precision;
import org.mockito.ArgumentMatcher;


public class AngleMatcher implements ArgumentMatcher<Double> {
public enum Units {
RADIANS, DEGREES;
}

private Units m_units;
private double m_value;
private double m_delta;

public AngleMatcher(Units units, double value, double delta) {
this.m_units = units;
this.m_value = value;
this.m_delta = delta;
}

@Override
public boolean matches(Double number) {
if (m_units.equals(Units.RADIANS) && Precision.equals(Math.abs(number), Math.PI, m_delta)) {
return Precision.equals(Math.abs(number), Math.PI, m_delta);
} else if (m_units.equals(Units.DEGREES) && Precision.equals(Math.abs(number), 180, m_delta)) {
return Precision.equals(Math.abs(number), 180, m_delta);
}

return Precision.equals(number, m_value, m_delta)
|| Precision.equals(number + (m_units.equals(Units.RADIANS) ? Math.PI : 180), m_value, m_delta);
}
}
43 changes: 22 additions & 21 deletions src/test/java/frc/robot/subsystems/DriveSubsystemTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,9 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import frc.robot.Constants;
import frc.robot.AbsoluteValueMatcher;
import frc.robot.AngleMatcher;
import frc.robot.Constants;
import frc.robot.subsystems.drive.DriveSubsystem;

@TestMethodOrder(MethodOrderer.OrderAnnotation.class)
Expand Down Expand Up @@ -228,13 +229,13 @@ public void forward() {

// Verify that motors are being driven with expected values
verify(m_lFrontDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, 0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
}

@Test
Expand All @@ -257,13 +258,13 @@ public void reverse() {

// Verify that motors are being driven with expected values
verify(m_lFrontDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, 0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
}

@Test
Expand All @@ -286,13 +287,13 @@ public void strafeLeft() {

// Verify motors are being driven with expected values
verify(m_lFrontDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, 0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
}

@Test
Expand All @@ -315,13 +316,13 @@ public void strafeRight() {

// Verify that motors are being driven with expected values
verify(m_lFrontDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, 0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
}

@Test
Expand Down Expand Up @@ -484,13 +485,13 @@ public void disableTractionControl() {

// Verify that motors are being driven with expected values
verify(m_lFrontDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, 0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(doubleThat(m_matchMaxLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AngleMatcher(AngleMatcher.Units.RADIANS, Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
}

@Test
Expand Down

0 comments on commit 21c3576

Please sign in to comment.