Skip to content

Commit 7870faf

Browse files
kennyseyboldkennyseybold
committed
fixed dio
1 parent 1387692 commit 7870faf

File tree

2 files changed

+68
-40
lines changed

2 files changed

+68
-40
lines changed

‎src/main/java/WallEye/WallEye.java‎

Lines changed: 65 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -33,10 +33,11 @@ public class WallEye {
3333
ArrayList<DigitalInput> dios = new ArrayList<DigitalInput>();
3434
DoubleSupplier gyro;
3535
int currentGyroIndex = 0;
36-
private final int maxGyroResultSize = 50;
36+
private final int maxGyroResultSize = 100;
3737
private final int camFPS = 50;
38-
private final double periodicLoop = 0.005;
38+
private final double periodicLoop = 0.001;
3939
DIOGyroResult[][] gyroResults;
40+
boolean[] hasTurnedOff;
4041
Notifier dioLoop = new Notifier(this::grabGyro);
4142
HashMap<Integer, Integer> dioHashMap = new HashMap<>();
4243

@@ -49,16 +50,17 @@ public class WallEye {
4950
* @param gyro a DoubleSupplier that supplies the gyro yaw for the robot
5051
* @param dioPorts an array that holds all the Ids for the dio ports that the cameras are connected to *EACH CAMERA MUST HAVE A DIO PORT* (to not use DIO yaw reporting put in an empty array)
5152
*/
52-
public WallEye(String tableName, int numCameras, DoubleSupplier gyro, int[] dioPorts)
53+
public WallEye(String tableName, int numCameras, DoubleSupplier gyro, DigitalInput[] dioPorts)
5354
{
5455
gyroResults = new DIOGyroResult[numCameras][maxGyroResultSize];
56+
hasTurnedOff = new boolean[numCameras];
5557
if (dioPorts.length > 0)
5658
dioLoop.startPeriodic(periodicLoop);
5759
this.gyro = gyro;
5860

5961
for(int i = 0; i < dioPorts.length; ++i) {
60-
dios.add(new DigitalInput(dioPorts[i]));
61-
dioHashMap.put(dioPorts[i], i);
62+
dios.add(dioPorts[i]);
63+
dioHashMap.put(dioPorts[i].getChannel(), i);
6264
}
6365

6466
camToCenter = new Transform3d[numCameras];
@@ -82,9 +84,15 @@ public WallEye(String tableName, int numCameras, DoubleSupplier gyro, int[] dioP
8284
private void grabGyro() {
8385
for (DigitalInput dio: dios)
8486
if (dio.get()) {
85-
gyroResults[dioHashMap.get(dio.getChannel())][currentGyroIndex] = new DIOGyroResult(gyro.getAsDouble(), RobotController.getFPGATime());
86-
currentGyroIndex++;
87-
currentGyroIndex %= maxGyroResultSize;
87+
if (!hasTurnedOff[dioHashMap.get(dio.getChannel())]) {
88+
gyroResults[dioHashMap.get(dio.getChannel())][currentGyroIndex] = new DIOGyroResult(gyro.getAsDouble(), RobotController.getFPGATime());
89+
currentGyroIndex++;
90+
currentGyroIndex %= maxGyroResultSize;
91+
hasTurnedOff[dioHashMap.get(dio.getChannel())] = true;
92+
} else {
93+
hasTurnedOff[dioHashMap.get(dio.getChannel())] = false;
94+
}
95+
8896
}
8997

9098
}
@@ -100,8 +108,8 @@ public int getNumCameras() {
100108
}
101109

102110
/**
103-
* Pulls most recent poses from Network Tables. FIXME : WILL HAVE TO SORT OUT NON CHANGED POSES
104-
*
111+
* Pulls most recent poses from Network Tables.
112+
* Array is structed as index 0, 1, 2 are position; 3, 4, 5 are rotation; 6 is a timestamp; 7 is n number of tags; 7 + n are tag ids; 7+n+1 is ambiguity
105113
*
106114
* @return Returns an array of WallEyeResult, with each nth result being the nth camera as shown on the web interface
107115
* @see WallEyeResult
@@ -122,47 +130,67 @@ public WallEyeResult[] getResults() {
122130
//(long)temp[6]
123131
if(dios.size() == 0 || gyroResults[i][maxGyroResultSize - 1] == null || temp[6] > gyroResults[i][currentGyroIndex - 1 >= 0 ? currentGyroIndex - 1 : maxGyroResultSize - 1].getTimestamp())
124132
results.add(new WallEyeResult(new Pose3d(new Translation3d(temp[0], temp[1], temp[2]), new Rotation3d(temp[3], temp[4], temp[5])),
125-
(long)temp[6] + sub.getAtomic().timestamp, i, curUpdateNum, (int) temp[7], tags, temp[8 + (int) temp[7]] ));
126-
else
127-
results.add(new WallEyeResult(new Pose3d(new Translation3d(temp[0], temp[1], temp[2]), findGyro((long)temp[6], i)),
128-
(long)temp[6] + sub.getAtomic().timestamp, i, curUpdateNum, (int) temp[7], tags, temp[8 + (int) temp[7]] ));
133+
(double)sub.getAtomic().timestamp + temp[6], i, curUpdateNum, (int) temp[7], tags, temp[8 + (int) temp[7]] ));
134+
else {
135+
DIOGyroResult savedStrobe = findGyro((sub.getAtomic().timestamp + (long)temp[6]), i, temp[5]);
136+
137+
//System.out.println(savedStrobe.getTimestamp() - ((double)sub.getAtomic().timestamp) + temp[6]);
129138

139+
results.add(new WallEyeResult(new Pose3d(new Translation3d(temp[0], temp[1], temp[2]), new Rotation3d(0, 0, savedStrobe.getGyro())),
140+
savedStrobe.getTimestamp(), i, curUpdateNum, (int) temp[7], tags, temp[8 + (int) temp[7]] ));
141+
}
130142
}
131143
WallEyeResult[] returnArray = new WallEyeResult[results.size()];
132144
returnArray = results.toArray(returnArray);
133145
return returnArray;
134146
}
135147

148+
136149
/**
137-
* A method that will find the yaw that has been saved off from the DIO port loop. It uses
138-
* a binary search on a circular buffer to find the yaw quickly
139-
*
150+
* A method that will go back until it gets a timestamp from before the network tables reports
151+
* the time
152+
*
140153
* @param timestamp a long that is the timestamp of the camera that is being searched for
141154
* @param camIndex an index that corresponds to the camera index
155+
* @param yaw a yaw that can be returned if it fails
142156
*
143-
* @return Returns a Rotation3d with the yaw of the saved gyro
157+
* @return Returns a DIOGyroResult with the data of the strobe result
144158
*/
145-
public Rotation3d findGyro(long timestamp, int camIndex) {
146-
int max = currentGyroIndex - 1 >= 0 ? currentGyroIndex - 1 : maxGyroResultSize - 1;
147-
int min = currentGyroIndex;
148-
int loops = 0;
149-
int mid = max > min ? (max - min) / 2 : (maxGyroResultSize - min + max) / 2 + min;
150-
mid %= maxGyroResultSize;
151-
152-
if (gyroResults[camIndex][maxGyroResultSize-1] == null)
153-
return new Rotation3d();
154-
155-
while ((timestamp - gyroResults[camIndex][mid].getTimestamp() > 1.0/camFPS * 1000000 || timestamp - gyroResults[camIndex][mid].getTimestamp() < 0)&& min != mid && loops < 10) {
156-
loops++;
157-
if (gyroResults[camIndex][mid].getTimestamp() > timestamp)
158-
max = mid;
159-
else
160-
min = mid;
161-
mid = max > min ? (max - min) / 2 + min: (maxGyroResultSize - min + max) / 2 + min;
162-
mid %= maxGyroResultSize;
159+
public DIOGyroResult findGyro(long timestamp, int camIndex, double yaw) {
160+
int index = currentGyroIndex - 1;
161+
if (index < 0)
162+
index += maxGyroResultSize;
163+
while((long)gyroResults[camIndex][index].getTimestamp() > timestamp) {
164+
//System.out.println(index);
165+
index--;
166+
if (index < 0)
167+
index += maxGyroResultSize;
168+
if (index == currentGyroIndex)
169+
return new DIOGyroResult(yaw, timestamp);
163170
}
164-
return new Rotation3d(0, 0, gyroResults[camIndex][mid].getGyro());
171+
return gyroResults[camIndex][index];
165172
}
173+
// public DIOGyroResult findGyro(long timestamp, int camIndex) {
174+
// int max = currentGyroIndex - 1 >= 0 ? currentGyroIndex - 1 : maxGyroResultSize - 1;
175+
// int min = currentGyroIndex;
176+
// int loops = 0;
177+
// int mid = max > min ? (max - min) / 2 : (maxGyroResultSize - min + max) / 2 + min;
178+
// mid %= maxGyroResultSize;
179+
180+
// if (gyroResults[camIndex][maxGyroResultSize-1] == null)
181+
// return new DIOGyroResult(0.0, 0);
182+
183+
// while ((timestamp - gyroResults[camIndex][mid].getTimestamp() > 1.0/camFPS * 1000000 || timestamp - gyroResults[camIndex][mid].getTimestamp() < 0)&& min != mid && loops < 10) {
184+
// loops++;
185+
// if (gyroResults[camIndex][mid].getTimestamp() > timestamp)
186+
// max = mid;
187+
// else
188+
// min = mid;
189+
// mid = max > min ? (max - min) / 2 + min: (maxGyroResultSize - min + max) / 2 + min;
190+
// mid %= maxGyroResultSize;
191+
// }
192+
// return gyroResults[camIndex][mid];
193+
// }
166194

167195
/**
168196
* Getter for the current update number

‎src/main/java/WallEye/WallEyeResult.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010
*/
1111
public class WallEyeResult {
1212
Pose3d cameraPose;
13-
long timeStamp;
13+
double timeStamp;
1414
int updateNum;
1515
int numTags;
1616
int originCam;
@@ -28,7 +28,7 @@ public class WallEyeResult {
2828
* @param tags an array of tag ids that was used to calculate pose
2929
* @param ambiguity a double that represents to confidence of the pose
3030
*/
31-
public WallEyeResult(Pose3d pose, long timeStamp, int originCam, int updateNum, int numTags, int[] tags, double ambiguity) {
31+
public WallEyeResult(Pose3d pose, double timeStamp, int originCam, int updateNum, int numTags, int[] tags, double ambiguity) {
3232
cameraPose = pose;
3333
this.timeStamp = timeStamp;
3434
this.updateNum = updateNum;
@@ -60,7 +60,7 @@ public Pose3d getCameraPose() {
6060
*
6161
* @return timestamp of the result
6262
*/
63-
public long getTimeStamp() {
63+
public double getTimeStamp() {
6464
return timeStamp;
6565
}
6666

0 commit comments

Comments
 (0)