Skip to content

Commit 04c9a62

Browse files
jhhjhh
authored
Merge pull request #56 from strykeforce/feature/disable-gyro
Add ability to disable field-oriented driving
2 parents 6453c9b + 0b150e7 commit 04c9a62

File tree

3 files changed

+180
-125
lines changed

3 files changed

+180
-125
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ buildscript {
1212

1313
configure(subprojects) {
1414
group = 'org.strykeforce.thirdcoast'
15-
version = '19.1.2'
15+
version = '19.2.0'
1616

1717
apply plugin: 'java-library'
1818
apply plugin: 'idea'

‎swerve/src/main/java/org/strykeforce/thirdcoast/swerve/SwerveDrive.java‎

Lines changed: 27 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -23,18 +23,18 @@ public class SwerveDrive {
2323
public static final int DEFAULT_ABSOLUTE_AZIMUTH_OFFSET = 200;
2424
private static final Logger logger = LoggerFactory.getLogger(SwerveDrive.class);
2525
private static final int WHEEL_COUNT = 4;
26-
final AHRS gyro;
26+
private final AHRS gyro;
2727
private final double kLengthComponent;
2828
private final double kWidthComponent;
2929
private final double kGyroRateCorrection;
3030
private final Wheel[] wheels;
3131
private final double[] ws = new double[WHEEL_COUNT];
3232
private final double[] wa = new double[WHEEL_COUNT];
33+
private boolean isFieldOriented;
3334

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

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

50-
if (gyro != null && gyro.isConnected()) {
50+
setFieldOriented(gyro != null && gyro.isConnected());
51+
52+
if (isFieldOriented) {
5153
gyro.enableLogging(config.gyroLoggingEnabled);
5254
double robotPeriod = config.robotPeriod;
5355
double gyroRateCoeff = config.gyroRateCoeff;
@@ -112,14 +114,14 @@ public void drive(double forward, double strafe, double azimuth) {
112114

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

120122
angle = Math.toRadians(angle);
121123
final double temp = forward * Math.cos(angle) + strafe * Math.sin(angle);
122-
strafe = -forward * Math.sin(angle) + strafe * Math.cos(angle);
124+
strafe = strafe * Math.cos(angle) - forward * Math.sin(angle);
123125
forward = temp;
124126
}
125127

@@ -228,6 +230,26 @@ public AHRS getGyro() {
228230
return gyro;
229231
}
230232

233+
/**
234+
* Get status of field-oriented driving.
235+
*
236+
* @return status of field-oriented driving.
237+
*/
238+
public boolean isFieldOriented() {
239+
return isFieldOriented;
240+
}
241+
242+
/**
243+
* Enable or disable field-oriented driving. Enabled by default if connected gyro is passed in via
244+
* {@code SwerveDriveConfig} during construction.
245+
*
246+
* @param enabled true to enable field-oriented driving.
247+
*/
248+
public void setFieldOriented(boolean enabled) {
249+
isFieldOriented = enabled;
250+
logger.info("field orientation driving is {}", isFieldOriented ? "ENABLED" : "DISABLED");
251+
}
252+
231253
/**
232254
* Unit testing
233255
*

‎swerve/src/test/java/org/strykeforce/thirdcoast/swerve/SwerveDriveTest.java‎

Lines changed: 152 additions & 119 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import com.kauailabs.navx.frc.AHRS;
99
import edu.wpi.first.wpilibj.Preferences;
1010
import org.junit.jupiter.api.BeforeEach;
11+
import org.junit.jupiter.api.Nested;
1112
import org.junit.jupiter.api.Test;
1213
import org.junit.jupiter.api.extension.ExtendWith;
1314
import org.junit.jupiter.params.ParameterizedTest;
@@ -19,140 +20,172 @@
1920
@ExtendWith(MockitoExtension.class)
2021
class SwerveDriveTest {
2122

22-
@Mock private Wheel wheel0, wheel1, wheel2, wheel3;
23-
private Wheel[] wheels;
24-
private SwerveDriveConfig config = new SwerveDriveConfig();
25-
@Mock private AHRS gyro;
26-
27-
@BeforeEach
28-
void setuUp() {
29-
wheels = new Wheel[] {wheel0, wheel1, wheel2, wheel3};
30-
config.wheels = wheels;
31-
when(gyro.isConnected()).thenReturn(true);
32-
config.gyro = gyro;
33-
}
23+
@Nested
24+
class WhenFieldOrientedEnabled {
25+
@Mock private Wheel wheel0, wheel1, wheel2, wheel3;
26+
private Wheel[] wheels;
27+
private SwerveDriveConfig config = new SwerveDriveConfig();
28+
@Mock private AHRS gyro;
29+
30+
@BeforeEach
31+
void setUp() {
32+
wheels = new Wheel[] {wheel0, wheel1, wheel2, wheel3};
33+
config.wheels = wheels;
34+
when(gyro.isConnected()).thenReturn(true);
35+
config.gyro = gyro;
36+
}
3437

35-
@Test
36-
void getPreferenceKeyForWheel() {
37-
assertThat(SwerveDrive.getPreferenceKeyForWheel(3).equals("SwerveDrive/wheel.3"));
38-
// avoid UnnecessaryStubbingException for gyro.isConnected()
39-
SwerveDrive swerve = new SwerveDrive(config);
40-
}
38+
@Test
39+
void getPreferenceKeyForWheel() {
40+
assertThat(SwerveDrive.getPreferenceKeyForWheel(3).equals("SwerveDrive/wheel.3"));
41+
// avoid UnnecessaryStubbingException for gyro.isConnected()
42+
SwerveDrive swerve = new SwerveDrive(config);
43+
}
4144

42-
@Test
43-
void getWheels() {
44-
SwerveDrive swerve = new SwerveDrive(config);
45-
assertThat(swerve.getWheels()).isSameAs(wheels);
46-
}
45+
@Test
46+
void getWheels() {
47+
SwerveDrive swerve = new SwerveDrive(config);
48+
assertThat(swerve.getWheels()).isSameAs(wheels);
49+
}
4750

48-
@Test
49-
void setDriveMode() {
50-
SwerveDrive swerve = new SwerveDrive(config);
51-
swerve.setDriveMode(TELEOP);
52-
for (Wheel wheel : wheels) verify(wheel).setDriveMode(TELEOP);
53-
}
51+
@Test
52+
void setDriveMode() {
53+
SwerveDrive swerve = new SwerveDrive(config);
54+
swerve.setDriveMode(TELEOP);
55+
for (Wheel wheel : wheels) verify(wheel).setDriveMode(TELEOP);
56+
}
5457

55-
@Test
56-
void set() {
57-
SwerveDrive swerve = new SwerveDrive(config);
58-
swerve.set(0.27, 0.67);
59-
for (Wheel wheel : wheels) verify(wheel).set(0.27, 0.67);
60-
}
58+
@Test
59+
void set() {
60+
SwerveDrive swerve = new SwerveDrive(config);
61+
swerve.set(0.27, 0.67);
62+
for (Wheel wheel : wheels) verify(wheel).set(0.27, 0.67);
63+
}
6164

62-
@Test
63-
void stop() {
64-
SwerveDrive swerve = new SwerveDrive(config);
65-
swerve.stop();
66-
for (Wheel wheel : wheels) verify(wheel).stop();
67-
}
65+
@Test
66+
void stop() {
67+
SwerveDrive swerve = new SwerveDrive(config);
68+
swerve.stop();
69+
for (Wheel wheel : wheels) verify(wheel).stop();
70+
}
6871

69-
@Test
70-
void saveAzimuthPositions(@Mock Preferences prefs) {
71-
SwerveDrive swerve = new SwerveDrive(config);
72-
for (int i = 0; i < 4; i++) when(wheels[i].getAzimuthAbsolutePosition()).thenReturn(i);
73-
swerve.saveAzimuthPositions(prefs);
74-
for (int i = 0; i < 4; i++) {
75-
String key = SwerveDrive.getPreferenceKeyForWheel(i);
76-
verify(prefs).putInt(key, i);
72+
@Test
73+
void saveAzimuthPositions(@Mock Preferences prefs) {
74+
SwerveDrive swerve = new SwerveDrive(config);
75+
for (int i = 0; i < 4; i++) when(wheels[i].getAzimuthAbsolutePosition()).thenReturn(i);
76+
swerve.saveAzimuthPositions(prefs);
77+
for (int i = 0; i < 4; i++) {
78+
String key = SwerveDrive.getPreferenceKeyForWheel(i);
79+
verify(prefs).putInt(key, i);
80+
}
7781
}
78-
}
7982

80-
@Test
81-
void zeroAzimuthEncoders(@Mock Preferences prefs) {
82-
SwerveDrive swerve = new SwerveDrive(config);
83-
for (int i = 0; i < 4; i++) {
84-
String key = SwerveDrive.getPreferenceKeyForWheel(i);
85-
doReturn(i).when(prefs).getInt(key, SwerveDrive.DEFAULT_ABSOLUTE_AZIMUTH_OFFSET);
83+
@Test
84+
void zeroAzimuthEncoders(@Mock Preferences prefs) {
85+
SwerveDrive swerve = new SwerveDrive(config);
86+
for (int i = 0; i < 4; i++) {
87+
String key = SwerveDrive.getPreferenceKeyForWheel(i);
88+
doReturn(i).when(prefs).getInt(key, SwerveDrive.DEFAULT_ABSOLUTE_AZIMUTH_OFFSET);
89+
}
90+
swerve.zeroAzimuthEncoders(prefs);
91+
92+
for (int i = 0; i < 4; i++) {
93+
verify(wheels[i]).setAzimuthZero(i);
94+
}
8695
}
87-
swerve.zeroAzimuthEncoders(prefs);
8896

89-
for (int i = 0; i < 4; i++) {
90-
verify(wheels[i]).setAzimuthZero(i);
97+
@Test
98+
void getGyro() {
99+
SwerveDrive swerve = new SwerveDrive(config);
100+
assertThat(swerve.getGyro()).isNotNull();
101+
assertThat(swerve.getGyro()).isSameAs(gyro);
91102
}
92-
}
93103

94-
@Test
95-
void getGyro() {
96-
SwerveDrive swerve = new SwerveDrive(config);
97-
assertThat(swerve.getGyro()).isSameAs(gyro);
98-
}
104+
@Test
105+
void enableFieldOriented() {
106+
SwerveDrive swerve = new SwerveDrive(config);
107+
assertThat(swerve.isFieldOriented()).isTrue();
108+
}
99109

100-
@Test
101-
void getLengthComponent() {
102-
config.length = 1;
103-
config.width = 2;
104-
SwerveDrive swerve = new SwerveDrive(config);
105-
assertThat(swerve.getLengthComponent()).isEqualTo(1 / Math.sqrt(5));
106-
}
110+
@Test
111+
void getLengthComponent() {
112+
config.length = 1;
113+
config.width = 2;
114+
SwerveDrive swerve = new SwerveDrive(config);
115+
assertThat(swerve.getLengthComponent()).isEqualTo(1 / Math.sqrt(5));
116+
}
117+
118+
@Test
119+
void getWidthComponent() {
120+
config.length = 1;
121+
config.width = 2;
122+
SwerveDrive swerve = new SwerveDrive(config);
123+
assertThat(swerve.getWidthComponent()).isEqualTo(2 / Math.sqrt(5));
124+
}
107125

108-
@Test
109-
void getWidthComponent() {
110-
config.length = 1;
111-
config.width = 2;
112-
SwerveDrive swerve = new SwerveDrive(config);
113-
assertThat(swerve.getWidthComponent()).isEqualTo(2 / Math.sqrt(5));
126+
@ParameterizedTest
127+
@CsvFileSource(resources = "/swervedrive_drive_cases.csv", numLinesToSkip = 1)
128+
void drive(
129+
double forward,
130+
double strafe,
131+
double yaw,
132+
double wheel0Azimuth,
133+
double wheel0Drive,
134+
double wheel1Azimuth,
135+
double wheel1Drive,
136+
double wheel2Azimuth,
137+
double wheel2Drive,
138+
double wheel3Azimuth,
139+
double wheel3Drive) {
140+
SwerveDrive swerve = new SwerveDrive(config);
141+
142+
ArgumentCaptor<Double> wheel0AzimuthArg = ArgumentCaptor.forClass(Double.class);
143+
ArgumentCaptor<Double> wheel0DriveArg = ArgumentCaptor.forClass(Double.class);
144+
ArgumentCaptor<Double> wheel1AzimuthArg = ArgumentCaptor.forClass(Double.class);
145+
ArgumentCaptor<Double> wheel1DriveArg = ArgumentCaptor.forClass(Double.class);
146+
ArgumentCaptor<Double> wheel2AzimuthArg = ArgumentCaptor.forClass(Double.class);
147+
ArgumentCaptor<Double> wheel2DriveArg = ArgumentCaptor.forClass(Double.class);
148+
ArgumentCaptor<Double> wheel3AzimuthArg = ArgumentCaptor.forClass(Double.class);
149+
ArgumentCaptor<Double> wheel3DriveArg = ArgumentCaptor.forClass(Double.class);
150+
151+
swerve.drive(forward, strafe, yaw);
152+
153+
verify(wheel0).set(wheel0AzimuthArg.capture(), wheel0DriveArg.capture());
154+
verify(wheel1).set(wheel1AzimuthArg.capture(), wheel1DriveArg.capture());
155+
verify(wheel2).set(wheel2AzimuthArg.capture(), wheel2DriveArg.capture());
156+
verify(wheel3).set(wheel3AzimuthArg.capture(), wheel3DriveArg.capture());
157+
158+
double tol = 1.5e-4;
159+
assertThat(wheel0AzimuthArg.getValue()).isCloseTo(wheel0Azimuth, byLessThan(tol));
160+
assertThat(wheel0DriveArg.getValue()).isCloseTo(wheel0Drive, byLessThan(tol));
161+
assertThat(wheel1AzimuthArg.getValue()).isCloseTo(wheel1Azimuth, byLessThan(tol));
162+
assertThat(wheel1DriveArg.getValue()).isCloseTo(wheel1Drive, byLessThan(tol));
163+
assertThat(wheel2AzimuthArg.getValue()).isCloseTo(wheel2Azimuth, byLessThan(tol));
164+
assertThat(wheel2DriveArg.getValue()).isCloseTo(wheel2Drive, byLessThan(tol));
165+
assertThat(wheel3AzimuthArg.getValue()).isCloseTo(wheel3Azimuth, byLessThan(tol));
166+
assertThat(wheel3DriveArg.getValue()).isCloseTo(wheel3Drive, byLessThan(tol));
167+
}
114168
}
115169

116-
@ParameterizedTest
117-
@CsvFileSource(resources = "/swervedrive_drive_cases.csv", numLinesToSkip = 1)
118-
void drive(
119-
double forward,
120-
double strafe,
121-
double yaw,
122-
double wheel0Azimuth,
123-
double wheel0Drive,
124-
double wheel1Azimuth,
125-
double wheel1Drive,
126-
double wheel2Azimuth,
127-
double wheel2Drive,
128-
double wheel3Azimuth,
129-
double wheel3Drive) {
130-
SwerveDrive swerve = new SwerveDrive(config);
131-
132-
ArgumentCaptor<Double> wheel0AzimuthArg = ArgumentCaptor.forClass(Double.class);
133-
ArgumentCaptor<Double> wheel0DriveArg = ArgumentCaptor.forClass(Double.class);
134-
ArgumentCaptor<Double> wheel1AzimuthArg = ArgumentCaptor.forClass(Double.class);
135-
ArgumentCaptor<Double> wheel1DriveArg = ArgumentCaptor.forClass(Double.class);
136-
ArgumentCaptor<Double> wheel2AzimuthArg = ArgumentCaptor.forClass(Double.class);
137-
ArgumentCaptor<Double> wheel2DriveArg = ArgumentCaptor.forClass(Double.class);
138-
ArgumentCaptor<Double> wheel3AzimuthArg = ArgumentCaptor.forClass(Double.class);
139-
ArgumentCaptor<Double> wheel3DriveArg = ArgumentCaptor.forClass(Double.class);
140-
141-
swerve.drive(forward, strafe, yaw);
142-
143-
verify(wheel0).set(wheel0AzimuthArg.capture(), wheel0DriveArg.capture());
144-
verify(wheel1).set(wheel1AzimuthArg.capture(), wheel1DriveArg.capture());
145-
verify(wheel2).set(wheel2AzimuthArg.capture(), wheel2DriveArg.capture());
146-
verify(wheel3).set(wheel3AzimuthArg.capture(), wheel3DriveArg.capture());
147-
148-
double tol = 1.5e-4;
149-
assertThat(wheel0AzimuthArg.getValue()).isCloseTo(wheel0Azimuth, byLessThan(tol));
150-
assertThat(wheel0DriveArg.getValue()).isCloseTo(wheel0Drive, byLessThan(tol));
151-
assertThat(wheel1AzimuthArg.getValue()).isCloseTo(wheel1Azimuth, byLessThan(tol));
152-
assertThat(wheel1DriveArg.getValue()).isCloseTo(wheel1Drive, byLessThan(tol));
153-
assertThat(wheel2AzimuthArg.getValue()).isCloseTo(wheel2Azimuth, byLessThan(tol));
154-
assertThat(wheel2DriveArg.getValue()).isCloseTo(wheel2Drive, byLessThan(tol));
155-
assertThat(wheel3AzimuthArg.getValue()).isCloseTo(wheel3Azimuth, byLessThan(tol));
156-
assertThat(wheel3DriveArg.getValue()).isCloseTo(wheel3Drive, byLessThan(tol));
170+
@Nested
171+
class WhenFieldOrientedDisabled {
172+
@Test
173+
void disableWithNullGyro() {
174+
SwerveDrive swerve = new SwerveDrive(new SwerveDriveConfig());
175+
assertThat(swerve.getGyro()).isNull();
176+
assertThat(swerve.isFieldOriented()).isFalse();
177+
}
178+
179+
@Test
180+
void disableWithDisconnectedGyro() {
181+
AHRS gyro = mock(AHRS.class);
182+
when(gyro.isConnected()).thenReturn(false);
183+
SwerveDriveConfig config = new SwerveDriveConfig();
184+
config.gyro = gyro;
185+
186+
SwerveDrive swerve = new SwerveDrive(config);
187+
assertThat(swerve.getGyro()).isNotNull();
188+
assertThat(swerve.isFieldOriented()).isFalse();
189+
}
157190
}
158191
}

0 commit comments

Comments
 (0)