Skip to content

Commit

Permalink
Add ability to disable field-oriented driving
Browse files Browse the repository at this point in the history
  • Loading branch information
@jhh
jhh committed Feb 3, 2019
1 parent 6453c9b commit 0b150e7
Show file tree
Hide file tree
Showing 3 changed files with 180 additions and 125 deletions.
2 changes: 1 addition & 1 deletion build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ buildscript {

configure(subprojects) {
group = 'org.strykeforce.thirdcoast'
version = '19.1.2'
version = '19.2.0'

apply plugin: 'java-library'
apply plugin: 'idea'
Expand Down
32 changes: 27 additions & 5 deletions swerve/src/main/java/org/strykeforce/thirdcoast/swerve/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,18 +23,18 @@ public class SwerveDrive {
public static final int DEFAULT_ABSOLUTE_AZIMUTH_OFFSET = 200;
private static final Logger logger = LoggerFactory.getLogger(SwerveDrive.class);
private static final int WHEEL_COUNT = 4;
final AHRS gyro;
private final AHRS gyro;
private final double kLengthComponent;
private final double kWidthComponent;
private final double kGyroRateCorrection;
private final Wheel[] wheels;
private final double[] ws = new double[WHEEL_COUNT];
private final double[] wa = new double[WHEEL_COUNT];
private boolean isFieldOriented;

public SwerveDrive(SwerveDriveConfig config) {
gyro = config.gyro;
wheels = config.wheels;
logger.info("field orientation driving is {}", gyro == null ? "DISABLED" : "ENABLED");

final boolean summarizeErrors = config.summarizeTalonErrors;
Errors.setSummarized(summarizeErrors);
Expand All @@ -47,7 +47,9 @@ public SwerveDrive(SwerveDriveConfig config) {
kLengthComponent = length / radius;
kWidthComponent = width / radius;

if (gyro != null && gyro.isConnected()) {
setFieldOriented(gyro != null && gyro.isConnected());

if (isFieldOriented) {
gyro.enableLogging(config.gyroLoggingEnabled);
double robotPeriod = config.robotPeriod;
double gyroRateCoeff = config.gyroRateCoeff;
Expand Down Expand Up @@ -112,14 +114,14 @@ public void drive(double forward, double strafe, double azimuth) {

// Use gyro for field-oriented drive. We use getAngle instead of getYaw to enable arbitrary
// autonomous starting positions.
if (gyro != null) {
if (isFieldOriented) {
double angle = gyro.getAngle();
angle += gyro.getRate() * kGyroRateCorrection;
angle = Math.IEEEremainder(angle, 360.0);

angle = Math.toRadians(angle);
final double temp = forward * Math.cos(angle) + strafe * Math.sin(angle);
strafe = -forward * Math.sin(angle) + strafe * Math.cos(angle);
strafe = strafe * Math.cos(angle) - forward * Math.sin(angle);
forward = temp;
}

Expand Down Expand Up @@ -228,6 +230,26 @@ public AHRS getGyro() {
return gyro;
}

/**
* Get status of field-oriented driving.
*
* @return status of field-oriented driving.
*/
public boolean isFieldOriented() {
return isFieldOriented;
}

/**
* Enable or disable field-oriented driving. Enabled by default if connected gyro is passed in via
* {@code SwerveDriveConfig} during construction.
*
* @param enabled true to enable field-oriented driving.
*/
public void setFieldOriented(boolean enabled) {
isFieldOriented = enabled;
logger.info("field orientation driving is {}", isFieldOriented ? "ENABLED" : "DISABLED");
}

/**
* Unit testing
*
Expand Down
271 changes: 152 additions & 119 deletions swerve/src/test/java/org/strykeforce/thirdcoast/swerve/SwerveDriveTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.Preferences;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Nested;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.api.extension.ExtendWith;
import org.junit.jupiter.params.ParameterizedTest;
Expand All @@ -19,140 +20,172 @@
@ExtendWith(MockitoExtension.class)
class SwerveDriveTest {

@Mock private Wheel wheel0, wheel1, wheel2, wheel3;
private Wheel[] wheels;
private SwerveDriveConfig config = new SwerveDriveConfig();
@Mock private AHRS gyro;

@BeforeEach
void setuUp() {
wheels = new Wheel[] {wheel0, wheel1, wheel2, wheel3};
config.wheels = wheels;
when(gyro.isConnected()).thenReturn(true);
config.gyro = gyro;
}
@Nested
class WhenFieldOrientedEnabled {
@Mock private Wheel wheel0, wheel1, wheel2, wheel3;
private Wheel[] wheels;
private SwerveDriveConfig config = new SwerveDriveConfig();
@Mock private AHRS gyro;

@BeforeEach
void setUp() {
wheels = new Wheel[] {wheel0, wheel1, wheel2, wheel3};
config.wheels = wheels;
when(gyro.isConnected()).thenReturn(true);
config.gyro = gyro;
}

@Test
void getPreferenceKeyForWheel() {
assertThat(SwerveDrive.getPreferenceKeyForWheel(3).equals("SwerveDrive/wheel.3"));
// avoid UnnecessaryStubbingException for gyro.isConnected()
SwerveDrive swerve = new SwerveDrive(config);
}
@Test
void getPreferenceKeyForWheel() {
assertThat(SwerveDrive.getPreferenceKeyForWheel(3).equals("SwerveDrive/wheel.3"));
// avoid UnnecessaryStubbingException for gyro.isConnected()
SwerveDrive swerve = new SwerveDrive(config);
}

@Test
void getWheels() {
SwerveDrive swerve = new SwerveDrive(config);
assertThat(swerve.getWheels()).isSameAs(wheels);
}
@Test
void getWheels() {
SwerveDrive swerve = new SwerveDrive(config);
assertThat(swerve.getWheels()).isSameAs(wheels);
}

@Test
void setDriveMode() {
SwerveDrive swerve = new SwerveDrive(config);
swerve.setDriveMode(TELEOP);
for (Wheel wheel : wheels) verify(wheel).setDriveMode(TELEOP);
}
@Test
void setDriveMode() {
SwerveDrive swerve = new SwerveDrive(config);
swerve.setDriveMode(TELEOP);
for (Wheel wheel : wheels) verify(wheel).setDriveMode(TELEOP);
}

@Test
void set() {
SwerveDrive swerve = new SwerveDrive(config);
swerve.set(0.27, 0.67);
for (Wheel wheel : wheels) verify(wheel).set(0.27, 0.67);
}
@Test
void set() {
SwerveDrive swerve = new SwerveDrive(config);
swerve.set(0.27, 0.67);
for (Wheel wheel : wheels) verify(wheel).set(0.27, 0.67);
}

@Test
void stop() {
SwerveDrive swerve = new SwerveDrive(config);
swerve.stop();
for (Wheel wheel : wheels) verify(wheel).stop();
}
@Test
void stop() {
SwerveDrive swerve = new SwerveDrive(config);
swerve.stop();
for (Wheel wheel : wheels) verify(wheel).stop();
}

@Test
void saveAzimuthPositions(@Mock Preferences prefs) {
SwerveDrive swerve = new SwerveDrive(config);
for (int i = 0; i < 4; i++) when(wheels[i].getAzimuthAbsolutePosition()).thenReturn(i);
swerve.saveAzimuthPositions(prefs);
for (int i = 0; i < 4; i++) {
String key = SwerveDrive.getPreferenceKeyForWheel(i);
verify(prefs).putInt(key, i);
@Test
void saveAzimuthPositions(@Mock Preferences prefs) {
SwerveDrive swerve = new SwerveDrive(config);
for (int i = 0; i < 4; i++) when(wheels[i].getAzimuthAbsolutePosition()).thenReturn(i);
swerve.saveAzimuthPositions(prefs);
for (int i = 0; i < 4; i++) {
String key = SwerveDrive.getPreferenceKeyForWheel(i);
verify(prefs).putInt(key, i);
}
}
}

@Test
void zeroAzimuthEncoders(@Mock Preferences prefs) {
SwerveDrive swerve = new SwerveDrive(config);
for (int i = 0; i < 4; i++) {
String key = SwerveDrive.getPreferenceKeyForWheel(i);
doReturn(i).when(prefs).getInt(key, SwerveDrive.DEFAULT_ABSOLUTE_AZIMUTH_OFFSET);
@Test
void zeroAzimuthEncoders(@Mock Preferences prefs) {
SwerveDrive swerve = new SwerveDrive(config);
for (int i = 0; i < 4; i++) {
String key = SwerveDrive.getPreferenceKeyForWheel(i);
doReturn(i).when(prefs).getInt(key, SwerveDrive.DEFAULT_ABSOLUTE_AZIMUTH_OFFSET);
}
swerve.zeroAzimuthEncoders(prefs);

for (int i = 0; i < 4; i++) {
verify(wheels[i]).setAzimuthZero(i);
}
}
swerve.zeroAzimuthEncoders(prefs);

for (int i = 0; i < 4; i++) {
verify(wheels[i]).setAzimuthZero(i);
@Test
void getGyro() {
SwerveDrive swerve = new SwerveDrive(config);
assertThat(swerve.getGyro()).isNotNull();
assertThat(swerve.getGyro()).isSameAs(gyro);
}
}

@Test
void getGyro() {
SwerveDrive swerve = new SwerveDrive(config);
assertThat(swerve.getGyro()).isSameAs(gyro);
}
@Test
void enableFieldOriented() {
SwerveDrive swerve = new SwerveDrive(config);
assertThat(swerve.isFieldOriented()).isTrue();
}

@Test
void getLengthComponent() {
config.length = 1;
config.width = 2;
SwerveDrive swerve = new SwerveDrive(config);
assertThat(swerve.getLengthComponent()).isEqualTo(1 / Math.sqrt(5));
}
@Test
void getLengthComponent() {
config.length = 1;
config.width = 2;
SwerveDrive swerve = new SwerveDrive(config);
assertThat(swerve.getLengthComponent()).isEqualTo(1 / Math.sqrt(5));
}

@Test
void getWidthComponent() {
config.length = 1;
config.width = 2;
SwerveDrive swerve = new SwerveDrive(config);
assertThat(swerve.getWidthComponent()).isEqualTo(2 / Math.sqrt(5));
}

@Test
void getWidthComponent() {
config.length = 1;
config.width = 2;
SwerveDrive swerve = new SwerveDrive(config);
assertThat(swerve.getWidthComponent()).isEqualTo(2 / Math.sqrt(5));
@ParameterizedTest
@CsvFileSource(resources = "/swervedrive_drive_cases.csv", numLinesToSkip = 1)
void drive(
double forward,
double strafe,
double yaw,
double wheel0Azimuth,
double wheel0Drive,
double wheel1Azimuth,
double wheel1Drive,
double wheel2Azimuth,
double wheel2Drive,
double wheel3Azimuth,
double wheel3Drive) {
SwerveDrive swerve = new SwerveDrive(config);

ArgumentCaptor<Double> wheel0AzimuthArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel0DriveArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel1AzimuthArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel1DriveArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel2AzimuthArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel2DriveArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel3AzimuthArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel3DriveArg = ArgumentCaptor.forClass(Double.class);

swerve.drive(forward, strafe, yaw);

verify(wheel0).set(wheel0AzimuthArg.capture(), wheel0DriveArg.capture());
verify(wheel1).set(wheel1AzimuthArg.capture(), wheel1DriveArg.capture());
verify(wheel2).set(wheel2AzimuthArg.capture(), wheel2DriveArg.capture());
verify(wheel3).set(wheel3AzimuthArg.capture(), wheel3DriveArg.capture());

double tol = 1.5e-4;
assertThat(wheel0AzimuthArg.getValue()).isCloseTo(wheel0Azimuth, byLessThan(tol));
assertThat(wheel0DriveArg.getValue()).isCloseTo(wheel0Drive, byLessThan(tol));
assertThat(wheel1AzimuthArg.getValue()).isCloseTo(wheel1Azimuth, byLessThan(tol));
assertThat(wheel1DriveArg.getValue()).isCloseTo(wheel1Drive, byLessThan(tol));
assertThat(wheel2AzimuthArg.getValue()).isCloseTo(wheel2Azimuth, byLessThan(tol));
assertThat(wheel2DriveArg.getValue()).isCloseTo(wheel2Drive, byLessThan(tol));
assertThat(wheel3AzimuthArg.getValue()).isCloseTo(wheel3Azimuth, byLessThan(tol));
assertThat(wheel3DriveArg.getValue()).isCloseTo(wheel3Drive, byLessThan(tol));
}
}

@ParameterizedTest
@CsvFileSource(resources = "/swervedrive_drive_cases.csv", numLinesToSkip = 1)
void drive(
double forward,
double strafe,
double yaw,
double wheel0Azimuth,
double wheel0Drive,
double wheel1Azimuth,
double wheel1Drive,
double wheel2Azimuth,
double wheel2Drive,
double wheel3Azimuth,
double wheel3Drive) {
SwerveDrive swerve = new SwerveDrive(config);

ArgumentCaptor<Double> wheel0AzimuthArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel0DriveArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel1AzimuthArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel1DriveArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel2AzimuthArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel2DriveArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel3AzimuthArg = ArgumentCaptor.forClass(Double.class);
ArgumentCaptor<Double> wheel3DriveArg = ArgumentCaptor.forClass(Double.class);

swerve.drive(forward, strafe, yaw);

verify(wheel0).set(wheel0AzimuthArg.capture(), wheel0DriveArg.capture());
verify(wheel1).set(wheel1AzimuthArg.capture(), wheel1DriveArg.capture());
verify(wheel2).set(wheel2AzimuthArg.capture(), wheel2DriveArg.capture());
verify(wheel3).set(wheel3AzimuthArg.capture(), wheel3DriveArg.capture());

double tol = 1.5e-4;
assertThat(wheel0AzimuthArg.getValue()).isCloseTo(wheel0Azimuth, byLessThan(tol));
assertThat(wheel0DriveArg.getValue()).isCloseTo(wheel0Drive, byLessThan(tol));
assertThat(wheel1AzimuthArg.getValue()).isCloseTo(wheel1Azimuth, byLessThan(tol));
assertThat(wheel1DriveArg.getValue()).isCloseTo(wheel1Drive, byLessThan(tol));
assertThat(wheel2AzimuthArg.getValue()).isCloseTo(wheel2Azimuth, byLessThan(tol));
assertThat(wheel2DriveArg.getValue()).isCloseTo(wheel2Drive, byLessThan(tol));
assertThat(wheel3AzimuthArg.getValue()).isCloseTo(wheel3Azimuth, byLessThan(tol));
assertThat(wheel3DriveArg.getValue()).isCloseTo(wheel3Drive, byLessThan(tol));
@Nested
class WhenFieldOrientedDisabled {
@Test
void disableWithNullGyro() {
SwerveDrive swerve = new SwerveDrive(new SwerveDriveConfig());
assertThat(swerve.getGyro()).isNull();
assertThat(swerve.isFieldOriented()).isFalse();
}

@Test
void disableWithDisconnectedGyro() {
AHRS gyro = mock(AHRS.class);
when(gyro.isConnected()).thenReturn(false);
SwerveDriveConfig config = new SwerveDriveConfig();
config.gyro = gyro;

SwerveDrive swerve = new SwerveDrive(config);
assertThat(swerve.getGyro()).isNotNull();
assertThat(swerve.isFieldOriented()).isFalse();
}
}
}

0 comments on commit 0b150e7

Please sign in to comment.