From da2e1769c037227a4dfb2556672dc56e1d211387 Mon Sep 17 00:00:00 2001 From: thomasclark Date: Fri, 16 May 2014 14:11:30 -0400 Subject: [PATCH] Updated CANJaguar.h/.cpp/.java for the RoboRIO Change-Id: I5134a78dc9dc945be1402c78d4fc4b9ef368b0b8 --- wpilibc/include/CANJaguar.h | 5 +- wpilibc/lib/CANJaguar.cpp | 124 ++++++++++++++++-- .../java/edu/wpi/first/wpilibj/CANJaguar.java | 111 +++++++++------- 3 files changed, 181 insertions(+), 59 deletions(-) diff --git a/wpilibc/include/CANJaguar.h b/wpilibc/include/CANJaguar.h index 74d1cd24f4..6b61060a06 100644 --- a/wpilibc/include/CANJaguar.h +++ b/wpilibc/include/CANJaguar.h @@ -46,6 +46,7 @@ public: // SpeedController interface virtual float Get(); virtual void Set(float value, uint8_t syncGroup=0); + void SetNoAck(float value, uint8_t syncGroup=0); virtual void Disable(); // PIDOutput interface @@ -110,7 +111,9 @@ protected: virtual void getTransaction(uint32_t messageID, uint8_t *data, uint8_t *dataSize); static int32_t sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize); - static int32_t receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, float timeout = 0.02); + //#define kTimeoutCan 0.02 + #define kTimeoutCan 0.0 + static int32_t receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, float timeout = kTimeoutCan); uint8_t m_deviceNumber; ControlMode m_controlMode; diff --git a/wpilibc/lib/CANJaguar.cpp b/wpilibc/lib/CANJaguar.cpp index fc209b47d3..5f4bb56fda 100644 --- a/wpilibc/lib/CANJaguar.cpp +++ b/wpilibc/lib/CANJaguar.cpp @@ -6,25 +6,42 @@ #include "CANJaguar.h" #define tNIRIO_i32 int -//#include "CAN/JaguarCANDriver.h" +#include "CAN/JaguarCANDriver.h" #include "CAN/can_proto.h" //#include "NetworkCommunication/UsageReporting.h" #include "WPIErrors.h" #include #include "LiveWindow/LiveWindow.h" -#define swap16(x) ( (((x)>>8) &0x00FF) \ - | (((x)<<8) &0xFF00) ) -#define swap32(x) ( (((x)>>24)&0x000000FF) \ - | (((x)>>8) &0x0000FF00) \ - | (((x)<<8) &0x00FF0000) \ - | (((x)<<24)&0xFF000000) ) +/* we are on ARM now, not Freescale so no need to swap */ +#define swap16(x) (x) +#define swap32(x) (x) +//#define swap16(x) ( (((x)>>8) &0x00FF) \ +// | (((x)<<8) &0xFF00) ) +//#define swap32(x) ( (((x)>>24)&0x000000FF) \ +// | (((x)>>8) &0x0000FF00) \ +// | (((x)<<8) &0x00FF0000) \ +// | (((x)<<24)&0xFF000000) ) #define kFullMessageIDMask (CAN_MSGID_API_M | CAN_MSGID_MFR_M | CAN_MSGID_DTYPE_M) const int32_t CANJaguar::kControllerRate; constexpr double CANJaguar::kApproxBusVoltage; + + +void _sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t *status) +{ + FRC_NetworkCommunication_JaguarCANDriver_sendMessage(messageID,data,dataSize,status); +} +void _receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, uint32_t timeoutMs, int32_t *status) +{ + FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(messageID,data,dataSize,timeoutMs,status); + +} + + + /** * Common initialization code called by all constructors. */ @@ -165,6 +182,90 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) setTransaction(messageID, dataBuffer, dataSize); if (m_safetyHelper) m_safetyHelper->Feed(); } +void CANJaguar::SetNoAck(float outputValue, uint8_t syncGroup) +{ + uint32_t messageID; + uint8_t dataBuffer[8]; + uint8_t dataSize; + + if (m_safetyHelper && !m_safetyHelper->IsAlive()) + { + EnableControl(); + } + + switch(m_controlMode) + { + case kPercentVbus: + { + messageID = LM_API_VOLT_T_SET_NO_ACK; //LM_API_VOLT_T_SET; + if (outputValue > 1.0) outputValue = 1.0; + if (outputValue < -1.0) outputValue = -1.0; + dataSize = packPercentage(dataBuffer, outputValue); + } + break; + case kSpeed: + { + messageID = LM_API_SPD_T_SET; + dataSize = packFXP16_16(dataBuffer, outputValue); + } + break; + case kPosition: + { + messageID = LM_API_POS_T_SET; + dataSize = packFXP16_16(dataBuffer, outputValue); + } + break; + case kCurrent: + { + messageID = LM_API_ICTRL_T_SET; + dataSize = packFXP8_8(dataBuffer, outputValue); + } + break; + case kVoltage: + { + messageID = LM_API_VCOMP_T_SET_NO_ACK; // LM_API_VCOMP_T_SET; + dataSize = packFXP8_8(dataBuffer, outputValue); + } + break; + default: + return; + } + if (syncGroup != 0) + { + dataBuffer[dataSize] = syncGroup; + dataSize++; + } + { //setTransaction(messageID, dataBuffer, dataSize); + + //uint32_t ackMessageID = LM_API_ACK | m_deviceNumber; + int32_t localStatus = 0; + + // If there was an error on this object and it wasn't a timeout, refuse to talk to the device + // Call ClearError() on the object to try again + //if (StatusIsFatal() && GetError().GetCode() != -44087) + // return; + + // Make sure we don't have more than one transaction with the same Jaguar outstanding. + takeMutex(m_transactionSemaphore); + + // Throw away any stale acks. + //receiveMessage(&ackMessageID, NULL, 0, 0.0f); + + // Send the message with the data. + localStatus = sendMessage(messageID | m_deviceNumber, dataBuffer, dataSize); + wpi_setErrorWithContext(localStatus, "sendMessage"); + + // Wait for an ack. + //localStatus = receiveMessage(&ackMessageID, NULL, 0); + //wpi_setErrorWithContext(localStatus, "receiveMessage"); + + // Transaction complete. + giveMutex(m_transactionSemaphore); + } + + + if (m_safetyHelper) m_safetyHelper->Feed(); +} /** * Get the recently set outputValue setpoint. @@ -357,12 +458,12 @@ int32_t CANJaguar::sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataBuffer[j + 2] = data[j]; } //TODO: put this back when CAN shows up - // FRC_NetworkCommunication_JaguarCANDriver_sendMessage(messageID, dataBuffer, dataSize + 2, &status); + _sendMessage(messageID, dataBuffer, dataSize + 2, &status); return status; } } //TODO: put this back when CAN shows up - // FRC_NetworkCommunication_JaguarCANDriver_sendMessage(messageID, data, dataSize, &status); + _sendMessage(messageID, data, dataSize, &status); return status; } @@ -379,8 +480,8 @@ int32_t CANJaguar::receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *d { int32_t status = 0; //TODO: put this back when CAN shows up - // FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(messageID, data, dataSize, - // (uint32_t)(timeout * 1000), &status); + _receiveMessage(messageID, data, dataSize, + (uint32_t)(timeout * 1000), &status); return status; } @@ -1116,6 +1217,7 @@ void CANJaguar::ConfigPotentiometerTurns(uint16_t turns) * that are based on the position feedback. If the position limit is reached or the * switch is opened, that direction will be disabled. * + * @param forwardLimitPosition The position that if exceeded will disable the forward direction. * @param reverseLimitPosition The position that if exceeded will disable the reverse direction. */ diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java index 024fb810fa..49e6c669f0 100644 --- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java +++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java @@ -5,12 +5,16 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package edu.wpi.first.wpilibj; +package com.first.team190.robot; import java.nio.ByteBuffer; import java.nio.ByteOrder; import java.nio.IntBuffer; +import edu.wpi.first.wpilibj.MotorSafety; +import edu.wpi.first.wpilibj.MotorSafetyHelper; +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.can.CANExceptionFactory; import edu.wpi.first.wpilibj.can.CANJaguarVersionException; import edu.wpi.first.wpilibj.can.CANJNI; @@ -170,60 +174,20 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW private MotorSafetyHelper m_safetyHelper; private static final byte[] kNoData = new byte[0]; - private final static int swap16(int x) { - return ((((x) >>> 8) & 0x00FF) | (((x) << 8) & 0xFF00)); - } - - private final static long swap32(long x) { - return ((((x) >> 24) & 0x000000FF) | (((x) >> 8) & 0x0000FF00) | (((x) << 8) & 0x00FF0000) | (((x) << 24) & 0xFF000000)); - } - private final static int swap16(int x, byte[] buffer) { - buffer[0] = (byte) x; - buffer[1] = (byte) ((x >>> 8) & 0x00FF); + buffer[0] = (byte) ((x >>> 8) & 0x00FF); + buffer[1] = (byte) x; return ((((x) >>> 8) & 0x00FF) | (((x) << 8) & 0xFF00)); } private final static long swap32(long x, byte[] buffer) { - buffer[0] = (byte) x; - buffer[1] = (byte) ((x >>> 8) & 0x00FF); - buffer[2] = (byte) ((x >>> 16) & 0x00FF); - buffer[3] = (byte) ((x >>> 24) & 0x00FF); + buffer[0] = (byte) ((x >>> 24) & 0x00FF); + buffer[1] = (byte) ((x >>> 16) & 0x00FF); + buffer[2] = (byte) ((x >>> 8) & 0x00FF); + buffer[3] = (byte) x; return ((((x) >> 24) & 0x000000FF) | (((x) >> 8) & 0x0000FF00) | (((x) << 8) & 0x00FF0000) | (((x) << 24) & 0xFF000000)); } - private final static int swap16(byte[] buffer) { - return ((((buffer[1]) >>> 8) & 0x00FF) | (((buffer[0]) << 8) & 0xFF00)); - } - - private final static long swap32(byte[] buffer) { - return ((((buffer[3]) >> 24) & 0x000000FF) | (((buffer[2]) >> 8) & 0x0000FF00) | (((buffer[1]) << 8) & 0x00FF0000) | (((buffer[0]) << 24) & 0xFF000000)); - } - - /** - * Pack 16-bit data in little-endian byte order - * @param data The data to be packed - * @param buffer The buffer to pack into - * @param offset The offset into data to pack the variable - */ - private static final void pack16(short data, byte[] buffer, int offset) { - buffer[offset] = (byte) (data & 0xFF); - buffer[offset + 1] = (byte) ((data >>> 8) & 0xFF); - } - - /** - * Pack 32-bit data in little-endian byte order - * @param data The data to be packed - * @param buffer The buffer to pack into - * @param offset The offset into data to pack the variable - */ - private static final void pack32(int data, byte[] buffer, int offset) { - buffer[offset] = (byte) (data & 0xFF); - buffer[offset + 1] = (byte) ((data >>> 8) & 0xFF); - buffer[offset + 2] = (byte) ((data >>> 16) & 0xFF); - buffer[offset + 3] = (byte) ((data >>> 24) & 0xFF); - } - /** * Unpack 16-bit data from a buffer in little-endian byte order * @param buffer The buffer to unpack from @@ -459,6 +423,59 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW return 0.0; } + + public void setNoAck(float value, byte syncGroup) throws CANTimeoutException { + int messageID; + byte[] dataBuffer = new byte[8]; + byte dataSize = 0; + + if (!m_safetyHelper.isAlive()) { + enableControl(); + } + + switch (m_controlMode.value) { + case ControlMode.kPercentVbus_val: + messageID = CANJNI.LM_API_VOLT_T_SET_NO_ACK; + if (value > 1.0) value = 1.0f; + if (value < -1.0) value = -1.0f; + packPercentage(dataBuffer, value); + dataSize = 2; + break; + case ControlMode.kSpeed_val: { + messageID = CANJNI.LM_API_SPD_T_SET; + dataSize = packFXP16_16(dataBuffer, value); + } + break; + case ControlMode.kPosition_val: { + messageID = CANJNI.LM_API_POS_T_SET; + dataSize = packFXP16_16(dataBuffer, value); + } + break; + case ControlMode.kCurrent_val: { + messageID = CANJNI.LM_API_ICTRL_T_SET; + dataSize = packFXP8_8(dataBuffer, value); + } + break; + case ControlMode.kVoltage_val: { + messageID = CANJNI.LM_API_VCOMP_T_SET_NO_ACK; + dataSize = packFXP8_8(dataBuffer, value); + } + break; + default: + return; + } + + if (syncGroup != 0) { + dataBuffer[dataSize] = syncGroup; + dataSize++; + } + + synchronized(m_transactionMutex) { + sendMessage(messageID | m_deviceNumber, dataBuffer, dataSize); + } + + m_safetyHelper.feed(); + } /** * Get the recently set outputValue setpoint.