001 /*----------------------------------------------------------------------------*/
002 /* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
003 /* Open Source Software - may be modified and shared by FRC teams. The code */
004 /* must be accompanied by the FIRST BSD license file in the root directory of */
005 /* the project. */
006 /*----------------------------------------------------------------------------*/
007 package edu.wpi.first.wpilibj;
008
009 import edu.wpi.first.wpilibj.communication.UsageReporting;
010 import edu.wpi.first.wpilibj.parsing.IInputOutput;
011 import java.util.Stack;
012
013 /**
014 * Pack data into the "user data" field that gets sent to the dashboard laptop
015 * via the driver station.
016 */
017 public class Dashboard implements IDashboard, IInputOutput {
018
019 protected class MemAccess {
020
021 byte[] m_bytes;
022
023 protected MemAccess(byte[] bytes) {
024 m_bytes = bytes;
025 }
026
027 protected MemAccess(int length) {
028 m_bytes = new byte[length];
029 }
030
031 public void setByte(int index, byte value) {
032 m_bytes[index] = value;
033 }
034
035 public void setShort(int index, short value) {
036 setByte(index++, (byte) (value >>> 8));
037 setByte(index++, (byte) (value));
038 }
039
040 public void setInt(int index, int value) {
041 setByte(index++, (byte) (value >>> 24));
042 setByte(index++, (byte) (value >>> 16));
043 setByte(index++, (byte) (value >>> 8));
044 setByte(index++, (byte) (value));
045 }
046
047 public void setFloat(int index, float value) {
048 setInt(index, Float.floatToIntBits(value));
049 }
050
051 public void setDouble(int index, double value) {
052 setInt(index, (int) (Double.doubleToLongBits(value) >>> 32));
053 setInt(index + 4, (int) Double.doubleToLongBits(value));
054 }
055
056 public void setString(int index, String value) {
057 setBytes(index, value.getBytes(), 0, value.length());
058 }
059
060 public void setBytes(int index, byte[] value, int offset, int number) {
061 for (int i = 0; i < number; i++) {
062 m_bytes[i + index] = value[i + offset];
063 }
064 }
065 }
066 private static final String kArray = "Array";
067 private static final String kCluster = "Cluster";
068 private static final Integer kByte = new Integer(0);
069 private static final Integer kShort = new Integer(1);
070 private static final Integer kInt = new Integer(2);
071 private static final Integer kFloat = new Integer(3);
072 private static final Integer kDouble = new Integer(4);
073 private static final Integer kString = new Integer(5);
074 private static final Integer kOther = new Integer(6);
075 private static final Integer kBoolean = new Integer(7);
076 private static final int kMaxDashboardDataSize = DriverStation.USER_STATUS_DATA_SIZE -
077 4 * 3 - 1; // 13 bytes needed for 3 size parameters and the sequence number
078 private static boolean m_reported = false;
079 protected MemAccess m_userStatus;
080 protected int m_userStatusSize = 0;
081 private MemAccess m_localBuffer;
082 private int m_packPtr;
083 private Stack m_expectedArrayElementType = new Stack();
084 private Stack m_arrayElementCount = new Stack();
085 private Stack m_arraySizePtr = new Stack();
086 private Stack m_complexTypeStack = new Stack();
087 private final Object m_statusDataSemaphore;
088
089 /**
090 * Dashboard constructor.
091 *
092 * This is only called once when the DriverStation constructor is called.
093 * @param statusDataSemaphore the object to synchronize on
094 */
095 protected Dashboard(Object statusDataSemaphore) {
096 m_userStatus = new MemAccess(kMaxDashboardDataSize);
097 m_localBuffer = new MemAccess(kMaxDashboardDataSize);
098 m_packPtr = 0;
099 m_statusDataSemaphore = statusDataSemaphore;
100 }
101
102 /**
103 * Pack a signed 8-bit int into the dashboard data structure.
104 * @param value Data to be packed into the structure.
105 * @return True on success
106 */
107 public boolean addByte(byte value) {
108 if (!validateAdd(1)) {
109 return false;
110 }
111 m_localBuffer.setByte(m_packPtr, value);
112 m_packPtr += 1;
113 return addedElement(kByte);
114 }
115
116 /**
117 * Pack a signed 16-bit int into the dashboard data structure.
118 * @param value Data to be packed into the structure.
119 * @return True on success
120 */
121 public boolean addShort(short value) {
122 if (!validateAdd(2)) {
123 return false;
124 }
125 m_localBuffer.setShort(m_packPtr, value);
126 m_packPtr += 2;
127 return addedElement(kShort);
128 }
129
130 /**
131 * Pack a signed 32-bit int into the dashboard data structure.
132 * @param value Data to be packed into the structure.
133 * @return True on success
134 */
135 public boolean addInt(int value) {
136 if (!validateAdd(4)) {
137 return false;
138 }
139 m_localBuffer.setInt(m_packPtr, value);
140 m_packPtr += 4;
141 return addedElement(kInt);
142 }
143
144 /**
145 * Pack a 32-bit floating point number into the dashboard data structure.
146 * @param value Data to be packed into the structure.
147 * @return True on success
148 */
149 public boolean addFloat(float value) {
150 if (!validateAdd(4)) {
151 return false;
152 }
153 m_localBuffer.setFloat(m_packPtr, value);
154 m_packPtr += 4;
155 return addedElement(kFloat);
156 }
157
158 /**
159 * Pack a 64-bit floating point number into the dashboard data structure.
160 * @param value Data to be packed into the structure.
161 * @return True on success
162 */
163 public boolean addDouble(double value) {
164 if (!validateAdd(8)) {
165 return false;
166 }
167 m_localBuffer.setDouble(m_packPtr, value);
168 m_packPtr += 8;
169 return addedElement(kDouble);
170 }
171
172 /**
173 * Pack a boolean into the dashboard data structure.
174 * @param value Data to be packed into the structure.
175 * @return True on success
176 */
177 public boolean addBoolean(boolean value) {
178 if (!validateAdd(1)) {
179 return false;
180 }
181 m_localBuffer.setByte(m_packPtr, (byte) (value ? 1 : 0));
182 m_packPtr += 1;
183 return addedElement(kBoolean);
184 }
185
186 /**
187 * Pack a NULL-terminated string of 8-bit characters into the dashboard data structure.
188 * @param value Data to be packed into the structure.
189 * @return True on success
190 */
191 public boolean addString(String value) {
192 if (!validateAdd(value.length() + 4)) {
193 return false;
194 }
195 m_localBuffer.setInt(m_packPtr, value.length());
196 m_packPtr += 4;
197 m_localBuffer.setString(m_packPtr, value);
198 m_packPtr += value.length();
199 return addedElement(kString);
200 }
201
202 /**
203 * Pack a string of 8-bit characters of specified length into the dashboard data structure.
204 * @param value Data to be packed into the structure.
205 * @param length The number of bytes in the string to pack.
206 * @return True on success
207 */
208 public boolean addString(String value, int length) {
209 return addString(value.substring(0, length));
210 }
211
212 /**
213 * Start an array in the packed dashboard data structure.
214 *
215 * After calling addArray(), call the appropriate Add method for each element of the array.
216 * Make sure you call the same add each time. An array must contain elements of the same type.
217 * You can use clusters inside of arrays to make each element of the array contain a structure of values.
218 * You can also nest arrays inside of other arrays.
219 * Every call to addArray() must have a matching call to finalizeArray().
220 * @return True on success
221 */
222 public boolean addArray() {
223 if (!validateAdd(4)) {
224 return false;
225 }
226 m_complexTypeStack.push(kArray);
227 m_arrayElementCount.push(new Integer(0));
228 m_arraySizePtr.push(new Integer(m_packPtr));
229 m_packPtr += 4;
230 return true;
231 }
232
233 /**
234 * Indicate the end of an array packed into the dashboard data structure.
235 *
236 * After packing data into the array, call finalizeArray().
237 * Every call to addArray() must have a matching call to finalizeArray().
238 * @return True on success
239 */
240 public boolean finalizeArray() {
241 if (m_complexTypeStack.peek() != kArray) {
242 System.err.println("Attempted to finalize an array in the middle of a cluster or without starting the array");
243 return false;
244 }
245 m_complexTypeStack.pop();
246 m_localBuffer.setInt(((Integer) m_arraySizePtr.pop()).intValue(),
247 ((Integer) m_arrayElementCount.peek()).intValue());
248
249
250 if (((Integer) m_arrayElementCount.peek()).intValue() != 0) {
251 m_expectedArrayElementType.pop();
252 }
253 m_arrayElementCount.pop();
254 return addedElement(kOther);
255 }
256
257 /**
258 * Start a cluster in the packed dashboard data structure.
259 *
260 * After calling addCluster(), call the appropriate Add method for each element of the cluster.
261 * You can use clusters inside of arrays to make each element of the array contain a structure of values.
262 * Every call to addCluster() must have a matching call to finalizeCluster().
263 * @return True on success
264 */
265 public boolean addCluster() {
266 m_complexTypeStack.push(kCluster);
267 return true;
268 }
269
270 /**
271 * Indicate the end of a cluster packed into the dashboard data structure.
272 *
273 * After packing data into the cluster, call finalizeCluster().
274 * Every call to addCluster() must have a matching call to finalizeCluster
275 * @return True on success
276 */
277 public boolean finalizeCluster() {
278 if (m_complexTypeStack.peek() != kCluster) {
279 System.err.println("Attempted to close a cluster on an open array or without starting the cluster");
280 return false;
281 }
282 m_complexTypeStack.pop();
283 return addedElement(kOther);
284 }
285
286 /**
287 * Indicate that the packing is complete and commit the buffer to the DriverStation.
288 *
289 * The packing of the dashboard packet is complete.
290 * If you are not using the packed dashboard data, you can call commit() to commit the Printf() buffer and the error string buffer.
291 * In effect, you are packing an empty structure.
292 * Prepares a packet to go to the dashboard...
293 * Pack the sequence number, Printf() buffer, the errors messages (not implemented yet), and packed dashboard data buffer.
294 * @return The total size of the data packed into the userData field of the status packet.
295 */
296 public synchronized int commit() {
297
298 if (!m_complexTypeStack.empty()) {
299 System.err.println("didn't finish complex type");
300 m_packPtr = 0;
301 System.err.println("didn't finish complex type");
302 return 0;
303 }
304
305 if(!m_reported){
306 UsageReporting.report(UsageReporting.kResourceType_Dashboard, 0);
307 m_reported = true;
308 }
309
310 synchronized (m_statusDataSemaphore) {
311 // Sequence number
312 DriverStation.getInstance().incrementUpdateNumber();
313
314 // Packed Dashboard Data
315 m_userStatusSize = m_packPtr;
316 m_userStatus.setBytes(0, m_localBuffer.m_bytes, 0, m_userStatusSize);
317 m_packPtr = 0;
318
319 }
320 return m_userStatusSize;
321 }
322
323 /**
324 * Validate that the data being packed will fit in the buffer.
325 */
326 private boolean validateAdd(int size) {
327 if (m_packPtr + size > kMaxDashboardDataSize) {
328 m_packPtr = 0;
329 System.err.println("Dashboard data is too long to send");
330 return false;
331 }
332 return true;
333 }
334
335 /**
336 * Check for consistent types when adding elements to an array and keep track of the number of elements in the array.
337 */
338 private boolean addedElement(Integer type) {
339 if (isArrayRoot()) {
340 if (((Integer) m_arrayElementCount.peek()).intValue() == 0) {
341 m_expectedArrayElementType.push(type);
342 } else {
343 if (type != m_expectedArrayElementType.peek()) {
344 System.err.println("Attempted to add multiple datatypes to the same array");
345 return false;
346 }
347 }
348 m_arrayElementCount.push(new Integer(((Integer) m_arrayElementCount.pop()).intValue() + 1));
349 }
350 return true;
351 }
352
353 /**
354 * If the top of the type stack an array?
355 */
356 private boolean isArrayRoot() {
357 return !m_complexTypeStack.empty() && m_complexTypeStack.peek() == kArray;
358 }
359
360 public byte[] getBytes() {
361 return m_userStatus.m_bytes;
362 }
363
364 public int getBytesLength() {
365 return m_userStatusSize;
366 }
367
368 public void flush() {
369 }
370 }