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 com.sun.cldc.jna.Pointer;
010 import com.sun.cldc.jna.Structure;
011 import edu.wpi.first.wpilibj.communication.FRCControl;
012 import edu.wpi.first.wpilibj.communication.UsageReporting;
013
014 /**
015 * @author bradmiller
016 * Handles input from the Joystick data sent by the FRC Kinect Server
017 * when used with a Kinect device connected to the Driver Station.
018 * Each time a value is requested the most recent value is returned.
019 * Default gestures embedded in the FRC Kinect Server are described
020 * in the document Getting Started with Microsoft Kinect for FRC.
021 */
022 public class KinectStick extends GenericHID {
023
024 private final static byte kJoystickDataID = 24;
025 private final static byte kJoystickDataSize = 18;
026 private int m_recentPacketNumber;
027
028 private int m_id;
029
030 static class JoystickDataBlock extends Structure {
031
032 byte joystick1[] = new byte[6];
033 short button1;
034 byte joystick2[] = new byte[6];
035 short button2;
036
037 public static final int size = kJoystickDataSize - 2;
038
039 JoystickDataBlock(Pointer backingMemory) {
040 useMemory(backingMemory);
041 }
042
043 public void read() {
044 backingNativeMemory.getBytes(0, joystick1, 0, 6);
045 button1 = backingNativeMemory.getShort(6);
046 backingNativeMemory.getBytes(8, joystick2, 0, 6);
047 button2 = backingNativeMemory.getShort(14);
048 }
049
050 public void write() {
051 backingNativeMemory.setBytes(0, joystick1, 0, 6);
052 backingNativeMemory.setShort(6, button1);
053 backingNativeMemory.setBytes(8, joystick2, 0, 6);
054 backingNativeMemory.setShort(14, button2);
055 }
056
057 public int size() {
058 return size;
059 }
060 }
061
062 class JoystickData extends FRCControl.DynamicControlData {
063
064 JoystickDataBlock data;
065
066 {
067 allocateMemory();
068 data = new JoystickDataBlock(
069 new Pointer(backingNativeMemory.address().toUWord().toPrimitive() + 2,
070 JoystickDataBlock.size));
071 }
072
073 public void read() {
074 data.read();
075 }
076
077 public void write() {
078 data.write();
079 }
080
081 public int size() {
082 return kJoystickDataSize;
083 }
084
085 public void copy(JoystickData dest) {
086 write();
087 Pointer.copyBytes(backingNativeMemory, 0, dest.backingNativeMemory, 0, size());
088 dest.read();
089 }
090 }
091 JoystickData tempOutputData = new JoystickData();
092
093 /**
094 * Construct an instance of a KinectStick.
095 * @param id which KinectStick to read, in the default gestures
096 * an id of 1 corresponds to the left arm and 2 to the right arm.
097 */
098 public KinectStick(int id) {
099 m_id = id;
100
101 UsageReporting.report(UsageReporting.kResourceType_KinectStick, id);
102 }
103
104 /**
105 * Update the data in this class with the latest data from the
106 * Driver Station.
107 */
108 private void getData() {
109 if (m_recentPacketNumber != DriverStation.getInstance().getPacketNumber()){
110 m_recentPacketNumber = DriverStation.getInstance().getPacketNumber();
111 int retVal = FRCControl.getDynamicControlData(kJoystickDataID, tempOutputData, tempOutputData.size(), 5);
112 if (retVal != 0) {
113 System.err.println("Bad retval: " + retVal);
114 }
115 }
116 }
117
118 /**
119 * Convert a value from a byte to a double in the
120 * -1 to 1 range
121 * @param rawValue the value to convert
122 * @return the normalized value
123 */
124 private double normalize(byte rawValue) {
125 if(rawValue >= 0)
126 return rawValue / 127.0;
127 else
128 return rawValue / 128.0;
129 }
130
131 /**
132 * Get the X value of the KinectStick. This axis
133 * is unimplemented in the default gestures but can
134 * be populated by teams editing the Kinect Server.
135 * See (@link Joystick for axis number mapping)
136 * @param hand Unused
137 * @return The X value of the KinectStick
138 */
139 public double getX(Hand hand) {
140 getData();
141 return getRawAxis(Joystick.kDefaultXAxis);
142 }
143
144 /**
145 * Get the Y value of the KinectStick. This axis
146 * represents arm angle in the default gestures
147 * See (@link Joystick for axis number mapping)
148 * @param hand Unused
149 * @return The Y value of the KinectStick
150 */
151 public double getY(Hand hand) {
152 getData();
153 return getRawAxis(Joystick.kDefaultYAxis);
154 }
155
156 /**
157 * Get the Z value of the KinectStick. This axis
158 * is unimplemented in the default gestures but can
159 * be populated by teams editing the Kinect Server.
160 * See (@link Joystick for axis number mapping)
161 * @param hand Unused
162 * @return The Z value of the KinectStick
163 */
164 public double getZ(Hand hand) {
165 getData();
166 return getRawAxis(Joystick.kDefaultZAxis);
167 }
168
169 /**
170 * Get the Twist value of the KinectStick. This axis
171 * is unimplemented in the default gestures but can
172 * be populated by teams editing the Kinect Server.
173 * See (@link Joystick for axis number mapping)
174 * @return The Twist value of the KinectStick
175 */
176 public double getTwist() {
177 getData();
178 return getRawAxis(Joystick.kDefaultTwistAxis);
179 }
180
181 /**
182 * Get the Throttle value of the KinectStick. This axis
183 * is unimplemented in the default gestures but can
184 * be populated by teams editing the Kinect Server.
185 * See (@link Joystick for axis number mapping)
186 * @return The Throttle value of the KinectStick
187 */
188 public double getThrottle() {
189 getData();
190 return getRawAxis(Joystick.kDefaultThrottleAxis);
191 }
192
193 /**
194 * Get the value of the KinectStick axis.
195 *
196 * @param axis The axis to read [1-6].
197 * @return The value of the axis
198 */
199 public double getRawAxis(int axis) {
200 if (axis < 1 || axis > DriverStation.kJoystickAxes)
201 return 0.0;
202
203 getData();
204 if (m_id == 1) {
205 return normalize(tempOutputData.data.joystick1[axis-1]);
206 } else {
207 return normalize(tempOutputData.data.joystick2[axis-1]);
208 }
209 }
210
211 /**
212 * Get the button value for the button set as the default trigger in
213 * (@link Joystick)
214 *
215 * @param hand Unused
216 * @return The state of the button.
217 */
218 public boolean getTrigger(Hand hand) {
219 getData();
220 return (tempOutputData.data.button1 & (short) Joystick.kDefaultTriggerButton) != 0;
221 }
222
223 /**
224 * Get the button value for the button set as the default top in
225 * (@link Joystick)
226 *
227 * @param hand Unused
228 * @return The state of the button.
229 */
230 public boolean getTop(Hand hand) {
231 getData();
232 return (tempOutputData.data.button1 & (short) Joystick.kDefaultTopButton) != 0;
233 }
234
235 /**
236 * Get the button value for the button set as the default bumper in
237 * (@link Joystick)
238 *
239 * @param hand Unused
240 * @return The state of the button.
241 */
242 public boolean getBumper(Hand hand) {
243 getData();
244 return (tempOutputData.data.button1 & (short) 4) != 0;
245 }
246
247 /**
248 * Get the button value for buttons 1 through 12. The default gestures
249 * implement only 9 buttons.
250 *
251 * The appropriate button is returned as a boolean value.
252 *
253 * @param button The button number to be read.
254 * @return The state of the button.
255 */
256 public boolean getRawButton(int button) {
257 getData();
258 return (tempOutputData.data.button1 & (short) (1 << (button - 1))) != 0;
259 }
260 }