Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Updated CANJaguar.h/.cpp/.java for the RoboRIO

Browse Source 
Change-Id: I5134a78dc9dc945be1402c78d4fc4b9ef368b0b8
This commit is contained in:
thomasclark
2014-05-16 14:11:30 -04:00
parent ed6ac3bffd
commit da2e1769c0
3 changed files with 181 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpilibc/include/CANJaguar.h
View File

@@ -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;

124
wpilibc/lib/CANJaguar.cpp
View File

@@ -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 <stdio.h>
#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.
*/

111
wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java
View File

@@ -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.