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 30, 2024
1 parent 4f0da97 commit 346ea41
Show file tree
Hide file tree
Showing 3 changed files with 50 additions and 44 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ public static class AutoNames {
}

public static class Drive {
public static final DriveWheel DRIVE_WHEEL = new DriveWheel(Units.Inches.of(3.0), Units.Value.of(1.1), Units.Value.of(1.0));
public static final DriveWheel DRIVE_WHEEL = new DriveWheel(Units.Inches.of(3.0), Units.Value.of(1.0), Units.Value.of(0.9));
public static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(8.0, 0.0, 0.3, 0.0, 0.0);
public static final Measure<Dimensionless> DRIVE_SLIP_RATIO = Units.Percent.of(8.0);
public static final double DRIVE_TURN_SCALAR = 90.0;
Expand Down
24 changes: 16 additions & 8 deletions src/test/java/frc/robot/commands/AntiTipCommandTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
package frc.robot.commands;

import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.mockito.ArgumentMatchers.doubleThat;
import static org.mockito.Mockito.mock;
import static org.mockito.Mockito.times;
import static org.mockito.Mockito.verify;
Expand All @@ -27,6 +28,7 @@
import org.lasarobotics.utils.GlobalConstants;
import org.lasarobotics.vision.AprilTagCamera;
import org.mockito.AdditionalMatchers;
import org.mockito.ArgumentMatcher;
import org.mockito.ArgumentMatchers;

import com.revrobotics.CANSparkBase.ControlType;
Expand All @@ -38,6 +40,7 @@
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.AbsoluteValueMatcher;
import frc.robot.Constants;
import frc.robot.subsystems.drive.DriveSubsystem;

Expand All @@ -55,6 +58,8 @@ public class AntiTipCommandTest {
private Spark m_lRearDriveMotor, m_lRearRotateMotor;
private Spark m_rRearDriveMotor, m_rRearRotateMotor;

private ArgumentMatcher<Double> m_matchLinearVelocity;

@BeforeEach
public void setup() {
HAL.initialize(500, 0);
Expand Down Expand Up @@ -180,6 +185,9 @@ public void setup() {
Constants.Drive.DRIVE_LOOKAHEAD
);

// Velocity matcher
m_matchLinearVelocity = new AbsoluteValueMatcher(m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond) / 4, DELTA);

// Create AntiTipCommand object
m_antiTipCommand = m_driveSubsystem.ANTI_TIP_COMMAND;
}
Expand Down Expand Up @@ -223,14 +231,14 @@ public void execute() {
m_antiTipCommand.execute();

// Verify motors are being driven with expected values
verify(m_lFrontDriveMotor, times(1)).set(AdditionalMatchers.eq(-m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond) / 4, DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(AdditionalMatchers.eq(+Math.PI, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(AdditionalMatchers.eq(+m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond) / 4, DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(AdditionalMatchers.eq(+Math.PI / 2, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(AdditionalMatchers.eq(+m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond) / 4, DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(AdditionalMatchers.eq(-Math.PI / 2, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(AdditionalMatchers.eq(-m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond) / 4, DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(AdditionalMatchers.eq(0.0, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lFrontDriveMotor, times(1)).set(doubleThat(m_matchLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(doubleThat(m_matchLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(doubleThat(m_matchLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(Math.PI / 2, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(doubleThat(m_matchLinearVelocity), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(doubleThat(new AbsoluteValueMatcher(0.0, DELTA)), ArgumentMatchers.eq(ControlType.kPosition));
}

@Test
Expand Down
68 changes: 33 additions & 35 deletions src/test/java/frc/robot/subsystems/DriveSubsystemTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,6 @@
import static org.mockito.Mockito.times;
import static org.mockito.Mockito.verify;
import static org.mockito.Mockito.when;
// import static org.mockito.AdditionalMatchers.eq;
// import static org.mockito.AdditionalMatchers.or;
// import static org.mockito.AdditionalMatchers.and;

import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
Expand Down Expand Up @@ -185,6 +182,7 @@ public void setup() {
Constants.Drive.DRIVE_LOOKAHEAD
);

// Max linear velocity matcher
m_matchMaxLinearVelocity = new AbsoluteValueMatcher(m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA);

// Disable traction control for unit tests
Expand Down Expand Up @@ -258,14 +256,14 @@ public void reverse() {
m_driveSubsystem.driveCommand(() -> -1.0, () -> 0.0, () -> 0.0).execute();

// Verify that motors are being driven with expected values
verify(m_lFrontDriveMotor, times(1)).set(AdditionalMatchers.eq(-m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(AdditionalMatchers.eq(+Math.PI / 2, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(AdditionalMatchers.eq(+m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(AdditionalMatchers.eq(0.0, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(AdditionalMatchers.eq(+m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(AdditionalMatchers.eq(+Math.PI, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(AdditionalMatchers.eq(-m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(AdditionalMatchers.eq(-Math.PI / 2, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
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_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_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_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));
}

@Test
Expand All @@ -287,14 +285,14 @@ public void strafeLeft() {
m_driveSubsystem.driveCommand(() -> 0.0, () -> +1.0, () -> 0.0).execute();

// Verify motors are being driven with expected values
verify(m_lFrontDriveMotor, times(1)).set(AdditionalMatchers.eq(+m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(AdditionalMatchers.eq(+Math.PI, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(AdditionalMatchers.eq(-m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(AdditionalMatchers.eq(+Math.PI / 2, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(AdditionalMatchers.eq(-m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(AdditionalMatchers.eq(-Math.PI / 2, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(AdditionalMatchers.eq(+m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(AdditionalMatchers.eq(0.0, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
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_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_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_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));
}

@Test
Expand All @@ -316,14 +314,14 @@ public void strafeRight() {
m_driveSubsystem.driveCommand(() -> 0.0, () -> -1.0, () -> 0.0).execute();

// Verify that motors are being driven with expected values
verify(m_lFrontDriveMotor, times(1)).set(AdditionalMatchers.eq(-m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(AdditionalMatchers.eq(-Math.PI, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(AdditionalMatchers.eq(+m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(AdditionalMatchers.eq(+Math.PI / 2, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(AdditionalMatchers.eq(+m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(AdditionalMatchers.eq(-Math.PI / 2, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(AdditionalMatchers.eq(-m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(AdditionalMatchers.eq(0.0, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
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_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_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_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));
}

@Test
Expand Down Expand Up @@ -485,14 +483,14 @@ public void disableTractionControl() {
m_driveSubsystem.driveCommand(() -> +1.0, () -> 0.0, () -> 0.0).execute();

// Verify that motors are being driven with expected values
verify(m_lFrontDriveMotor, times(1)).set(AdditionalMatchers.eq(+m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lFrontRotateMotor, times(1)).set(AdditionalMatchers.eq(+Math.PI / 2, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rFrontDriveMotor, times(1)).set(AdditionalMatchers.eq(-m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rFrontRotateMotor, times(1)).set(AdditionalMatchers.eq(0.0, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_lRearDriveMotor, times(1)).set(AdditionalMatchers.eq(-m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_lRearRotateMotor, times(1)).set(AdditionalMatchers.eq(+Math.PI, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
verify(m_rRearDriveMotor, times(1)).set(AdditionalMatchers.eq(+m_driveSubsystem.DRIVE_MAX_LINEAR_SPEED.in(Units.MetersPerSecond), DELTA), ArgumentMatchers.eq(ControlType.kVelocity));
verify(m_rRearRotateMotor, times(1)).set(AdditionalMatchers.eq(-Math.PI / 2, DELTA), ArgumentMatchers.eq(ControlType.kPosition));
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_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_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_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));
}

@Test
Expand Down

0 comments on commit 346ea41

Please sign in to comment.