Skip to content

Commit 5ff3318

Browse files
CoolSpy3CoolSpy3
committed
update version
2 parents d7e5f8d + eb14e89 commit 5ff3318

27 files changed

Lines changed: 332 additions & 41 deletions

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -94,7 +94,7 @@ publishing {
9494
gpr(MavenPublication) {
9595
groupId = 'org.carlmontrobotics'
9696
artifactId = 'lib199'
97-
version = '1.0.1'
97+
version = '1.0.2'
9898

9999
from components.java
100100
}

‎src/main/java/org/carlmontrobotics/lib199/DummySparkMaxAnswer.java‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,6 @@
99
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
1010
import com.revrobotics.SparkMaxPIDController.AccelStrategy;
1111

12-
import org.mockito.Mockito;
1312
import org.mockito.invocation.InvocationOnMock;
1413

1514
public class DummySparkMaxAnswer extends REVLibErrorAnswer {
@@ -18,12 +17,12 @@ public class DummySparkMaxAnswer extends REVLibErrorAnswer {
1817

1918
public static final DummySparkMaxAnswer ANSWER = new DummySparkMaxAnswer();
2019

21-
public static final CANSparkMax DUMMY_SPARK_MAX = Mockito.mock(CANSparkMax.class, ANSWER);
20+
public static final CANSparkMax DUMMY_SPARK_MAX = Mocks.mock(CANSparkMax.class, ANSWER);
2221

23-
public static final RelativeEncoder DUMMY_ENCODER = Mockito.mock(RelativeEncoder.class, REVLibErrorAnswer.ANSWER);
24-
public static final SparkMaxAnalogSensor DUMMY_ANALOG_SENSOR = Mockito.mock(SparkMaxAnalogSensor.class, REVLibErrorAnswer.ANSWER);
25-
public static final SparkMaxLimitSwitch DUMMY_LIMIT_SWITCH = Mockito.mock(SparkMaxLimitSwitch.class, REVLibErrorAnswer.ANSWER);
26-
public static final SparkMaxPIDController DUMMY_PID_CONTROLLER = Mockito.mock(SparkMaxPIDController.class, ANSWER);
22+
public static final RelativeEncoder DUMMY_ENCODER = Mocks.mock(RelativeEncoder.class, REVLibErrorAnswer.ANSWER);
23+
public static final SparkMaxAnalogSensor DUMMY_ANALOG_SENSOR = Mocks.mock(SparkMaxAnalogSensor.class, REVLibErrorAnswer.ANSWER);
24+
public static final SparkMaxLimitSwitch DUMMY_LIMIT_SWITCH = Mocks.mock(SparkMaxLimitSwitch.class, REVLibErrorAnswer.ANSWER);
25+
public static final SparkMaxPIDController DUMMY_PID_CONTROLLER = Mocks.mock(SparkMaxPIDController.class, ANSWER);
2726

2827
@Override
2928
public Object answer(InvocationOnMock invocation) throws Throwable {

‎src/main/java/org/carlmontrobotics/lib199/Limelight.java‎

Lines changed: 23 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,8 @@ public enum Mode {
4141
// Mounting angle is the angle of the limelight (angled up = +, angled down = -)
4242
private double mountingAngleDeg;
4343

44+
private TurnDirection idleTurnDirection = TurnDirection.CW;
45+
4446
private PIDController pidController;
4547
private boolean newPIDLoop = false;
4648
public Limelight(){
@@ -143,13 +145,13 @@ public double steeringAssist() {
143145
newPIDLoop = true;
144146
pidController.setSetpoint(Math.signum(prev_txDeg) * config.backlashSteeringOffsetDegs);
145147
}
148+
149+
adjustment = Math.copySign(Math.min(Math.abs(adjustment), config.maxSteeringAdjustment), txDeg);
146150
} else {
147151
newPIDLoop = false;
148152
pidController.reset();
149-
adjustment = Math.copySign(config.steeringFactor, prev_txDeg);
153+
adjustment = Math.copySign(config.steeringFactor, idleTurnDirection.sign);
150154
}
151-
152-
adjustment = Math.copySign(Math.min(Math.abs(adjustment), config.maxSteeringAdjustment), txDeg);
153155
putValue("adjustment", adjustment);
154156
return adjustment;
155157
}
@@ -172,6 +174,14 @@ public PIDController getPIDController() {
172174
public void putValue(String valueName, double value){
173175
SmartDashboard.putNumber("Limelight("+ config.ntName + "), (" + valueName + ")", value);
174176
}
177+
178+
public TurnDirection getIdleTurnDirection() {
179+
return idleTurnDirection;
180+
}
181+
182+
public void setIdleTurnDirection(TurnDirection direction) {
183+
idleTurnDirection = direction;
184+
}
175185

176186
public static class Config {
177187
/**
@@ -207,4 +217,14 @@ public static class Config {
207217
*/
208218
public double kP = 0.225;
209219
}
220+
221+
public static enum TurnDirection {
222+
CW(-1), CCW(+1);
223+
224+
public final int sign;
225+
226+
private TurnDirection(int sign) {
227+
this.sign = sign;
228+
}
229+
}
210230
}

‎src/main/java/org/carlmontrobotics/lib199/LinearActuator.java‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
package org.carlmontrobotics.lib199;
2+
3+
import edu.wpi.first.wpilibj.PWM;
4+
5+
public class LinearActuator extends PWM {
6+
7+
private final int minLength, maxLength;
8+
9+
public LinearActuator(int channel, int minLength, int maxLength) {
10+
super(channel);
11+
this.minLength = minLength;
12+
this.maxLength = maxLength;
13+
// Our linear actators accept a pulse between 1ms and 2ms
14+
setBounds(2, 0, 0, 0, 1);
15+
}
16+
17+
public void set(double value) {
18+
setPosition(value);
19+
}
20+
21+
public void setLength(double length) {
22+
// Get the length as a percentage of maxLength
23+
set((length - minLength) / (maxLength - minLength));
24+
}
25+
26+
public double getLength() {
27+
return get() * (maxLength - minLength) + minLength;
28+
}
29+
30+
public double get() {
31+
return getPosition();
32+
}
33+
34+
}

‎src/main/java/org/carlmontrobotics/lib199/LinearInterpolation.java‎

Lines changed: 18 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -3,23 +3,27 @@
33
import java.io.FileNotFoundException;
44
import java.io.FileReader;
55
import java.io.IOException;
6+
import java.nio.file.Paths;
67
import java.util.Arrays;
8+
import java.util.HashMap;
79
import java.util.List;
810

911
import org.apache.commons.csv.CSVFormat;
1012
import org.apache.commons.csv.CSVParser;
1113
import org.apache.commons.csv.CSVRecord;
1214

15+
import edu.wpi.first.wpilibj.Filesystem;
16+
1317
public class LinearInterpolation {
1418
public double[] xs, ys;
1519
public double[] slopes, intercepts;
1620
public double minX, maxX;
1721
public int numPoints = 0;
1822

19-
// Performs linear interpolation for a strictly monotonically increasing function.
23+
// Performs linear interpolation (Pass a filename relative to the src/main/deploy directory File extensions are not assumed)
2024
public LinearInterpolation(String filename) {
2125
try {
22-
CSVParser csvParser = CSVFormat.DEFAULT.parse(new FileReader(filename));
26+
CSVParser csvParser = CSVFormat.DEFAULT.parse(new FileReader(Filesystem.getDeployDirectory().toPath().resolve(Paths.get(filename)).toFile()));
2327
List<CSVRecord> records = csvParser.getRecords();
2428
numPoints = records.size() - 1; // Subtract 1 because of the labels.
2529
// Set the size of the arrays
@@ -36,10 +40,9 @@ public LinearInterpolation(String filename) {
3640
}
3741
}
3842
csvParser.close();
39-
Arrays.sort(xs);
43+
sortData();
4044
minX = xs[0];
4145
maxX = xs[xs.length - 1];
42-
Arrays.sort(ys);
4346
for (int i = 1; i < numPoints; i++) {
4447
// Linear interpolation (y = mx + b)
4548
slopes[i - 1] = (ys[i] - ys[i - 1]) / (xs[i] - xs[i - 1]);
@@ -68,4 +71,15 @@ public double calculate(double x) {
6871
return 0.0;
6972
}
7073
}
74+
75+
private void sortData() {
76+
HashMap<Double, Double> points = new HashMap<>();
77+
for(int i = 0; i < numPoints; i++) {
78+
points.put(xs[i], ys[i]);
79+
}
80+
Arrays.sort(xs);
81+
for(int i = 0; i < xs.length; i++) {
82+
ys[i] = points.get(xs[i]);
83+
}
84+
}
7185
}

‎src/main/java/org/carlmontrobotics/lib199/Mocks.java‎

Lines changed: 75 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,44 @@
11
package org.carlmontrobotics.lib199;
22

3+
import java.lang.ref.WeakReference;
34
import java.lang.reflect.InvocationTargetException;
45
import java.lang.reflect.Method;
56
import java.lang.reflect.Modifier;
67
import java.util.ArrayList;
78
import java.util.Arrays;
9+
import java.util.Collections;
810
import java.util.HashMap;
11+
import java.util.List;
12+
import java.util.function.Consumer;
13+
import java.util.function.Predicate;
914
import java.util.stream.Collectors;
10-
import org.mockito.MockSettings;
1115

16+
import org.mockito.MockSettings;
1217
import org.mockito.Mockito;
1318
import org.mockito.internal.stubbing.defaultanswers.ReturnsSmartNulls;
1419
import org.mockito.invocation.InvocationOnMock;
1520
import org.mockito.stubbing.Answer;
1621

1722
public final class Mocks {
23+
24+
private static final List<WeakReference<Object>> MOCKS = Collections.synchronizedList(new ArrayList<>());
25+
private static final Predicate<WeakReference<?>> IS_REFERENCE_CLEARED = reference -> reference.get() == null;
26+
private static final Consumer<WeakReference<Object>> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK = reference -> Mockito.clearInvocations(reference.get());
27+
private static final Predicate<WeakReference<Object>> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERNCE_NOT_CLEARED = reference -> {
28+
if(IS_REFERENCE_CLEARED.test(reference)) return true;
29+
CLEAR_INVOCATIONS_ON_REFERENCED_MOCK.accept(reference);
30+
return false;
31+
};
32+
33+
static {
34+
// Use a single predicate so that clearing references and invocations is an atomic operation
35+
// Otherwise, we could (rarely) run into:
36+
// 1) Mock is added
37+
// 2) Garbage collected references are removed
38+
// 3) Mock is garbage collected
39+
// 4) Mock invocations are cleared -> throws NullPointerException
40+
Lib199Subsystem.registerPeriodic(() -> MOCKS.removeIf(CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERNCE_NOT_CLEARED));
41+
}
1842

1943
/**
2044
* Attempts to create an instance of a class in which some or all of the classes methods are replaced with a mocked implementation
@@ -77,7 +101,7 @@ public static <T, U> T createMock(Class<T> classToMock, U implClass, Answer<Obje
77101
settings = Mockito.withSettings().extraInterfaces(interfaces);
78102
}
79103
settings = settings.defaultAnswer(new MockAnswer<>(methods, implClass, defaultAnswer));
80-
T mock = Mockito.mock(classToMock, settings);
104+
T mock = mock(classToMock, settings);
81105
return mock;
82106
}
83107

@@ -88,6 +112,55 @@ public static Method[] listMethods(Class<?> base, Class<?>... interfaces) {
88112
return out.toArray(Method[]::new);
89113
}
90114

115+
/**
116+
* A wrapper for the underlying Mockito method which automatically calls {@link Mockito#clearInvocations(Object...)} to prevent memory leaks
117+
*
118+
* @see Mockito#mock(Class)
119+
*/
120+
public static <T> T mock(Class<T> classToMock) {
121+
return reportMock(Mockito.mock(classToMock));
122+
}
123+
124+
/**
125+
* A wrapper for the underlying Mockito method which automatically calls {@link Mockito#clearInvocations(Object...)} to prevent memory leaks
126+
*
127+
* @see Mockito#mock(Class, String)
128+
*/
129+
public static <T> T mock(Class<T> classToMock, String name) {
130+
return reportMock(Mockito.mock(classToMock, name));
131+
}
132+
133+
/**
134+
* A wrapper for the underlying Mockito method which automatically calls {@link Mockito#clearInvocations(Object...)} to prevent memory leaks
135+
*
136+
* @see Mockito#mock(Class, Answer)
137+
*/
138+
public static <T> T mock(Class<T> classToMock, Answer<?> defaultAnswer) {
139+
return reportMock(Mockito.mock(classToMock, defaultAnswer));
140+
}
141+
142+
/**
143+
* A wrapper for the underlying Mockito method which automatically calls {@link Mockito#clearInvocations(Object...)} to prevent memory leaks
144+
*
145+
* @see Mockito#mock(Class, MockSettings)
146+
*/
147+
public static <T> T mock(Class<T> classToMock, MockSettings mockSettings) {
148+
return reportMock(Mockito.mock(classToMock, mockSettings));
149+
}
150+
151+
/**
152+
* Registers a Mockito mock and periodically calls {@link Mockito#clearInvocations(Object...)} on it to prevent memory leaks
153+
*
154+
* @param <T> The type of the mock
155+
* @param t The mock
156+
* @return The mock
157+
*/
158+
public static <T> T reportMock(T t) {
159+
// Wrap in a WeakReference to prevent memory leaks on objects with no more references
160+
if(Mockito.mockingDetails(t).isMock()) MOCKS.add(new WeakReference<Object>(t));
161+
return t;
162+
}
163+
91164
private Mocks() {}
92165

93166
private static final class MockAnswer<U> implements Answer<Object> {

‎src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java‎

Lines changed: 48 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -10,17 +10,22 @@
1010
import com.ctre.phoenix.motorcontrol.NeutralMode;
1111
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
1212
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
13+
import com.ctre.phoenix.sensors.CANCoder;
1314
import com.revrobotics.CANSparkMax;
1415
import com.revrobotics.CANSparkMax.ExternalFollower;
1516
import com.revrobotics.CANSparkMax.IdleMode;
17+
import com.revrobotics.CANSparkMaxLowLevel;
18+
import com.revrobotics.SparkMaxPIDController;
1619

20+
import org.carlmontrobotics.lib199.MotorErrors.TemperatureLimit;
1721
import org.carlmontrobotics.lib199.sim.MockSparkMax;
1822
import org.carlmontrobotics.lib199.sim.MockTalonSRX;
1923
import org.carlmontrobotics.lib199.sim.MockVictorSPX;
24+
import org.carlmontrobotics.lib199.sim.MockedCANCoder;
2025

21-
import com.revrobotics.CANSparkMaxLowLevel;
22-
import com.revrobotics.SparkMaxPIDController;
23-
26+
import edu.wpi.first.cameraserver.CameraServer;
27+
import edu.wpi.first.cscore.UsbCamera;
28+
import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
2429
import edu.wpi.first.wpilibj.RobotBase;
2530

2631
/**
@@ -73,7 +78,11 @@ public static WPI_TalonSRX createTalon(int id) {
7378

7479
//checks for spark max errors
7580

76-
public static CANSparkMax createSparkMax(int id) {
81+
public static CANSparkMax createSparkMax(int id, TemperatureLimit temperatureLimit) {
82+
return createSparkMax(id, temperatureLimit.limit);
83+
}
84+
85+
public static CANSparkMax createSparkMax(int id, int temperatureLimit) {
7786
CANSparkMax spark;
7887
if (RobotBase.isReal()) {
7988
spark = new CachedSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless);
@@ -86,7 +95,7 @@ public static CANSparkMax createSparkMax(int id) {
8695
spark = MockSparkMax.createMockSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless);
8796
}
8897

89-
MotorErrors.reportSparkMaxTemp(spark);
98+
MotorErrors.reportSparkMaxTemp(spark, temperatureLimit);
9099

91100
MotorErrors.reportError(spark.restoreFactoryDefaults());
92101
MotorErrors.reportError(spark.follow(ExternalFollower.kFollowerDisabled, 0));
@@ -105,4 +114,38 @@ public static CANSparkMax createSparkMax(int id) {
105114

106115
return spark;
107116
}
117+
118+
public static CANCoder createCANCoder(int port) {
119+
CANCoder canCoder = new CANCoder(port);
120+
if(RobotBase.isSimulation()) new MockedCANCoder(canCoder);
121+
return canCoder;
122+
}
123+
124+
/**
125+
* Configures a USB Camera.
126+
* See {@link CameraServer#startAutomaticCapture} for more details.
127+
* This MUST be called AFTER AHRS initialization or the code will be unable to connect to the gyro.
128+
*
129+
* @return The configured camera
130+
*/
131+
public static UsbCamera configureCamera() {
132+
UsbCamera camera = CameraServer.startAutomaticCapture();
133+
camera.setConnectionStrategy(ConnectionStrategy.kKeepOpen);
134+
CameraServer.getServer().setSource(camera);
135+
return camera;
136+
}
137+
138+
/**
139+
* This method is equivilent to calling {@link #configureCamera()} {@link numCameras} times.
140+
* The last camera will be set as the primary Camera feed.
141+
* To change it, call {@code CameraServer.getServer().setSource()}.
142+
*
143+
* @param numCameras The number of cameras to configure
144+
* @return The configured cameras.
145+
*/
146+
public static UsbCamera[] configureCameras(int numCameras) {
147+
UsbCamera[] cameras = new UsbCamera[numCameras];
148+
for(int i = 0; i < numCameras; i++) cameras[i] = configureCamera();
149+
return cameras;
150+
}
108151
}

0 commit comments

Comments
 (0)