Skip to content

Commit 881c3ac

Browse files
viggy96viggy96
committed
Add hardware
1 parent 74c0093 commit 881c3ac

File tree

8 files changed

+319
-9
lines changed

8 files changed

+319
-9
lines changed

‎src/main/java/org/lasarobotics/hardware/Analog.java

+73
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
// Copyright (c) LASA Robotics and other contributors
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the MIT license file in the root directory of this project.
4+
5+
package org.lasarobotics.hardware;
6+
7+
import org.littletonrobotics.junction.AutoLog;
8+
import org.littletonrobotics.junction.Logger;
9+
10+
import edu.wpi.first.wpilibj.AnalogInput;
11+
12+
public class Analog implements LoggableHardware, AutoCloseable {
13+
/** Analog sensor ID */
14+
public static class ID {
15+
public final String name;
16+
public final int port;
17+
18+
/**
19+
* Analog sensor ID
20+
* @param name Device name for logging
21+
* @param port Port number
22+
*/
23+
public ID(String name, int port) {
24+
this.name = name;
25+
this.port = port;
26+
}
27+
}
28+
29+
@AutoLog
30+
public static class AnalogInputs {
31+
public double voltage;
32+
}
33+
34+
private AnalogInput m_analogInput;
35+
36+
private ID m_id;
37+
private AnalogInputsAutoLogged m_inputs;
38+
39+
public Analog(Analog.ID id) {
40+
this.m_id = id;
41+
this.m_analogInput = new AnalogInput(id.port);
42+
this.m_inputs = new AnalogInputsAutoLogged();
43+
}
44+
45+
/**
46+
* Update sensor input readings
47+
*/
48+
private void updateInputs() {
49+
m_inputs.voltage = m_analogInput.getVoltage();
50+
}
51+
52+
/**
53+
* Call this method periodically
54+
*/
55+
public void periodic() {
56+
updateInputs();
57+
Logger.getInstance().processInputs(m_id.name, m_inputs);
58+
}
59+
60+
/**
61+
* Get latest sensor input data
62+
* @return Latest sensor data
63+
*/
64+
public AnalogInputsAutoLogged getInputs() {
65+
return m_inputs;
66+
}
67+
68+
@Override
69+
public void close() {
70+
m_analogInput.close();
71+
}
72+
73+
}

‎src/main/java/org/lasarobotics/hardware/DMAService.java

+52
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
package org.lasarobotics.hardware;
2+
3+
import org.lasarobotics.utils.GlobalConstants;
4+
import org.littletonrobotics.junction.Logger;
5+
6+
import edu.wpi.first.wpilibj.DMA;
7+
import edu.wpi.first.wpilibj.DMASample;
8+
// Copyright (c) LASA Robotics and other contributors
9+
// Open Source Software; you can modify and/or share it under the terms of
10+
// the MIT license file in the root directory of this project.
11+
12+
import edu.wpi.first.wpilibj.DMASample.DMAReadStatus;
13+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
14+
import edu.wpi.first.wpilibj.Notifier;
15+
16+
public class DMAService extends SubsystemBase {
17+
private static DMAService m_dmaService;
18+
19+
20+
private static final double TIMEOUT = 1e-3;
21+
private static final int QUEUE_DEPTH = 1024;
22+
private static final String READ_STATUS_ENTRY = "DMAService/ReadStatus";
23+
24+
DMA m_dma;
25+
DMASample m_dmaSample;
26+
DMAReadStatus m_readStatus;
27+
Notifier m_dmaThread;
28+
29+
private DMAService() {
30+
this.m_dma = new DMA();
31+
this.m_dmaSample = new DMASample();
32+
this.m_readStatus = DMAReadStatus.kOk;
33+
this.m_dmaThread = new Notifier(() -> run());
34+
35+
m_dma.setTimedTrigger(GlobalConstants.ROBOT_LOOP_PERIOD / 2);
36+
}
37+
38+
public static DMAService getInstance() {
39+
if (m_dmaService == null) m_dmaService = new DMAService();
40+
return m_dmaService;
41+
}
42+
43+
private void run() {
44+
m_readStatus = m_dmaSample.update(m_dma, TIMEOUT);
45+
Logger.getInstance().recordOutput(READ_STATUS_ENTRY, m_readStatus.name());
46+
}
47+
48+
public void start() {
49+
m_dma.start(QUEUE_DEPTH);
50+
m_dmaThread.startPeriodic(GlobalConstants.ROBOT_LOOP_PERIOD / 2);
51+
}
52+
}

‎src/main/java/org/lasarobotics/hardware/LimitSwitch.java

