-
Notifications
You must be signed in to change notification settings - Fork 3
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
migrate HYPERVision and VisionConnector into hyperLib
- Loading branch information
Showing
12 changed files
with
734 additions
and
179 deletions.
There are no files selected for viewing
102 changes: 102 additions & 0 deletions
102
src/main/java/org/hyperonline/hyperlib/HYPERVision.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,102 @@ | ||
package org.hyperonline.hyperlib; | ||
|
||
import edu.wpi.first.cameraserver.CameraServer; | ||
import edu.wpi.first.networktables.NetworkTableInstance; | ||
|
||
/** | ||
* HYPERVision provide code which is mostly the same in every vision application. | ||
* | ||
* @author Chris McGroarty | ||
*/ | ||
public abstract class HYPERVision { | ||
|
||
protected CameraServer m_cameraServer; | ||
protected NetworkTableInstance m_tableInstance; | ||
|
||
// override this property to true in a child class that runs on the robot | ||
protected boolean m_isRobot = false; | ||
|
||
/** | ||
* Initialize the HYPERVision application with components initialized in a specific order | ||
*/ | ||
public final void visionInit() { | ||
initNetworkTable(); | ||
initCameraServer(); | ||
initCameras(); | ||
initConnectors(); | ||
initProcessors(); | ||
initPipelines(); | ||
initModules(); | ||
} | ||
|
||
/** | ||
* Method runs throughout the application, allowing for | ||
* the scheduling of runnable tasks in the PeriodicScheduler | ||
*/ | ||
private void visionPeriodic() { | ||
PeriodicScheduler.getInstance().run(); | ||
} | ||
|
||
/** | ||
* Run the Vision application | ||
* Matches startCompetition of a Robot | ||
*/ | ||
public void startCompetition() { | ||
visionInit(); | ||
startModules(); | ||
|
||
// only run periodic when not running on the robot | ||
// robot has its own periodics | ||
while (!m_isRobot) { | ||
visionPeriodic(); | ||
} | ||
} | ||
|
||
/** | ||
* Initialize the NetworkTableInstance and start our team's client | ||
*/ | ||
protected void initNetworkTable() { | ||
m_tableInstance = NetworkTableInstance.getDefault(); | ||
m_tableInstance.startClientTeam(69); | ||
} | ||
|
||
/** | ||
* Initialize the cameras used in this HYPERVision | ||
*/ | ||
protected abstract void initCameras(); | ||
|
||
/** | ||
* Initialize the VisionConnectors used in this HYPERVision | ||
*/ | ||
protected abstract void initConnectors(); | ||
|
||
/** | ||
* Initialize the TargetProcessors used in this HYPERVision | ||
*/ | ||
protected abstract void initProcessors(); | ||
/** | ||
* Initialize the VIsionGUIPipelines used in this HYPERVision | ||
*/ | ||
protected abstract void initPipelines(); | ||
/** | ||
* Initialize the VisionModules used in this HYPERVision | ||
*/ | ||
protected abstract void initModules(); | ||
/** | ||
* Start the VisionModules used in this HYPERVision | ||
*/ | ||
protected abstract void startModules(); | ||
|
||
/** | ||
* Initialize the CameraServer used in this HYPERVision | ||
*/ | ||
protected void initCameraServer() { | ||
m_cameraServer = CameraServer.getInstance(); | ||
} | ||
|
||
/** | ||
* Update properties when Preferences have changed | ||
*/ | ||
protected abstract void onPreferencesUpdated(); | ||
} | ||
|
76 changes: 19 additions & 57 deletions
76
src/main/java/org/hyperonline/hyperlib/vision/AbstractTargetProcessor.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,80 +1,42 @@ | ||
package org.hyperonline.hyperlib.vision; | ||
|
||
import java.util.List; | ||
|
||
import org.hyperonline.hyperlib.pid.DisplacementPIDSource; | ||
import org.opencv.core.Rect; | ||
|
||
import edu.wpi.first.wpilibj.PIDSource; | ||
import java.util.List; | ||
|
||
/** | ||
* Base class with common functionality used by TargetProcessors. In particular, | ||
* this provides methods to store a result, and access it via PID sources. | ||
* | ||
* @author James Hagborg | ||
* | ||
* @param <T> | ||
* VisionResult type of this AbstractTargetProcessor | ||
* @param <T> VisionResult type of this AbstractTargetProcessor | ||
* @author James Hagborg | ||
*/ | ||
public abstract class AbstractTargetProcessor<T extends VisionResult> implements TargetProcessor { | ||
|
||
private volatile T m_lastResult; | ||
|
||
private AbstractVisionConnector<T> m_visionConnector; | ||
|
||
protected AbstractTargetProcessor(AbstractVisionConnector<T> connector) { | ||
m_visionConnector = connector; | ||
} | ||
|
||
/** | ||
* | ||
* @param targets | ||
* list of targets to compute | ||
* @param targets list of targets to compute | ||
* @return {T} | ||
*/ | ||
public abstract T computeResult(List<Rect> targets); | ||
|
||
/** | ||
* | ||
* @return {T} | ||
*/ | ||
public abstract T getDefaultValue(); | ||
|
||
@Override | ||
public final void process(List<Rect> targets) { | ||
m_lastResult = computeResult(targets); | ||
} | ||
|
||
/** | ||
* Get the most recent result of this processor. If no result has been | ||
* produced yet, returns the default value. | ||
* | ||
* @return The most recent result. | ||
*/ | ||
public T getLastResult() { | ||
T res = m_lastResult; | ||
return res == null ? getDefaultValue() : res; | ||
} | ||
|
||
/** | ||
* Get a PID source which tracks the x-coordinate of the target. | ||
* | ||
* @return A PID source tracking the x-coordinate of the target. | ||
*/ | ||
public PIDSource xPID() { | ||
return new DisplacementPIDSource() { | ||
@Override | ||
public double pidGet() { | ||
return getLastResult().xError(); | ||
} | ||
}; | ||
protected T getLastResult(){ | ||
return m_visionConnector.getLastResult(); | ||
} | ||
|
||
/** | ||
* Get a PID source which tracks the y-coordinate of the target. | ||
* | ||
* @return A PID source tracking the y-coordinate of the target. | ||
* | ||
* @param targets | ||
*/ | ||
public PIDSource yPID() { | ||
return new DisplacementPIDSource() { | ||
@Override | ||
public double pidGet() { | ||
return getLastResult().yError(); | ||
} | ||
}; | ||
@Override | ||
public final void process(List<Rect> targets) { | ||
T res = computeResult(targets); | ||
m_visionConnector.updateResult(res); | ||
} | ||
} | ||
|
164 changes: 164 additions & 0 deletions
164
src/main/java/org/hyperonline/hyperlib/vision/AbstractVisionConnector.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,164 @@ | ||
package org.hyperonline.hyperlib.vision; | ||
|
||
import edu.wpi.first.networktables.*; | ||
import edu.wpi.first.wpilibj.PIDSource; | ||
import org.hyperonline.hyperlib.pid.DisplacementPIDSource; | ||
|
||
import java.util.Objects; | ||
|
||
/** | ||
* Base class with common functionality used by VisionConnectors. In particular, | ||
* this provides methods to store, publish, and subscribe to a result, | ||
* and access it via PID sources. | ||
* | ||
* @param <T> VisionResult type of this AbstractVisionConnector | ||
* @author Chris McGroarty | ||
*/ | ||
public abstract class AbstractVisionConnector<T extends VisionResult> implements VisionConnector { | ||
|
||
protected final NetworkTableEntry m_xError; | ||
protected final NetworkTableEntry m_yError; | ||
protected final NetworkTableEntry m_xAbs; | ||
protected final NetworkTableEntry m_yAbs; | ||
protected final NetworkTableEntry m_foundTarget; | ||
protected final NetworkTableEntry m_lastResultTimestamp; | ||
protected T m_lastResult; | ||
protected NetworkTable m_table; | ||
private String m_mainTableName = "hypervision"; | ||
private String m_subTableName; | ||
private NetworkTableInstance m_inst; | ||
private int m_listenerID; | ||
|
||
protected AbstractVisionConnector(String subTableName, NetworkTableInstance inst) { | ||
m_subTableName = Objects.requireNonNull(subTableName); | ||
|
||
m_inst = Objects.requireNonNull(inst); | ||
m_table = m_inst.getTable(getTableName()); | ||
m_lastResult = getDefaultValue(); | ||
|
||
m_xError = m_table.getEntry("xError"); | ||
m_yError = m_table.getEntry("yError"); | ||
m_xAbs = m_table.getEntry("xAbsolute"); | ||
m_yAbs = m_table.getEntry("yAbsolute"); | ||
m_foundTarget = m_table.getEntry("foundTarget"); | ||
m_lastResultTimestamp = m_table.getEntry("lastResultTimestamp"); | ||
} | ||
|
||
private String getTableName() { | ||
return "/" + m_mainTableName + "/" + m_subTableName; | ||
} | ||
|
||
/** | ||
* add listener for the lastResultTimestamp entry in NetworkTables | ||
* process the next result on an update | ||
*/ | ||
public void subscribe() { | ||
m_listenerID = m_table.addEntryListener("lastResultTimestamp", this::next, EntryListenerFlags.kUpdate); | ||
} | ||
|
||
/** | ||
* remove listener for the lastResultTimestamp entry in NetworkTables | ||
*/ | ||
public void unsubscribe() { | ||
m_table.removeEntryListener(m_listenerID); | ||
} | ||
|
||
/** | ||
* @return {T} | ||
*/ | ||
public abstract T getDefaultValue(); | ||
|
||
/** | ||
* @param result {T} | ||
*/ | ||
public void updateResult(T result) { | ||
if (m_lastResult == null) { | ||
m_lastResult = getDefaultValue(); | ||
} | ||
if (!m_lastResult.equals(result)) { | ||
m_lastResult = (result == null ? getDefaultValue() : result); | ||
if (m_inst.isConnected()) { | ||
this.publish(); | ||
} | ||
} else { | ||
System.out.println("NetworkTableInstance(69) not connected"); | ||
} | ||
} | ||
|
||
/** | ||
* publish VisionResult values to corresponding NetworkTable Entries | ||
* should be overriden and supered by any children of type T extends VisionResult | ||
*/ | ||
public void publish() { | ||
m_lastResultTimestamp.setDouble(System.currentTimeMillis()); | ||
m_xError.setDouble(m_lastResult.xError()); | ||
m_yError.setDouble(m_lastResult.yError()); | ||
m_xAbs.setDouble(m_lastResult.xAbsolute()); | ||
m_yAbs.setDouble(m_lastResult.yAbsolute()); | ||
m_foundTarget.setBoolean(m_lastResult.foundTarget()); | ||
} | ||
|
||
/** | ||
* retrieve a VisionResult from NetworkTable Entries | ||
* should be overriden and supered by any children of type T extends VisionResult | ||
* | ||
* @return {VisionResult} | ||
*/ | ||
protected VisionResult retrieve() { | ||
return new VisionResult( | ||
m_xError.getDouble(0), | ||
m_yError.getDouble(0), | ||
m_xAbs.getDouble(0), | ||
m_yAbs.getDouble(0), | ||
m_foundTarget.getBoolean(false) | ||
); | ||
} | ||
|
||
/** | ||
* evaluate and process the results in NetworkTables to my lastResult | ||
* | ||
* @param table | ||
* @param key | ||
* @param entry | ||
* @param value | ||
* @param flags | ||
*/ | ||
protected abstract void next(NetworkTable table, String key, NetworkTableEntry entry, | ||
NetworkTableValue value, int flags); | ||
|
||
|
||
/** | ||
* @return {T} | ||
*/ | ||
public T getLastResult() { | ||
return m_lastResult; | ||
} | ||
|
||
/** | ||
* Get a PID source which tracks the x-coordinate of the target. | ||
* | ||
* @return A PID source tracking the x-coordinate of the target. | ||
*/ | ||
public PIDSource xPID() { | ||
return new DisplacementPIDSource() { | ||
@Override | ||
public double pidGet() { | ||
return getLastResult().xError(); | ||
} | ||
}; | ||
} | ||
|
||
/** | ||
* Get a PID source which tracks the y-coordinate of the target. | ||
* | ||
* @return A PID source tracking the y-coordinate of the target. | ||
*/ | ||
public PIDSource yPID() { | ||
return new DisplacementPIDSource() { | ||
@Override | ||
public double pidGet() { | ||
return getLastResult().yError(); | ||
} | ||
}; | ||
} | ||
} |
Oops, something went wrong.