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
008 package edu.wpi.first.wpilibj;
009
010 import edu.wpi.first.wpilibj.can.*;
011 import edu.wpi.first.wpilibj.communication.Semaphore;
012 import edu.wpi.first.wpilibj.communication.UsageReporting;
013 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
014 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
015 import edu.wpi.first.wpilibj.tables.ITable;
016 import edu.wpi.first.wpilibj.tables.ITableListener;
017
018 public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveWindowSendable {
019
020 // The internal PID control loop in the Jaguar runs at 1kHz.
021 public static final int kControllerRate = 1000;
022 public static final double kApproxBusVoltage = 12.0;
023
024 /**
025 * Mode determines how the Jaguar is controlled
026 */
027 public static class ControlMode {
028
029 public final int value;
030 static final int kPercentVbus_val = 0;
031 static final int kCurrent_val = 1;
032 static final int kSpeed_val = 2;
033 static final int kPosition_val = 3;
034 static final int kVoltage_val = 4;
035 public static final ControlMode kPercentVbus = new ControlMode(kPercentVbus_val);
036 public static final ControlMode kCurrent = new ControlMode(kCurrent_val);
037 public static final ControlMode kSpeed = new ControlMode(kSpeed_val);
038 public static final ControlMode kPosition = new ControlMode(kPosition_val);
039 public static final ControlMode kVoltage = new ControlMode(kVoltage_val);
040
041 private ControlMode(int value) {
042 this.value = value;
043 }
044 }
045
046 /**
047 * Faults reported by the Jaguar
048 */
049 public static class Faults {
050
051 public final int value;
052 static final int kCurrentFault_val = 1;
053 static final int kTemperatureFault_val = 2;
054 static final int kBusVoltageFault_val = 4;
055 static final int kGateDriverFault_val = 8;
056 public static final Faults kCurrentFault = new Faults(kCurrentFault_val);
057 public static final Faults kTemperatureFault = new Faults(kTemperatureFault_val);
058 public static final Faults kBusVoltageFault = new Faults(kBusVoltageFault_val);
059 public static final Faults kGateDriverFault = new Faults(kGateDriverFault_val);
060
061 private Faults(int value) {
062 this.value = value;
063 }
064 }
065
066 /**
067 * Limit switch masks
068 */
069 public static class Limits {
070
071 public final int value;
072 static final int kForwardLimit_val = 1;
073 static final int kReverseLimit_val = 2;
074 public static final Limits kForwardLimit = new Limits(kForwardLimit_val);
075 public static final Limits kReverseLimit = new Limits(kReverseLimit_val);
076
077 private Limits(int value) {
078 this.value = value;
079 }
080 }
081
082 /**
083 * Determines which sensor to use for position reference.
084 */
085 public static class PositionReference {
086
087 public final byte value;
088 static final byte kQuadEncoder_val = 0;
089 static final byte kPotentiometer_val = 1;
090 static final byte kNone_val = (byte)0xFF;
091 public static final PositionReference kQuadEncoder = new PositionReference(kQuadEncoder_val);
092 public static final PositionReference kPotentiometer = new PositionReference(kPotentiometer_val);
093 public static final PositionReference kNone = new PositionReference(kNone_val);
094
095 private PositionReference(byte value) {
096 this.value = value;
097 }
098 }
099
100 /**
101 * Determines which sensor to use for speed reference.
102 */
103 public static class SpeedReference {
104
105 public final byte value;
106 static final byte kEncoder_val = 0;
107 static final byte kInvEncoder_val = 2;
108 static final byte kQuadEncoder_val = 3;
109 static final byte kNone_val = (byte)0xFF;
110 public static final SpeedReference kEncoder = new SpeedReference(kEncoder_val);
111 public static final SpeedReference kInvEncoder = new SpeedReference(kInvEncoder_val);
112 public static final SpeedReference kQuadEncoder = new SpeedReference(kQuadEncoder_val);
113 public static final SpeedReference kNone = new SpeedReference(kNone_val);
114
115 private SpeedReference(byte value) {
116 this.value = value;
117 }
118 }
119
120 /**
121 * Determines how the Jaguar behaves when sending a zero signal.
122 */
123 public static class NeutralMode {
124
125 public final byte value;
126 static final byte kJumper_val = 0;
127 static final byte kBrake_val = 1;
128 static final byte kCoast_val = 2;
129 public static final NeutralMode kJumper = new NeutralMode(kJumper_val);
130 public static final NeutralMode kBrake = new NeutralMode(kBrake_val);
131 public static final NeutralMode kCoast = new NeutralMode(kCoast_val);
132
133 private NeutralMode(byte value) {
134 this.value = value;
135 }
136 }
137
138 /**
139 * Determines which sensor to use for position reference.
140 */
141 public static class LimitMode {
142
143 public final byte value;
144 static final byte kSwitchInputsOnly_val = 0;
145 static final byte kSoftPositionLimit_val = 1;
146 public static final LimitMode kSwitchInputsOnly = new LimitMode(kSwitchInputsOnly_val);
147 public static final LimitMode kSoftPostionLimits = new LimitMode(kSoftPositionLimit_val);
148
149 private LimitMode(byte value) {
150 this.value = value;
151 }
152 }
153
154 private final Object m_transactionMutex = new Object();
155 private byte m_deviceNumber;
156 private ControlMode m_controlMode;
157 private double m_maxOutputVoltage;
158 //private Semaphore m_receiveSemaphore;
159 private MotorSafetyHelper m_safetyHelper;
160 private static final byte[] kNoData = new byte[0];
161
162 private final static int swap16(int x) {
163 return ((((x) >>> 8) & 0x00FF) | (((x) << 8) & 0xFF00));
164 }
165
166 private final static long swap32(long x) {
167 return ((((x) >> 24) & 0x000000FF) | (((x) >> 8) & 0x0000FF00) | (((x) << 8) & 0x00FF0000) | (((x) << 24) & 0xFF000000));
168 }
169
170 private final static int swap16(int x, byte[] buffer) {
171 buffer[0] = (byte) x;
172 buffer[1] = (byte) ((x >>> 8) & 0x00FF);
173 return ((((x) >>> 8) & 0x00FF) | (((x) << 8) & 0xFF00));
174 }
175
176 private final static long swap32(long x, byte[] buffer) {
177 buffer[0] = (byte) x;
178 buffer[1] = (byte) ((x >>> 8) & 0x00FF);
179 buffer[2] = (byte) ((x >>> 16) & 0x00FF);
180 buffer[3] = (byte) ((x >>> 24) & 0x00FF);
181 return ((((x) >> 24) & 0x000000FF) | (((x) >> 8) & 0x0000FF00) | (((x) << 8) & 0x00FF0000) | (((x) << 24) & 0xFF000000));
182 }
183
184 private final static int swap16(byte[] buffer) {
185 return ((((buffer[1]) >>> 8) & 0x00FF) | (((buffer[0]) << 8) & 0xFF00));
186 }
187
188 private final static long swap32(byte[] buffer) {
189 return ((((buffer[3]) >> 24) & 0x000000FF) | (((buffer[2]) >> 8) & 0x0000FF00) | (((buffer[1]) << 8) & 0x00FF0000) | (((buffer[0]) << 24) & 0xFF000000));
190 }
191
192 /**
193 * Pack 16-bit data in little-endian byte order
194 * @param data The data to be packed
195 * @param buffer The buffer to pack into
196 * @param offset The offset into data to pack the variable
197 */
198 private static final void pack16(short data, byte[] buffer, int offset) {
199 buffer[offset] = (byte) (data & 0xFF);
200 buffer[offset + 1] = (byte) ((data >>> 8) & 0xFF);
201 }
202
203 /**
204 * Pack 32-bit data in little-endian byte order
205 * @param data The data to be packed
206 * @param buffer The buffer to pack into
207 * @param offset The offset into data to pack the variable
208 */
209 private static final void pack32(int data, byte[] buffer, int offset) {
210 buffer[offset] = (byte) (data & 0xFF);
211 buffer[offset + 1] = (byte) ((data >>> 8) & 0xFF);
212 buffer[offset + 2] = (byte) ((data >>> 16) & 0xFF);
213 buffer[offset + 3] = (byte) ((data >>> 24) & 0xFF);
214 }
215
216 /**
217 * Unpack 16-bit data from a buffer in little-endian byte order
218 * @param buffer The buffer to unpack from
219 * @param offset The offset into he buffer to unpack
220 * @return The data that was unpacked
221 */
222 private static final short unpack16(byte[] buffer, int offset) {
223 return (short) (((int) buffer[offset] & 0xFF) | (short) ((buffer[offset + 1] << 8)) & 0xFF00);
224 }
225
226 /**
227 * Unpack 32-bit data from a buffer in little-endian byte order
228 * @param buffer The buffer to unpack from
229 * @param offset The offset into he buffer to unpack
230 * @return The data that was unpacked
231 */
232 private static final int unpack32(byte[] buffer, int offset) {
233 return ((int) buffer[offset] & 0xFF) | ((buffer[offset + 1] << 8) & 0xFF00) |
234 ((buffer[offset + 2] << 16) & 0xFF0000) | ((buffer[offset + 3] << 24) & 0xFF000000);
235 }
236 private static final int kFullMessageIDMask = 0x1FFFFFC0;
237
238 /**
239 * Common initialization code called by all constructors.
240 */
241 private void initCANJaguar() throws CANTimeoutException {
242 if (m_deviceNumber < 1 || m_deviceNumber > 63) {
243 throw new RuntimeException("Invalid CAN device number \"" +
244 m_deviceNumber + "\" - must be between 1 and 63.");
245 }
246
247 // VxWorks semaphore for calling async CAN receive API.
248 //Semaphore.Options options = new Semaphore.Options();
249 //options.setPrioritySorted(true);
250 //m_receiveSemaphore = new Semaphore(options, false);
251
252 int fwVer = getFirmwareVersion();
253 if (fwVer >= CANJaguarVersionException.kMinRDKFirmwareVersion ||
254 fwVer < CANJaguarVersionException.kMinLegalFIRSTFirmwareVersion)
255 {
256 throw new CANJaguarVersionException(m_deviceNumber, fwVer);
257 }
258
259 switch (m_controlMode.value) {
260 case ControlMode.kPercentVbus_val:
261 case ControlMode.kVoltage_val:
262 enableControl();
263 break;
264 default:
265 break;
266 }
267 m_safetyHelper = new MotorSafetyHelper(this);
268
269 UsageReporting.report(UsageReporting.kResourceType_CANJaguar, m_deviceNumber, m_controlMode.value);
270 LiveWindow.addActuator("CANJaguar", m_deviceNumber, 0, this);
271 }
272
273 /**
274 * Constructor
275 * Default to percent Vbus control mode.
276 * @param deviceNumber The address of the Jaguar on the CAN bus.
277 */
278 public CANJaguar(int deviceNumber) throws CANTimeoutException {
279 m_deviceNumber = (byte) deviceNumber;
280 m_controlMode = ControlMode.kPercentVbus;
281 m_maxOutputVoltage = kApproxBusVoltage;
282 initCANJaguar();
283 }
284
285 /**
286 * Constructor
287 * @param deviceNumber The address of the Jaguar on the CAN bus.
288 * @param controlMode The control mode that the Jaguar will run in.
289 */
290 public CANJaguar(int deviceNumber, ControlMode controlMode) throws CANTimeoutException {
291 m_deviceNumber = (byte) deviceNumber;
292 m_controlMode = controlMode;
293 m_maxOutputVoltage = kApproxBusVoltage;
294 initCANJaguar();
295 }
296
297 /**
298 * Set the output set-point value.
299 *
300 * The scale and the units depend on the mode the Jaguar is in.
301 * In PercentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM Jaguar).
302 * In Voltage Mode, the outputValue is in Volts.
303 * In Current Mode, the outputValue is in Amps.
304 * In Speed Mode, the outputValue is in Rotations/Minute.
305 * In Position Mode, the outputValue is in Rotations.
306 *
307 * @param outputValue The set-point to sent to the motor controller.
308 */
309 public void setX(double outputValue) throws CANTimeoutException {
310 setX(outputValue, (byte) 0);
311 }
312
313 /**
314 * Set the output set-point value.
315 *
316 * Needed by the SpeedControl interface (swallows CANTimeoutExceptions).
317 *
318 * @deprecated Use setX instead.
319 * @param outputValue The set-point to sent to the motor controller.
320 */
321 public void set(double outputValue) {
322 set(outputValue, (byte) 0);
323 }
324
325 /**
326 * Set the output set-point value.
327 *
328 * The scale and the units depend on the mode the Jaguar is in.
329 * In PercentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM Jaguar).
330 * In Voltage Mode, the outputValue is in Volts.
331 * In Current Mode, the outputValue is in Amps.
332 * In Speed Mode, the outputValue is in Rotations/Minute.
333 * In Position Mode, the outputValue is in Rotations.
334 *
335 * @param outputValue The set-point to sent to the motor controller.
336 * @param syncGroup The update group to add this set() to, pending updateSyncGroup(). If 0, update immediately.
337 */
338 public void setX(double outputValue, byte syncGroup) throws CANTimeoutException {
339 int messageID = 0;
340 byte[] dataBuffer = new byte[8];
341 byte dataSize = 0;
342
343 if (!m_safetyHelper.isAlive()) {
344 enableControl();
345 }
346
347 switch (m_controlMode.value) {
348 case ControlMode.kPercentVbus_val:
349 messageID = JaguarCANProtocol.LM_API_VOLT_T_SET;
350 if (outputValue > 1.0) outputValue = 1.0;
351 if (outputValue < -1.0) outputValue = -1.0;
352 packPercentage(dataBuffer, outputValue);
353 dataSize = 2;
354 break;
355 case ControlMode.kSpeed_val: {
356 messageID = JaguarCANProtocol.LM_API_SPD_T_SET;
357 dataSize = packFXP16_16(dataBuffer, outputValue);
358 }
359 break;
360 case ControlMode.kPosition_val: {
361 messageID = JaguarCANProtocol.LM_API_POS_T_SET;
362 dataSize = packFXP16_16(dataBuffer, outputValue);
363 }
364 break;
365 case ControlMode.kCurrent_val: {
366 messageID = JaguarCANProtocol.LM_API_ICTRL_T_SET;
367 dataSize = packFXP8_8(dataBuffer, outputValue);
368 }
369 break;
370 case ControlMode.kVoltage_val: {
371 messageID = JaguarCANProtocol.LM_API_VCOMP_T_SET;
372 dataSize = packFXP8_8(dataBuffer, outputValue);
373 }
374 break;
375 default:
376 return;
377 }
378 if (syncGroup != 0) {
379 dataBuffer[dataSize] = syncGroup;
380 dataSize++;
381 }
382 setTransaction(messageID, dataBuffer, dataSize);
383 m_safetyHelper.feed();
384 }
385
386 /**
387 * Set the output set-point value.
388 *
389 * Needed by the SpeedControl interface (swallows CANTimeoutExceptions).
390 *
391 * @deprecated Use setX instead.
392 * @param outputValue The set-point to sent to the motor controller.
393 * @param syncGroup The update group to add this set() to, pending updateSyncGroup(). If 0, update immediately.
394 */
395 public void set(double outputValue, byte syncGroup) {
396 try {
397 setX(outputValue, syncGroup);
398 } catch (CANTimeoutException e) {}
399 }
400
401 /**
402 * Get the recently set outputValue setpoint.
403 *
404 * The scale and the units depend on the mode the Jaguar is in.
405 * In PercentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM Jaguar).
406 * In Voltage Mode, the outputValue is in Volts.
407 * In Current Mode, the outputValue is in Amps.
408 * In Speed Mode, the outputValue is in Rotations/Minute.
409 * In Position Mode, the outputValue is in Rotations.
410 *
411 * @return The most recently set outputValue setpoint.
412 */
413 public double getX() throws CANTimeoutException {
414 byte[] dataBuffer = new byte[8];
415 byte dataSize = 0;
416
417 switch (m_controlMode.value) {
418 case ControlMode.kPercentVbus_val:
419 dataSize = getTransaction(JaguarCANProtocol.LM_API_VOLT_SET, dataBuffer);
420 if (dataSize == 2) {
421 return unpackPercentage(dataBuffer);
422 }
423 break;
424 case ControlMode.kSpeed_val:
425 dataSize = getTransaction(JaguarCANProtocol.LM_API_SPD_SET, dataBuffer);
426 if (dataSize == 4) {
427 return unpackFXP16_16(dataBuffer);
428 }
429 break;
430 case ControlMode.kPosition_val:
431 dataSize = getTransaction(JaguarCANProtocol.LM_API_POS_SET, dataBuffer);
432 if (dataSize == 4) {
433 return unpackFXP16_16(dataBuffer);
434 }
435 break;
436 case ControlMode.kCurrent_val:
437 dataSize = getTransaction(JaguarCANProtocol.LM_API_ICTRL_SET, dataBuffer);
438 if (dataSize == 2) {
439 return unpackFXP8_8(dataBuffer);
440 }
441 break;
442 case ControlMode.kVoltage_val:
443 dataSize = getTransaction(JaguarCANProtocol.LM_API_VCOMP_SET, dataBuffer);
444 if (dataSize == 2) {
445 return unpackFXP8_8(dataBuffer);
446 }
447 break;
448 }
449 return 0.0;
450
451 }
452
453 /**
454 * Get the recently set outputValue setpoint.
455 *
456 * Needed by the SpeedControl interface (swallows CANTimeoutExceptions).
457 *
458 * @deprecated Use getX instead.
459 * @return The most recently set outputValue setpoint.
460 */
461 public double get() {
462 try {
463 return getX();
464 } catch (CANTimeoutException e) {
465 return 0.0;
466 }
467 }
468
469 /**
470 * Common interface for disabling a motor.
471 *
472 * Needed by the SpeedControl interface (swallows CANTimeoutExceptions).
473 *
474 * @deprecated Use disableControl instead.
475 */
476 public void disable() {
477 try {
478 disableControl();
479 } catch (CANTimeoutException e) {}
480 }
481
482 /**
483 * Write out the PID value as seen in the PIDOutput base object.
484 *
485 * @deprecated Use setX instead.
486 * @param output Write out the percentage voltage value as was computed by the PIDController
487 */
488 public void pidWrite(double output) {
489 if (m_controlMode == ControlMode.kPercentVbus) {
490 set(output);
491 } else {
492 // TODO: Error... only percent vbus mode supported for PID API
493 }
494 }
495
496 byte packPercentage(byte[] buffer, double value) {
497 short intValue = (short) (value * 32767.0);
498 swap16(intValue, buffer);
499 return 2;
500 }
501
502 byte packFXP8_8(byte[] buffer, double value) {
503 short intValue = (short) (value * 256.0);
504 swap16(intValue, buffer);
505 return 2;
506 }
507
508 byte packFXP16_16(byte[] buffer, double value) {
509 int intValue = (int) (value * 65536.0);
510 swap32(intValue, buffer);
511 return 4;
512 }
513
514 byte packINT16(byte[] buffer, short value) {
515 swap16(value, buffer);
516 return 2;
517 }
518
519 byte packINT32(byte[] buffer, int value) {
520 swap32(value, buffer);
521 return 4;
522 }
523
524 double unpackPercentage(byte[] buffer) {
525 return unpack16(buffer,0) / 32767.0;
526 }
527
528 double unpackFXP8_8(byte[] buffer) {
529 return unpack16(buffer,0) / 256.0;
530 }
531
532 double unpackFXP16_16(byte[] buffer) {
533 return unpack32(buffer,0) / 65536.0;
534 }
535
536 int unpackINT16(byte[] buffer) {
537 return unpack16(buffer,0);
538 }
539
540 long unpackINT32(byte[] buffer) {
541 return unpack32(buffer,0);
542 }
543 private static final byte[] sendTrustedDataBuffer = new byte[JaguarCANDriver.kMaxMessageDataSize];
544
545 /**
546 * Send a message on the CAN bus through the CAN driver in FRC_NetworkCommunication
547 *
548 * Trusted messages require a 2-byte token at the beginning of the data payload.
549 * If the message being sent is trusted, make space for the token.
550 *
551 * @param messageID The messageID to be used on the CAN bus
552 * @param data The up to 8 bytes of data to be sent with the message
553 * @param dataSize Specify how much of the data in "data" to send
554 */
555 protected static void sendMessage(int messageID, byte[] data, int dataSize) throws CANTimeoutException {
556 final int[] kTrustedMessages = {
557 JaguarCANProtocol.LM_API_VOLT_T_EN, JaguarCANProtocol.LM_API_VOLT_T_SET,
558 JaguarCANProtocol.LM_API_SPD_T_EN, JaguarCANProtocol.LM_API_SPD_T_SET,
559 JaguarCANProtocol.LM_API_VCOMP_T_EN, JaguarCANProtocol.LM_API_VCOMP_T_SET,
560 JaguarCANProtocol.LM_API_POS_T_EN, JaguarCANProtocol.LM_API_POS_T_SET,
561 JaguarCANProtocol.LM_API_ICTRL_T_EN, JaguarCANProtocol.LM_API_ICTRL_T_SET};
562
563 byte i;
564 for (i = 0; i < kTrustedMessages.length; i++) {
565 if ((kFullMessageIDMask & messageID) == kTrustedMessages[i]) {
566 sendTrustedDataBuffer[0] = 0;
567 sendTrustedDataBuffer[1] = 0;
568 // Make sure the data will still fit after adjusting for the token.
569 if (dataSize > JaguarCANDriver.kMaxMessageDataSize - 2) {
570 throw new RuntimeException("CAN message has too much data.");
571 }
572
573 byte j;
574 for (j = 0; j < dataSize; j++) {
575 sendTrustedDataBuffer[j + 2] = data[j];
576 }
577
578 JaguarCANDriver.sendMessage(messageID, sendTrustedDataBuffer, dataSize + 2);
579 return;
580 }
581 }
582 JaguarCANDriver.sendMessage(messageID, data, dataSize);
583 }
584
585 /**
586 * Receive a message from the CAN bus through the CAN driver in FRC_NetworkCommunication
587 *
588 * @param messageID The messageID to read from the CAN bus
589 * @param data The up to 8 bytes of data that was received with the message
590 * @param timeout Specify how long to wait for a message (in seconds)
591 */
592 protected static byte receiveMessage(int messageID, byte[] data, double timeout) throws CANTimeoutException {
593 JaguarCANDriver canDriver = new JaguarCANDriver();
594 byte dataSize = canDriver.receiveMessage(messageID, data, timeout);
595 return dataSize;
596 }
597
598 protected static byte receiveMessage(int messageID, byte[] data) throws CANTimeoutException {
599 return receiveMessage(messageID, data, 0.01);
600 }
601
602 /**
603 * Execute a transaction with a Jaguar that sets some property.
604 *
605 * Jaguar always acks when it receives a message. If we don't wait for an ack,
606 * the message object in the Jaguar could get overwritten before it is handled.
607 *
608 * @param messageID The messageID to be used on the CAN bus (device number is added internally)
609 * @param data The up to 8 bytes of data to be sent with the message
610 * @param dataSize Specify how much of the data in "data" to send
611 */
612 protected byte setTransaction(int messageID, byte[] data, byte dataSize) throws CANTimeoutException {
613 int ackMessageID = JaguarCANProtocol.LM_API_ACK | m_deviceNumber;
614
615 // Make sure we don't have more than one transaction with the same Jaguar outstanding.
616 synchronized (m_transactionMutex) {
617 // Throw away any stale acks.
618 try {
619 receiveMessage(ackMessageID, kNoData, 0.0);
620 }
621 catch(CANTimeoutException e) {}
622 // Send the message with the data.
623 sendMessage(messageID | m_deviceNumber, data, dataSize);
624 // Wait for an ack.
625 dataSize = receiveMessage(ackMessageID, kNoData);
626 }
627 return dataSize;
628 }
629
630 /**
631 * Execute a transaction with a Jaguar that gets some property.
632 *
633 * Jaguar always generates a message with the same message ID when replying.
634 *
635 * @param messageID The messageID to read from the CAN bus (device number is added internally)
636 * @param data The up to 8 bytes of data that was received with the message
637 * @return Indicates how much data was received
638 */
639 protected byte getTransaction(int messageID, byte[] data) throws CANTimeoutException {
640 int targetedMessageID = messageID | m_deviceNumber;
641 byte dataSize = 0;
642 // Make sure we don't have more than one transaction with the same Jaguar outstanding.
643 synchronized (m_transactionMutex) {
644 // Send the message requesting data.
645 sendMessage(targetedMessageID, kNoData, 0);
646 // Caller may have set bit31 for remote frame transmission so clear invalid bits[31-29]
647 targetedMessageID &= 0x1FFFFFFF;
648 // Wait for the data.
649 dataSize = receiveMessage(targetedMessageID, data);
650 }
651 return dataSize;
652 }
653
654 /**
655 * Set the reference source device for speed controller mode.
656 *
657 * Choose encoder as the source of speed feedback when in speed control mode.
658 *
659 * @param reference Specify a SpeedReference.
660 */
661 public void setSpeedReference(SpeedReference reference) throws CANTimeoutException {
662 byte[] dataBuffer = new byte[8];
663
664 dataBuffer[0] = reference.value;
665 setTransaction(JaguarCANProtocol.LM_API_SPD_REF, dataBuffer, (byte) 1);
666 }
667
668 /**
669 * Get the reference source device for speed controller mode.
670 *
671 * @return A SpeedReference indicating the currently selected reference device for speed controller mode.
672 */
673 public SpeedReference getSpeedReference() throws CANTimeoutException {
674 byte[] dataBuffer = new byte[8];
675 byte dataSize = 0;
676
677 dataSize = getTransaction(JaguarCANProtocol.LM_API_SPD_REF, dataBuffer);
678 if (dataSize == 1) {
679 switch (dataBuffer[0]) {
680 case SpeedReference.kEncoder_val:
681 return SpeedReference.kEncoder;
682 case SpeedReference.kInvEncoder_val:
683 return SpeedReference.kInvEncoder;
684 case SpeedReference.kQuadEncoder_val:
685 return SpeedReference.kQuadEncoder;
686 case SpeedReference.kNone_val:
687 return SpeedReference.kNone;
688 }
689 }
690 return SpeedReference.kNone;
691 }
692
693 /**
694 * Set the reference source device for position controller mode.
695 *
696 * Choose between using and encoder and using a potentiometer
697 * as the source of position feedback when in position control mode.
698 *
699 * @param reference Specify a PositionReference.
700 */
701 public void setPositionReference(PositionReference reference) throws CANTimeoutException {
702 byte[] dataBuffer = new byte[8];
703
704 dataBuffer[0] = reference.value;
705 setTransaction(JaguarCANProtocol.LM_API_POS_REF, dataBuffer, (byte) 1);
706 }
707
708 /**
709 * Get the reference source device for position controller mode.
710 *
711 * @return A PositionReference indicating the currently selected reference device for position controller mode.
712 */
713 public PositionReference getPositionReference() throws CANTimeoutException {
714 byte[] dataBuffer = new byte[8];
715 byte dataSize = 0;
716
717 dataSize = getTransaction(JaguarCANProtocol.LM_API_POS_REF, dataBuffer);
718 if (dataSize == 1) {
719 switch (dataBuffer[0]) {
720 case PositionReference.kPotentiometer_val:
721 return PositionReference.kPotentiometer;
722 case PositionReference.kQuadEncoder_val:
723 return PositionReference.kQuadEncoder;
724 case PositionReference.kNone_val:
725 return PositionReference.kNone;
726 }
727 }
728 return PositionReference.kNone;
729 }
730
731 /**
732 * Set the P, I, and D constants for the closed loop modes.
733 *
734 * @param p The proportional gain of the Jaguar's PID controller.
735 * @param i The integral gain of the Jaguar's PID controller.
736 * @param d The differential gain of the Jaguar's PID controller.
737 */
738 public void setPID(double p, double i, double d) throws CANTimeoutException {
739 byte[] dataBuffer = new byte[8];
740 byte dataSize;
741
742 switch (m_controlMode.value) {
743 case ControlMode.kPercentVbus_val:
744 case ControlMode.kVoltage_val:
745 // TODO: Error, Not Valid
746 break;
747 case ControlMode.kSpeed_val:
748 dataSize = packFXP16_16(dataBuffer, p);
749 setTransaction(JaguarCANProtocol.LM_API_SPD_PC, dataBuffer, dataSize);
750 dataSize = packFXP16_16(dataBuffer, i);
751 setTransaction(JaguarCANProtocol.LM_API_SPD_IC, dataBuffer, dataSize);
752 dataSize = packFXP16_16(dataBuffer, d);
753 setTransaction(JaguarCANProtocol.LM_API_SPD_DC, dataBuffer, dataSize);
754 break;
755 case ControlMode.kPosition_val:
756 dataSize = packFXP16_16(dataBuffer, p);
757 setTransaction(JaguarCANProtocol.LM_API_POS_PC, dataBuffer, dataSize);
758 dataSize = packFXP16_16(dataBuffer, i);
759 setTransaction(JaguarCANProtocol.LM_API_POS_IC, dataBuffer, dataSize);
760 dataSize = packFXP16_16(dataBuffer, d);
761 setTransaction(JaguarCANProtocol.LM_API_POS_DC, dataBuffer, dataSize);
762 break;
763 case ControlMode.kCurrent_val:
764 dataSize = packFXP16_16(dataBuffer, p);
765 setTransaction(JaguarCANProtocol.LM_API_ICTRL_PC, dataBuffer, dataSize);
766 dataSize = packFXP16_16(dataBuffer, i);
767 setTransaction(JaguarCANProtocol.LM_API_ICTRL_IC, dataBuffer, dataSize);
768 dataSize = packFXP16_16(dataBuffer, d);
769 setTransaction(JaguarCANProtocol.LM_API_ICTRL_DC, dataBuffer, dataSize);
770 break;
771 }
772 }
773
774 /**
775 * Get the Proportional gain of the controller.
776 *
777 * @return The proportional gain.
778 */
779 public double getP() throws CANTimeoutException {
780 byte[] dataBuffer = new byte[8];
781 byte dataSize = 0;
782
783 switch (m_controlMode.value) {
784 case ControlMode.kPercentVbus_val:
785 case ControlMode.kVoltage_val:
786 // TODO: Error, Not Valid
787 break;
788 case ControlMode.kSpeed_val:
789 dataSize = getTransaction(JaguarCANProtocol.LM_API_SPD_PC, dataBuffer);
790 if (dataSize == 4) {
791 return unpackFXP16_16(dataBuffer);
792 }
793 break;
794 case ControlMode.kPosition_val:
795 dataSize = getTransaction(JaguarCANProtocol.LM_API_POS_PC, dataBuffer);
796 if (dataSize == 4) {
797 return unpackFXP16_16(dataBuffer);
798 }
799 break;
800 case ControlMode.kCurrent_val:
801 dataSize = getTransaction(JaguarCANProtocol.LM_API_ICTRL_PC, dataBuffer);
802 if (dataSize == 4) {
803 return unpackFXP16_16(dataBuffer);
804 }
805 break;
806 }
807 return 0.0;
808 }
809
810 /**
811 * Get the Intregral gain of the controller.
812 *
813 * @return The integral gain.
814 */
815 public double getI() throws CANTimeoutException {
816 byte[] dataBuffer = new byte[8];
817 byte dataSize = 0;
818
819 switch (m_controlMode.value) {
820 case ControlMode.kPercentVbus_val:
821 case ControlMode.kVoltage_val:
822 // TODO: Error, Not Valid
823 break;
824 case ControlMode.kSpeed_val:
825 dataSize = getTransaction(JaguarCANProtocol.LM_API_SPD_IC, dataBuffer);
826 if (dataSize == 4) {
827 return unpackFXP16_16(dataBuffer);
828 }
829 break;
830 case ControlMode.kPosition_val:
831 dataSize = getTransaction(JaguarCANProtocol.LM_API_POS_IC, dataBuffer);
832 if (dataSize == 4) {
833 return unpackFXP16_16(dataBuffer);
834 }
835 break;
836 case ControlMode.kCurrent_val:
837 dataSize = getTransaction(JaguarCANProtocol.LM_API_ICTRL_IC, dataBuffer);
838 if (dataSize == 4) {
839 return unpackFXP16_16(dataBuffer);
840 }
841 break;
842 }
843 return 0.0;
844 }
845
846 /**
847 * Get the Differential gain of the controller.
848 *
849 * @return The differential gain.
850 */
851 public double getD() throws CANTimeoutException {
852 byte[] dataBuffer = new byte[8];
853 byte dataSize = 0;
854
855 switch (m_controlMode.value) {
856 case ControlMode.kPercentVbus_val:
857 case ControlMode.kVoltage_val:
858 // TODO: Error, Not Valid
859 break;
860 case ControlMode.kSpeed_val:
861 dataSize = getTransaction(JaguarCANProtocol.LM_API_SPD_DC, dataBuffer);
862 if (dataSize == 4) {
863 return unpackFXP16_16(dataBuffer);
864 }
865 break;
866 case ControlMode.kPosition_val:
867 dataSize = getTransaction(JaguarCANProtocol.LM_API_POS_DC, dataBuffer);
868 if (dataSize == 4) {
869 return unpackFXP16_16(dataBuffer);
870 }
871 break;
872 case ControlMode.kCurrent_val:
873 dataSize = getTransaction(JaguarCANProtocol.LM_API_ICTRL_DC, dataBuffer);
874 if (dataSize == 4) {
875 return unpackFXP16_16(dataBuffer);
876 }
877 break;
878 }
879 return 0.0;
880 }
881
882 /**
883 * Enable the closed loop controller.
884 *
885 * Start actually controlling the output based on the feedback.
886 */
887 public void enableControl() throws CANTimeoutException {
888 enableControl(0.0);
889 }
890
891 /**
892 * Enable the closed loop controller.
893 *
894 * Start actually controlling the output based on the feedback.
895 * If starting a position controller with an encoder reference,
896 * use the encoderInitialPosition parameter to initialize the
897 * encoder state.
898 * @param encoderInitialPosition Encoder position to set if position with encoder reference. Ignored otherwise.
899 */
900 public void enableControl(double encoderInitialPosition) throws CANTimeoutException {
901 byte[] dataBuffer = new byte[8];
902 byte dataSize = 0;
903
904 switch (m_controlMode.value) {
905 case ControlMode.kPercentVbus_val:
906 setTransaction(JaguarCANProtocol.LM_API_VOLT_T_EN, dataBuffer, dataSize);
907 break;
908 case ControlMode.kSpeed_val:
909 setTransaction(JaguarCANProtocol.LM_API_SPD_T_EN, dataBuffer, dataSize);
910 break;
911 case ControlMode.kPosition_val:
912 dataSize = packFXP16_16(dataBuffer, encoderInitialPosition);
913 setTransaction(JaguarCANProtocol.LM_API_POS_T_EN, dataBuffer, dataSize);
914 break;
915 case ControlMode.kCurrent_val:
916 setTransaction(JaguarCANProtocol.LM_API_ICTRL_T_EN, dataBuffer, dataSize);
917 break;
918 case ControlMode.kVoltage_val:
919 setTransaction(JaguarCANProtocol.LM_API_VCOMP_T_EN, dataBuffer, dataSize);
920 break;
921 }
922 }
923
924 /**
925 * Disable the closed loop controller.
926 *
927 * Stop driving the output based on the feedback.
928 */
929 public void disableControl() throws CANTimeoutException {
930 byte[] dataBuffer = new byte[8];
931 byte dataSize = 0;
932
933 switch (m_controlMode.value) {
934 case ControlMode.kPercentVbus_val:
935 setTransaction(JaguarCANProtocol.LM_API_VOLT_DIS, dataBuffer, dataSize);
936 break;
937 case ControlMode.kSpeed_val:
938 setTransaction(JaguarCANProtocol.LM_API_SPD_DIS, dataBuffer, dataSize);
939 break;
940 case ControlMode.kPosition_val:
941 setTransaction(JaguarCANProtocol.LM_API_POS_DIS, dataBuffer, dataSize);
942 break;
943 case ControlMode.kCurrent_val:
944 setTransaction(JaguarCANProtocol.LM_API_ICTRL_DIS, dataBuffer, dataSize);
945 break;
946 case ControlMode.kVoltage_val:
947 setTransaction(JaguarCANProtocol.LM_API_VCOMP_DIS, dataBuffer, dataSize);
948 break;
949 }
950 }
951
952 /**
953 * Change the control mode of this Jaguar object.
954 *
955 * After changing modes, configure any PID constants or other settings needed
956 * and then enableControl() to actually change the mode on the Jaguar.
957 *
958 * @param controlMode The new mode.
959 */
960 public void changeControlMode(ControlMode controlMode) throws CANTimeoutException {
961 // Disable the previous mode
962 disableControl();
963
964 // Update the local mode
965 m_controlMode = controlMode;
966
967 UsageReporting.report(UsageReporting.kResourceType_CANJaguar, m_deviceNumber, m_controlMode.value);
968 }
969
970 /**
971 * Get the active control mode from the Jaguar.
972 *
973 * Ask the Jag what mode it is in.
974 *
975 * @return ControlMode that the Jag is in.
976 */
977 public ControlMode getControlMode() throws CANTimeoutException {
978 byte[] dataBuffer = new byte[8];
979 byte dataSize = 0;
980
981 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_CMODE, dataBuffer);
982 if (dataSize == 1)
983 {
984 switch (dataBuffer[0]) {
985 case ControlMode.kPercentVbus_val:
986 return ControlMode.kPercentVbus;
987 case ControlMode.kCurrent_val:
988 return ControlMode.kCurrent;
989 case ControlMode.kSpeed_val:
990 return ControlMode.kSpeed;
991 case ControlMode.kPosition_val:
992 return ControlMode.kPosition;
993 case ControlMode.kVoltage_val:
994 return ControlMode.kVoltage;
995 }
996 }
997 return ControlMode.kPercentVbus;
998 }
999
1000 /**
1001 * Get the voltage at the battery input terminals of the Jaguar.
1002 *
1003 * @return The bus voltage in Volts.
1004 */
1005 public double getBusVoltage() throws CANTimeoutException {
1006 byte[] dataBuffer = new byte[8];
1007 byte dataSize = 0;
1008
1009 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_VOLTBUS, dataBuffer);
1010 if (dataSize == 2) {
1011 return unpackFXP8_8(dataBuffer);
1012 }
1013 return 0.0;
1014 }
1015
1016 /**
1017 * Get the voltage being output from the motor terminals of the Jaguar.
1018 *
1019 * @return The output voltage in Volts.
1020 */
1021 public double getOutputVoltage() throws CANTimeoutException {
1022 byte[] dataBuffer = new byte[8];
1023 byte dataSize = 0;
1024
1025 // Read the volt out which is in Volts units.
1026 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_VOUT, dataBuffer);
1027 if (dataSize == 2) {
1028 return unpackFXP8_8(dataBuffer);
1029 }
1030 return 0.0;
1031 }
1032
1033 /**
1034 * Get the current through the motor terminals of the Jaguar.
1035 *
1036 * @return The output current in Amps.
1037 */
1038 public double getOutputCurrent() throws CANTimeoutException {
1039 byte[] dataBuffer = new byte[8];
1040 byte dataSize;
1041
1042 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_CURRENT, dataBuffer);
1043 if (dataSize == 2) {
1044 return unpackFXP8_8(dataBuffer);
1045 }
1046 return 0.0;
1047
1048 }
1049
1050 /**
1051 * Get the internal temperature of the Jaguar.
1052 *
1053 * @return The temperature of the Jaguar in degrees Celsius.
1054 */
1055 public double getTemperature() throws CANTimeoutException {
1056 byte[] dataBuffer = new byte[8];
1057 byte dataSize;
1058
1059 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_TEMP, dataBuffer);
1060 if (dataSize == 2) {
1061 return unpackFXP8_8(dataBuffer);
1062 }
1063 return 0.0;
1064 }
1065
1066 /**
1067 * Get the position of the encoder or potentiometer.
1068 *
1069 * @return The position of the motor based on the configured feedback.
1070 */
1071 public double getPosition() throws CANTimeoutException {
1072 byte[] dataBuffer = new byte[8];
1073 byte dataSize;
1074
1075 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_POS, dataBuffer);
1076 if (dataSize == 4) {
1077 return unpackFXP16_16(dataBuffer);
1078 }
1079 return 0.0;
1080 }
1081
1082 /**
1083 * Get the speed of the encoder.
1084 *
1085 * @return The speed of the motor in RPM based on the configured feedback.
1086 */
1087 public double getSpeed() throws CANTimeoutException {
1088 byte[] dataBuffer = new byte[8];
1089 byte dataSize;
1090
1091 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_SPD, dataBuffer);
1092 if (dataSize == 4) {
1093 return unpackFXP16_16(dataBuffer);
1094 }
1095 return 0.0;
1096 }
1097
1098 /**
1099 * Get the status of the forward limit switch.
1100 *
1101 * @return The motor is allowed to turn in the forward direction when true.
1102 */
1103 public boolean getForwardLimitOK() throws CANTimeoutException {
1104 byte[] dataBuffer = new byte[8];
1105 byte dataSize;
1106
1107 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_LIMIT, dataBuffer);
1108 if (dataSize == 1) {
1109 return (dataBuffer[0] & Limits.kForwardLimit_val) != 0;
1110 }
1111 return false;
1112 }
1113
1114 /**
1115 * Get the status of the reverse limit switch.
1116 *
1117 * @return The motor is allowed to turn in the reverse direction when true.
1118 */
1119 public boolean getReverseLimitOK() throws CANTimeoutException {
1120 byte[] dataBuffer = new byte[8];
1121 byte dataSize;
1122
1123 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_LIMIT, dataBuffer);
1124 if (dataSize == 1) {
1125 return (dataBuffer[0] & Limits.kReverseLimit_val) != 0;
1126 }
1127 return false;
1128 }
1129
1130 /**
1131 * Get the status of any faults the Jaguar has detected.
1132 *
1133 * @return A bit-mask of faults defined by the "Faults" enum class.
1134 */
1135 public short getFaults() throws CANTimeoutException {
1136 byte[] dataBuffer = new byte[8];
1137 byte dataSize;
1138
1139 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_FAULT, dataBuffer);
1140 if (dataSize == 2) {
1141 return (short)unpackINT16(dataBuffer);
1142 }
1143 return 0;
1144 }
1145
1146 /**
1147 * Check if the Jaguar's power has been cycled since this was last called.
1148 *
1149 * This should return true the first time called after a Jaguar power up,
1150 * and false after that.
1151 *
1152 * @return The Jaguar was power cycled since the last call to this function.
1153 */
1154 public boolean getPowerCycled() throws CANTimeoutException {
1155 byte[] dataBuffer = new byte[8];
1156 byte dataSize;
1157
1158 dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_POWER, dataBuffer);
1159 if (dataSize == 1) {
1160 boolean powerCycled = dataBuffer[0] != 0;
1161
1162 // Clear the power cycled bit now that we've accessed it
1163 if (powerCycled) {
1164 dataBuffer[0] = 1;
1165 setTransaction(JaguarCANProtocol.LM_API_STATUS_POWER, dataBuffer, (byte) 1);
1166 }
1167
1168 return powerCycled;
1169 }
1170 return false;
1171 }
1172
1173 /**
1174 * Set the maximum voltage change rate.
1175 *
1176 * When in percent voltage output mode, the rate at which the voltage changes can
1177 * be limited to reduce current spikes. Set this to 0.0 to disable rate limiting.
1178 *
1179 * @param rampRate The maximum rate of voltage change in Percent Voltage mode in V/s.
1180 */
1181 public void setVoltageRampRate(double rampRate) throws CANTimeoutException {
1182 byte[] dataBuffer = new byte[8];
1183 byte dataSize;
1184
1185 switch (m_controlMode.value) {
1186 case ControlMode.kPercentVbus_val:
1187 dataSize = packPercentage(dataBuffer, rampRate / (m_maxOutputVoltage * kControllerRate));
1188 setTransaction(JaguarCANProtocol.LM_API_VOLT_SET_RAMP, dataBuffer, dataSize);
1189 break;
1190 case ControlMode.kVoltage_val:
1191 dataSize = packFXP8_8(dataBuffer, rampRate / kControllerRate);
1192 setTransaction(JaguarCANProtocol.LM_API_VCOMP_IN_RAMP, dataBuffer, dataSize);
1193 break;
1194 default:
1195 return;
1196 }
1197 }
1198
1199 /**
1200 * Get the version of the firmware running on the Jaguar.
1201 *
1202 * @return The firmware version. 0 if the device did not respond.
1203 */
1204 public int getFirmwareVersion() throws CANTimeoutException {
1205 byte[] dataBuffer = new byte[8];
1206 byte dataSize;
1207
1208 // Set the MSB to tell the 2CAN that this is a remote message.
1209 dataSize = getTransaction(0x80000000 | JaguarCANProtocol.CAN_MSGID_API_FIRMVER, dataBuffer);
1210 if (dataSize == 4) {
1211 return (int)unpackINT32(dataBuffer);
1212 }
1213 return 0;
1214 }
1215
1216 /**
1217 * Get the version of the Jaguar hardware.
1218 *
1219 * @return The hardware version. 1: Jaguar, 2: Black Jaguar
1220 */
1221 public byte getHardwareVersion() throws CANTimeoutException {
1222 byte[] dataBuffer = new byte[8];
1223 byte dataSize;
1224
1225 dataSize = getTransaction(JaguarCANProtocol.LM_API_HWVER, dataBuffer);
1226 if (dataSize == 1 + 1) {
1227 if (dataBuffer[0] == m_deviceNumber) {
1228 return dataBuffer[1];
1229 }
1230 }
1231 // Assume Gray Jag if there is no response
1232 return JaguarCANProtocol.LM_HWVER_JAG_1_0;
1233 }
1234
1235 /**
1236 * Configure what the controller does to the H-Bridge when neutral (not driving the output).
1237 *
1238 * This allows you to override the jumper configuration for brake or coast.
1239 *
1240 * @param mode Select to use the jumper setting or to override it to coast or brake.
1241 */
1242 public void configNeutralMode(NeutralMode mode) throws CANTimeoutException {
1243 byte[] dataBuffer = new byte[8];
1244
1245 dataBuffer[0] = mode.value;
1246 setTransaction(JaguarCANProtocol.LM_API_CFG_BRAKE_COAST, dataBuffer, (byte) 1);
1247 }
1248
1249 /**
1250 * Configure how many codes per revolution are generated by your encoder.
1251 *
1252 * @param codesPerRev The number of counts per revolution in 1X mode.
1253 */
1254 public void configEncoderCodesPerRev(int codesPerRev) throws CANTimeoutException {
1255 byte[] dataBuffer = new byte[8];
1256 byte dataSize;
1257
1258 dataSize = packINT16(dataBuffer, (short)codesPerRev);
1259 setTransaction(JaguarCANProtocol.LM_API_CFG_ENC_LINES, dataBuffer, dataSize);
1260 }
1261
1262 /**
1263 * Configure the number of turns on the potentiometer.
1264 *
1265 * There is no special support for continuous turn potentiometers.
1266 * Only integer numbers of turns are supported.
1267 *
1268 * @param turns The number of turns of the potentiometer
1269 */
1270 public void configPotentiometerTurns(int turns) throws CANTimeoutException {
1271 byte[] dataBuffer = new byte[8];
1272 byte dataSize;
1273
1274 dataSize = packINT16(dataBuffer, (short)turns);
1275 setTransaction(JaguarCANProtocol.LM_API_CFG_POT_TURNS, dataBuffer, dataSize);
1276 }
1277
1278 /**
1279 * Configure Soft Position Limits when in Position Controller mode.
1280 *
1281 * When controlling position, you can add additional limits on top of the limit switch inputs
1282 * that are based on the position feedback. If the position limit is reached or the
1283 * switch is opened, that direction will be disabled.
1284 *
1285 * @param forwardLimitPosition The position that if exceeded will disable the forward direction.
1286 * @param reverseLimitPosition The position that if exceeded will disable the reverse direction.
1287 */
1288 public void configSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition) throws CANTimeoutException {
1289 byte[] dataBuffer = new byte[8];
1290 byte dataSize;
1291
1292 dataSize = packFXP16_16(dataBuffer, forwardLimitPosition);
1293 dataBuffer[dataSize++] = (forwardLimitPosition > reverseLimitPosition) ? (byte) 1 : (byte) 0;
1294 setTransaction(JaguarCANProtocol.LM_API_CFG_LIMIT_FWD, dataBuffer, dataSize);
1295
1296 dataSize = packFXP16_16(dataBuffer, reverseLimitPosition);
1297 dataBuffer[dataSize++] = forwardLimitPosition <= reverseLimitPosition ? (byte) 1 : (byte) 0;
1298 setTransaction(JaguarCANProtocol.LM_API_CFG_LIMIT_REV, dataBuffer, dataSize);
1299
1300 dataBuffer[0] = LimitMode.kSoftPositionLimit_val;
1301 setTransaction(JaguarCANProtocol.LM_API_CFG_LIMIT_MODE, dataBuffer, (byte) 1);
1302 }
1303
1304 /**
1305 * Disable Soft Position Limits if previously enabled.
1306 *
1307 * Soft Position Limits are disabled by default.
1308 */
1309 public void disableSoftPositionLimits() throws CANTimeoutException {
1310 byte[] dataBuffer = new byte[8];
1311
1312 dataBuffer[0] = LimitMode.kSwitchInputsOnly_val;
1313 setTransaction(JaguarCANProtocol.LM_API_CFG_LIMIT_MODE, dataBuffer, (byte) 1);
1314 }
1315
1316 /**
1317 * Configure the maximum voltage that the Jaguar will ever output.
1318 *
1319 * This can be used to limit the maximum output voltage in all modes so that
1320 * motors which cannot withstand full bus voltage can be used safely.
1321 *
1322 * @param voltage The maximum voltage output by the Jaguar.
1323 */
1324 public void configMaxOutputVoltage(double voltage) throws CANTimeoutException {
1325 byte[] dataBuffer = new byte[8];
1326 byte dataSize;
1327
1328 m_maxOutputVoltage = voltage;
1329 dataSize = packFXP8_8(dataBuffer, voltage);
1330 setTransaction(JaguarCANProtocol.LM_API_CFG_MAX_VOUT, dataBuffer, dataSize);
1331 }
1332
1333 /**
1334 * Configure how long the Jaguar waits in the case of a fault before resuming operation.
1335 *
1336 * Faults include over temerature, over current, and bus under voltage.
1337 * The default is 3.0 seconds, but can be reduced to as low as 0.5 seconds.
1338 *
1339 * @param faultTime The time to wait before resuming operation, in seconds.
1340 */
1341 public void configFaultTime(double faultTime) throws CANTimeoutException {
1342 byte[] dataBuffer = new byte[8];
1343 byte dataSize;
1344
1345 // Message takes ms
1346 dataSize = packINT16(dataBuffer, (short) (faultTime * 1000.0));
1347 setTransaction(JaguarCANProtocol.LM_API_CFG_FAULT_TIME, dataBuffer, dataSize);
1348 }
1349
1350 /**
1351 * Update all the motors that have pending sets in the syncGroup.
1352 *
1353 * @param syncGroup A bitmask of groups to generate synchronous output.
1354 */
1355 public static void updateSyncGroup(byte syncGroup) throws CANTimeoutException {
1356 byte[] dataBuffer = new byte[8];
1357
1358 dataBuffer[0] = syncGroup;
1359 sendMessage(JaguarCANProtocol.CAN_MSGID_API_SYNC, dataBuffer, 1);
1360 }
1361
1362
1363 public void setExpiration(double timeout) {
1364 m_safetyHelper.setExpiration(timeout);
1365 }
1366
1367 public double getExpiration() {
1368 return m_safetyHelper.getExpiration();
1369 }
1370
1371 public boolean isAlive() {
1372 return m_safetyHelper.isAlive();
1373 }
1374
1375 public boolean isSafetyEnabled() {
1376 return m_safetyHelper.isSafetyEnabled();
1377 }
1378
1379 public void setSafetyEnabled(boolean enabled) {
1380 m_safetyHelper.setSafetyEnabled(enabled);
1381 }
1382
1383 public String getDescription() {
1384 return "CANJaguar ID "+m_deviceNumber;
1385 }
1386
1387 /**
1388 * Common interface for stopping a motor.
1389 *
1390 * @deprecated Use disableControl instead.
1391 */
1392 public void stopMotor() {
1393 try {
1394 disableControl();
1395 } catch (CANTimeoutException e) {}
1396 }
1397
1398 /*
1399 * Live Window code, only does anything if live window is activated.
1400 */
1401 public String getSmartDashboardType(){
1402 return "Speed Controller";
1403 }
1404 private ITable m_table;
1405 private ITableListener m_table_listener;
1406
1407 /**
1408 * {@inheritDoc}
1409 */
1410 public void initTable(ITable subtable) {
1411 m_table = subtable;
1412 updateTable();
1413 }
1414
1415 /**
1416 * {@inheritDoc}
1417 */
1418 public void updateTable() {
1419 if (m_table != null) {
1420 m_table.putNumber("Value", get());
1421 }
1422 }
1423
1424 /**
1425 * {@inheritDoc}
1426 */
1427 public ITable getTable(){
1428 return m_table;
1429 }
1430
1431 /**
1432 * {@inheritDoc}
1433 */
1434 public void startLiveWindowMode() {
1435 set(0); // Stop for safety
1436 m_table_listener = new ITableListener() {
1437 public void valueChanged(ITable itable, String key, Object value, boolean bln) {
1438 set(((Double) value).doubleValue());
1439 }
1440 };
1441 m_table.addTableListener("Value", m_table_listener, true);
1442 }
1443
1444 /**
1445 * {@inheritDoc}
1446 */
1447 public void stopLiveWindowMode() {
1448 set(0); // Stop for safety
1449 // TODO: Broken, should only remove the listener from "Value" only.
1450 m_table.removeTableListener(m_table_listener);
1451 }
1452 }