+92
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
// Copyright (c) LASA Robotics and other contributors
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the MIT license file in the root directory of this project.
4+
5+
package org.lasarobotics.hardware;
6+
7+
import org.littletonrobotics.junction.AutoLog;
8+
import org.littletonrobotics.junction.Logger;
9+
10+
import edu.wpi.first.wpilibj.DigitalInput;
11+
12+
public class LimitSwitch implements LoggableHardware, AutoCloseable {
13+
/** Limit switch ID */
14+
public static class ID {
15+
public final String name;
16+
public final int port;
17+
18+
/**
19+
* Limit switch ID
20+
* @param name Device name for logging
21+
* @param port Port number
22+
*/
23+
public ID(String name, int port) {
24+
this.name = name;
25+
this.port = port;
26+
}
27+
}
28+
29+
public enum SwitchPolarity {
30+
NORMALLY_OPEN {
31+
@Override
32+
public boolean getValue(boolean value) { return value; }
33+
},
34+
NORMALLY_CLOSED {
35+
@Override
36+
public boolean getValue(boolean value) { return !value; }
37+
};
38+
39+
public abstract boolean getValue(boolean value);
40+
}
41+
42+
@AutoLog
43+
public static class LimitSwitchInputs {
44+
public boolean value;
45+
}
46+
47+
private DigitalInput m_limitSwitch;
48+
49+
private ID m_id;
50+
private SwitchPolarity m_switchPolarity;
51+
private LimitSwitchInputsAutoLogged m_inputs;
52+
53+
/**
54+
* Create a limit switch object with built-in logging
55+
* @param id Limit switch ID
56+
* @param switchPolarity Switch polarity
57+
*/
58+
public LimitSwitch(LimitSwitch.ID id, SwitchPolarity switchPolarity) {
59+
this.m_id = id;
60+
this.m_switchPolarity = switchPolarity;
61+
this.m_limitSwitch = new DigitalInput(m_id.port);
62+
this.m_inputs = new LimitSwitchInputsAutoLogged();
63+
}
64+
65+
/**
66+
* Update sensor input readings
67+
*/
68+
private void updateInputs() {
69+
m_inputs.value = m_switchPolarity.getValue(m_limitSwitch.get());
70+
}
71+
72+
/**
73+
* Call this method periodically
74+
*/
75+
public void periodic() {
76+
updateInputs();
77+
Logger.getInstance().processInputs(m_id.name, m_inputs);
78+
}
79+
80+
/**
81+
* Get latest sensor input data
82+
* @return Latest sensor data
83+
*/
84+
public LimitSwitchInputsAutoLogged getInputs() {
85+
return m_inputs;
86+
}
87+
88+
@Override
89+
public void close() {
90+
m_limitSwitch.close();
91+
}
92+
}

‎src/main/java/org/lasarobotics/hardware/LoggableHardware.java

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
// Copyright (c) LASA Robotics and other contributors
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the MIT license file in the root directory of this project.
4+
5+
package org.lasarobotics.hardware;
6+
7+
import org.littletonrobotics.junction.inputs.LoggableInputs;
8+
9+
public interface LoggableHardware {
10+
public void periodic();
11+
public LoggableInputs getInputs();
12+
}

‎src/main/java/org/lasarobotics/hardware/NavX2.java

+2-2
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
1414
import edu.wpi.first.wpilibj.SPI;
1515

