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.communication;
008
009 import com.sun.cldc.jna.Function;
010 import com.sun.cldc.jna.BlockingFunction;
011 import com.sun.cldc.jna.Pointer;
012 import com.sun.cldc.jna.NativeLibrary;
013 import com.sun.cldc.jna.Structure;
014 import com.sun.cldc.jna.TaskExecutor;
015
016 /**
017 * Contains the code necessary to communicate between the robot and the driver station.
018 */
019 public final class FRCControl {
020
021 private static final TaskExecutor taskExecutor = new TaskExecutor("FRCControl Task executor");
022
023 // int getCommonControlData(FRCCommonControlData *data, int wait_ms);
024 private static final BlockingFunction getCommonControlDataFn = NativeLibrary.getDefaultInstance().getBlockingFunction("getCommonControlData");
025
026 // int getDynamicControlData(UINT8 type, char *dynamicData, INT32 maxLength, int wait_ms);
027 private static final BlockingFunction getDynamicControlDataFn = NativeLibrary.getDefaultInstance().getBlockingFunction("getDynamicControlData");
028
029 // int setStatusDataFloatAsInt(int battery, UINT8 dsDigitalOut, UINT8 updateNumber,
030 // const char *userDataHigh, int userDataHighLength,
031 // const char *userDataLow, int userDataLowLength, int wait_ms);
032 private static final BlockingFunction setStatusDataFn = NativeLibrary.getDefaultInstance().getBlockingFunction("setStatusDataFloatAsInt");
033
034 // int setErrorData(const char *errors, int errorsLength, int wait_ms);
035 private static final BlockingFunction setErrorDataFn = NativeLibrary.getDefaultInstance().getBlockingFunction("setErrorData");
036
037 // int setUserDsLcdData(const char *userDsLcdData, int userDsLcdDataLength, int wait_ms);
038 private static final BlockingFunction setUserDsLcdDataFn = NativeLibrary.getDefaultInstance().getBlockingFunction("setUserDsLcdData");
039
040 // int overrideIOConfig(const char *ioConfig, int wait_ms);
041 private static final BlockingFunction overrideIOConfigFn = NativeLibrary.getDefaultInstance().getBlockingFunction("overrideIOConfig");
042
043 // void setNewDataSem(SEM_ID);
044 private static final BlockingFunction setNewDataSemFn = NativeLibrary.getDefaultInstance().getBlockingFunction("setNewDataSem");
045
046 // void FRC_NetworkCommunication_observeUserProgramStarting(void);
047 private static final Function observeUserProgramStartingFn = NativeLibrary.getDefaultInstance().getFunction("FRC_NetworkCommunication_observeUserProgramStarting");
048
049 // void FRC_NetworkCommunication_observeUserProgramDisabled(void);
050 private static final Function observeUserProgramDisabledFn = NativeLibrary.getDefaultInstance().getFunction("FRC_NetworkCommunication_observeUserProgramDisabled");
051
052 // void FRC_NetworkCommunication_observeUserProgramAutonomous(void);
053 private static final Function observeUserProgramAutonomousFn = NativeLibrary.getDefaultInstance().getFunction("FRC_NetworkCommunication_observeUserProgramAutonomous");
054
055 // void FRC_NetworkCommunication_observeUserProgramTeleop(void);
056 private static final Function observeUserProgramTeleopFn = NativeLibrary.getDefaultInstance().getFunction("FRC_NetworkCommunication_observeUserProgramTeleop");
057
058 // void FRC_NetworkCommunication_observeUserProgramTeleop(void);
059 private static final Function observeUserProgramTestFn = NativeLibrary.getDefaultInstance().getFunction("FRC_NetworkCommunication_observeUserProgramTest");
060
061
062 static {
063 getCommonControlDataFn.setTaskExecutor(taskExecutor);
064 getDynamicControlDataFn.setTaskExecutor(taskExecutor);
065 setStatusDataFn.setTaskExecutor(taskExecutor);
066 setErrorDataFn.setTaskExecutor(taskExecutor);
067 setUserDsLcdDataFn.setTaskExecutor(taskExecutor);
068 setNewDataSemFn.setTaskExecutor(taskExecutor);
069 overrideIOConfigFn.setTaskExecutor(taskExecutor);
070 }
071
072 /**
073 * The size of the IO configuration data
074 */
075 public static final int IO_CONFIG_DATA_SIZE = 32;
076 /**
077 * The size of the user control data
078 */
079 public static final int USER_CONTROL_DATA_SIZE = 936 - IO_CONFIG_DATA_SIZE;
080 /**
081 * The size of the user status data
082 */
083 public static final int USER_STATUS_DATA_SIZE = 984;
084 /**
085 * The size of the user driver station display data
086 */
087 public static final int USER_DS_LCD_DATA_SIZE = 128;
088
089 private FRCControl() {
090 }
091
092 /**
093 * A simple 1-element cache that keeps a pointer to native memory around.
094 * Works best if repeatedly asked for buffers of same size.
095 *
096 * WARNING: It's expected that the users of this cache are synchronized
097 *
098 * @TODO free the cache at shutdown...
099 */
100 public static class CachedNativeBuffer {
101
102 private Pointer buffer;
103
104 public Pointer getBufferSized(int size) {
105 if (buffer == null) {
106 buffer = new Pointer(size);
107 }
108 if (size > buffer.getSize()) {
109 buffer.free();
110 buffer = new Pointer(size);
111 }
112 return buffer;
113 }
114
115 public void free() {
116 if (buffer != null) {
117 buffer.free();
118 buffer = null;
119 }
120 }
121 } /* CachedNativeBuffer */
122
123
124 public static abstract class DynamicControlData extends Structure {
125 }
126
127 private static final CachedNativeBuffer controlDataCache = new CachedNativeBuffer();
128 private static final CachedNativeBuffer statusDataCacheHigh = new CachedNativeBuffer();
129 private static final CachedNativeBuffer statusDataCacheLow = new CachedNativeBuffer();
130 private static final CachedNativeBuffer ioConfigDataCache = new CachedNativeBuffer();
131
132 /**
133 * Get the control data from the driver station. The parameter "data"
134 * is only updated when the method returns 0.
135 *
136 * @param data the object to store the results in (out param)
137 * @param wait_ms the maximum time to wait
138 * @return 0 if new data, 1 if no new data, 2 if access timed out.
139 */
140 public static int getCommonControlData(FRCCommonControlData data, int wait_ms) {
141 int res = getCommonControlDataFn.call2(data.getPointer(), wait_ms);
142 if (res == 0) {
143 // Copy the FRCControlData from C-accessible memory
144 data.read();
145 }
146
147 return res;
148 }
149
150 /**
151 * Get the dynamic control data from the driver station. The parameter
152 * "dynamicData" is only updated when the method returns 0.
153 * @param type The type to get.
154 * @param dynamicData The array to hold the result in.
155 * @param maxLength The maximum length of the data.
156 * @param wait_ms The maximum time to wait.
157 * @return 0 if new data, 1 if no new data, 2 if access timed out.
158 */
159 public static int getDynamicControlData(byte type, DynamicControlData dynamicData, int maxLength, int wait_ms) {
160 synchronized (controlDataCache) {
161 dynamicData.write();
162 int res = getDynamicControlDataFn.call4(type, dynamicData.getPointer(), maxLength, wait_ms);
163 if (res == 0)
164 dynamicData.read();
165 return res;
166 }
167 }
168
169 /**
170 * Right new io config data.
171 * @param ioConfig The data to write / read
172 * @param wait_ms The maximum time to wait.
173 * @return 0 if new data, 1 if no new data, 2 if access timed out.
174 */
175 public static int overrideIOConfig(DynamicControlData ioConfig, int wait_ms) {
176 synchronized (ioConfigDataCache) {
177 ioConfig.write();
178 int res = overrideIOConfigFn.call2(ioConfig.getPointer(), wait_ms);
179 if (res == 0)
180 ioConfig.read();
181 return res;
182 }
183 }
184
185 /**
186 * Set the status data to send to the ds
187 *
188 * @param battery the battery voltage
189 * @param dsDigitalOut value to set the digital outputs on the ds to
190 * @param updateNumber unique ID for this update (incrementing)
191 * @param userDataHigh additional high-priority user data bytes
192 * @param userDataHighLength number of high-priority data bytes
193 * @param userDataLow additional low-priority user data bytes
194 * @param userDataLowLength number of low-priority data bytes
195 * @param wait_ms the timeout
196 * @return 0 on success, 1 if userData.length is too big, 2 if semaphore could not be taken in wait_ms.
197 */
198 public static int setStatusData(double battery, int dsDigitalOut,
199 int updateNumber, byte[] userDataHigh, int userDataHighLength,
200 byte[] userDataLow, int userDataLowLength, int wait_ms) {
201 synchronized (statusDataCacheHigh) {
202 //System.out.println("udl " + userData.length);
203 // Copy the userdata byte[] to C-accessible memory
204 Pointer userDataPtrHigh = statusDataCacheHigh.getBufferSized(userDataHighLength); // new Pointer(userData.length);
205 userDataPtrHigh.setBytes(0, userDataHigh, 0, userDataHighLength);
206 Pointer userDataPtrLow = statusDataCacheLow.getBufferSized(userDataLowLength); // new Pointer(userData.length);
207 userDataPtrLow.setBytes(0, userDataLow, 0, userDataLowLength);
208
209 //System.out.print("Writing ");
210 //System.out.print(userData.length);
211 //System.out.println(" bytes of status data to network.");
212
213 int res = setStatusDataFn.call8(Float.floatToIntBits((float) battery),
214 dsDigitalOut, updateNumber,
215 userDataPtrHigh.address().toUWord().toPrimitive(), userDataHighLength,
216 userDataPtrLow.address().toUWord().toPrimitive(), userDataLowLength,
217 wait_ms);
218
219 //System.out.println("Data transfered.");
220 return res;
221 }
222 }
223
224 /**
225 * Send data to the driver station's error panel
226 * @param bytes the byte array containing the properly formatted information for the display
227 * @param length the length of the byte array
228 * @param timeOut the maximum time to wait
229 */
230 public static void setErrorData(byte[] bytes, int length, int timeOut) {
231 Pointer textPtr = new Pointer(bytes.length);
232 textPtr.setBytes(0, bytes, 0, bytes.length);
233 setErrorDataFn.call3(textPtr, length, timeOut);
234 textPtr.free();
235 }
236
237 /**
238 * Send data to the driver station's error panel
239 * @param textPtr pointer to C byte array containing the properly formatted information for the display
240 * @param length the length of the byte array
241 * @param timeOut the maximum time to wait
242 */
243 public static void setErrorData(Pointer textPtr, int length, int timeOut) {
244 if (length > textPtr.getSize()) {
245 throw new IllegalArgumentException();
246 }
247 setErrorDataFn.call3(textPtr, length, timeOut);
248 }
249
250 /**
251 * Send data to the driver station's user panel
252 * @param bytes the byte array containing the properly formatted information for the display
253 * @param length the length of the byte array
254 * @param timeOut the maximum time to wait
255 */
256 public static void setUserDsLcdData(byte[] bytes, int length, int timeOut) {
257 Pointer textPtr = new Pointer(bytes.length);
258 textPtr.setBytes(0, bytes, 0, bytes.length);
259 setUserDsLcdDataFn.call3(textPtr, length, timeOut);
260 textPtr.free();
261 }
262
263 /**
264 * Set the semaphore for the communications task to use
265 * @param sem the semaphore to use
266 */
267 public static void setNewDataSem(Semaphore sem) {
268 if (sem == null || sem.m_semaphore == null) {
269 throw new NullPointerException("Null provided for a semaphore");
270 }
271 setNewDataSemFn.call1(sem.m_semaphore);
272 }
273
274 /**
275 * Let the DS know that the user is loading a new app.
276 */
277 public static void observeUserProgramStarting() {
278 observeUserProgramStartingFn.call0();
279 }
280
281 public static void observeUserProgramDisabled() {
282 observeUserProgramDisabledFn.call0();
283 }
284
285 public static void observeUserProgramAutonomous() {
286 observeUserProgramAutonomousFn.call0();
287 }
288
289 public static void observeUserProgramTeleop() {
290 observeUserProgramTeleopFn.call0();
291 }
292
293 public static void observeUserProgramTest() {
294 observeUserProgramTestFn.call0();
295 }
296 }