16-
public class NavX2 implements AutoCloseable {
16+
public class NavX2 implements LoggableHardware, AutoCloseable {
1717
/** NavX2 ID */
1818
public static class ID {
1919
public final String name;
@@ -155,7 +155,7 @@ public void periodic() {
155155
* Get latest sensor input data
156156
* @return Latest NavX data
157157
*/
158-
public NavX2Inputs getInputs() {
158+
public NavX2InputsAutoLogged getInputs() {
159159
return m_inputs;
160160
}
161161

‎src/main/java/org/lasarobotics/hardware/Servo.java

+81
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
// Copyright (c) LASA Robotics and other contributors
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the MIT license file in the root directory of this project.
4+
5+
package org.lasarobotics.hardware;
6+
7+
import org.littletonrobotics.junction.Logger;
8+
import org.littletonrobotics.junction.inputs.LoggableInputs;
9+
10+
public class Servo implements LoggableHardware {
11+
/** Servo ID */
12+
public static class ID {
13+
public final String name;
14+
public final int port;
15+
16+
/**
17+
* Servo ID
18+
* @param name Device name for logging
19+
* @param port Port number
20+
*/
21+
public ID(String name, int port) {
22+
this.name = name;
23+
this.port = port;
24+
}
25+
}
26+
27+
private static final String VALUE_LOG_ENTRY = "/OutputValue";
28+
29+
private edu.wpi.first.wpilibj.Servo m_servo;
30+
31+
private ID m_id;
32+
private double m_conversionFactor;
33+
34+
/**
35+
* Create a servo object with built-in logging
36+
* @param id Servo ID
37+
* @param conversionFactor Output conversion factor
38+
*/
39+
public Servo(Servo.ID id, double conversionFactor) {
40+
this.m_id = id;
41+
this.m_conversionFactor = conversionFactor;
42+
this.m_servo = new edu.wpi.first.wpilibj.Servo(m_id.port);
43+
}
44+
45+
/**
46+
* Create a servo object with built-in logging
47+
* @param id
48+
*/
49+
public Servo(Servo.ID id) {
50+
this(id, 1.0);
51+
}
52+
53+
private void logOutputs(double value) {
54+
Logger.getInstance().recordOutput(m_id.name + VALUE_LOG_ENTRY, value);
55+
}
56+
57+
@Override
58+
public void periodic() {}
59+
60+
@Override
61+
public LoggableInputs getInputs() {
62+
return null;
63+
}
64+
65+
/**
66+
* Set conversion factor for output
67+
* @param conversionFactor Conversion factor
68+
*/
69+
public void setConversionFactor(double conversionFactor) {
70+
m_conversionFactor = conversionFactor;
71+
}
72+
73+
/**
74+
* Set position of servo
75+
* @param value Position value
76+
*/
77+
public void setPosition(double value) {
78+
m_servo.set(value / m_conversionFactor);
79+
logOutputs(value);
80+
}
81+
}

‎src/main/java/org/lasarobotics/hardware/SparkMax.java

+2-2
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@
2727
import edu.wpi.first.math.trajectory.TrapezoidProfile;
2828
import edu.wpi.first.wpilibj.Timer;
2929

30-
public class SparkMax implements AutoCloseable {
30+
public class SparkMax implements LoggableHardware, AutoCloseable {
3131
/** Spark Max ID */
3232
public static class ID {
3333
public final String name;
@@ -247,7 +247,7 @@ public void periodic() {
247247
* Get latest sensor input data
248248
* @return Latest sensor data
249249
*/
250-
public SparkMaxInputs getInputs() {
250+
public SparkMaxInputsAutoLogged getInputs() {
251251
return m_inputs;
252252
}
253253

‎src/test/java/org/lasarobotics/drive/MAXSwerveModuleTest.java

+5-5
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919
import org.lasarobotics.drive.MAXSwerveModule.GearRatio;
2020
import org.lasarobotics.drive.MAXSwerveModule.ModuleLocation;
2121
import org.lasarobotics.hardware.SparkMax;
22-
import org.lasarobotics.hardware.SparkMax.SparkMaxInputs;
22+
import org.lasarobotics.hardware.SparkMaxInputsAutoLogged;
2323
import org.lasarobotics.utils.GlobalConstants;
2424
import org.mockito.AdditionalMatchers;
2525
import org.mockito.ArgumentMatchers;
@@ -121,7 +121,7 @@ public void moduleLocation() {
121121
@DisplayName("Test if module can set state")
122122
public void set() {
123123
// Hardcode sensor values
124-
SparkMaxInputs sparkMaxInputs = new SparkMaxInputs();
124+
SparkMaxInputsAutoLogged sparkMaxInputs = new SparkMaxInputsAutoLogged();
125125
when(m_lFrontRotateMotor.getInputs()).thenReturn(sparkMaxInputs);
126126
when(m_rFrontRotateMotor.getInputs()).thenReturn(sparkMaxInputs);
127127
when(m_lRearRotateMotor.getInputs()).thenReturn(sparkMaxInputs);
@@ -150,7 +150,7 @@ public void set() {
150150
@DisplayName("Test if module will auto-lock")
151151
public void autoLock() {
152152
// Hardcode sensor values
153-
SparkMaxInputs sparkMaxInputs = new SparkMaxInputs();
153+
SparkMaxInputsAutoLogged sparkMaxInputs = new SparkMaxInputsAutoLogged();
154154
when(m_lFrontRotateMotor.getInputs()).thenReturn(sparkMaxInputs);
155155
when(m_rFrontRotateMotor.getInputs()).thenReturn(sparkMaxInputs);
156156
when(m_lRearRotateMotor.getInputs()).thenReturn(sparkMaxInputs);
@@ -179,8 +179,8 @@ public void autoLock() {
179179
@DisplayName("Test if module works in simulation")
180180
public void simulation() {
181181
// Hardcode sensor values
182-
SparkMaxInputs defaultInputs = new SparkMaxInputs();
183-
SparkMaxInputs lFrontRotateMotorInputs = new SparkMaxInputs();
182+
SparkMaxInputsAutoLogged defaultInputs = new SparkMaxInputsAutoLogged();
183+
SparkMaxInputsAutoLogged lFrontRotateMotorInputs = new SparkMaxInputsAutoLogged();
184184
lFrontRotateMotorInputs.absoluteEncoderPosition = ModuleLocation.LeftFront.offset.getRadians();
185185
when(m_lFrontDriveMotor.getInputs()).thenReturn(defaultInputs);
186186
when(m_lFrontRotateMotor.getInputs()).thenReturn(lFrontRotateMotorInputs);

0 commit comments

Comments
 (0)