mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[hal, wpilib] Remove DMA (#7701)
This commit is contained in:
@@ -1,251 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.hal;
|
||||
|
||||
/**
|
||||
* DMA HAL JNI functions.
|
||||
*
|
||||
* @see "hal/DHA.h"
|
||||
*/
|
||||
public class DMAJNI extends JNIWrapper {
|
||||
/**
|
||||
* Initializes an object for performing DMA transfers.
|
||||
*
|
||||
* @return the created dma handle
|
||||
* @see "HAL_InitializeDMA"
|
||||
*/
|
||||
public static native int initialize();
|
||||
|
||||
/**
|
||||
* Frees a DMA object.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @see "HAL_FreeDMA"
|
||||
*/
|
||||
public static native void free(int handle);
|
||||
|
||||
/**
|
||||
* Pauses or unpauses a DMA transfer.
|
||||
*
|
||||
* <p>This can only be called while DMA is running.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param pause true to pause transfers, false to resume.
|
||||
* @see "HAL_SetDMAPause"
|
||||
*/
|
||||
public static native void setPause(int handle, boolean pause);
|
||||
|
||||
/**
|
||||
* Sets DMA transfers to occur at a specific timed interval.
|
||||
*
|
||||
* <p>This will remove any external triggers. Only timed or external is supported.
|
||||
*
|
||||
* <p>Only 1 timed period is supported.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param periodSeconds the period to trigger in seconds
|
||||
* @see "HAL_SetDMATimedTrigger"
|
||||
*/
|
||||
public static native void setTimedTrigger(int handle, double periodSeconds);
|
||||
|
||||
/**
|
||||
* Sets DMA transfers to occur at a specific timed interval in FPGA cycles.
|
||||
*
|
||||
* <p>This will remove any external triggers. Only timed or external is supported.
|
||||
*
|
||||
* <p>Only 1 timed period is supported
|
||||
*
|
||||
* <p>The FPGA currently runs at 40 MHz, but this can change.
|
||||
* HAL_GetSystemClockTicksPerMicrosecond can be used to get a computable value for this.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param cycles the period to trigger in FPGA cycles
|
||||
* @see "HAL_SetDMATimedTriggerCycles"
|
||||
*/
|
||||
public static native void setTimedTriggerCycles(int handle, int cycles);
|
||||
|
||||
/**
|
||||
* Adds position data for an encoder to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param encoderHandle the encoder to add
|
||||
* @see "HAL_AddDMAEncoder"
|
||||
*/
|
||||
public static native void addEncoder(int handle, int encoderHandle);
|
||||
|
||||
/**
|
||||
* Adds timer data for an encoder to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param encoderHandle the encoder to add
|
||||
* @see "HAL_AddDMAEncoderPeriod"
|
||||
*/
|
||||
public static native void addEncoderPeriod(int handle, int encoderHandle);
|
||||
|
||||
/**
|
||||
* Adds position data for an counter to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param counterHandle the counter to add
|
||||
* @see "HAL_AddDMACounter"
|
||||
*/
|
||||
public static native void addCounter(int handle, int counterHandle);
|
||||
|
||||
/**
|
||||
* Adds timer data for an counter to be collected by DMA.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param counterHandle the counter to add
|
||||
* @see "HAL_AddDMACounterPeriod"
|
||||
*/
|
||||
public static native void addCounterPeriod(int handle, int counterHandle);
|
||||
|
||||
/**
|
||||
* Adds a digital source to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param digitalSourceHandle the digital source to add
|
||||
* @see "HAL_AddDMADigitalSource"
|
||||
*/
|
||||
public static native void addDigitalSource(int handle, int digitalSourceHandle);
|
||||
|
||||
/**
|
||||
* Adds a duty cycle input to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param dutyCycleHandle the duty cycle input to add
|
||||
* @see "HAL_AddDMADutyCycle"
|
||||
*/
|
||||
public static native void addDutyCycle(int handle, int dutyCycleHandle);
|
||||
|
||||
/**
|
||||
* Adds an analog input to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param analogInputHandle the analog input to add
|
||||
* @see "HAL_AddDMAAnalogInput"
|
||||
*/
|
||||
public static native void addAnalogInput(int handle, int analogInputHandle);
|
||||
|
||||
/**
|
||||
* Adds averaged data of an analog input to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param analogInputHandle the analog input to add
|
||||
* @see "HAL_AddDMAAveragedAnalogInput"
|
||||
*/
|
||||
public static native void addAveragedAnalogInput(int handle, int analogInputHandle);
|
||||
|
||||
/**
|
||||
* Adds accumulator data of an analog input to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param analogInputHandle the analog input to add
|
||||
* @see "HAL_AddDMAAnalogAccumulator"
|
||||
*/
|
||||
public static native void addAnalogAccumulator(int handle, int analogInputHandle);
|
||||
|
||||
/**
|
||||
* Sets DMA transfers to occur on an external trigger.
|
||||
*
|
||||
* <p>This will remove any timed trigger set. Only timed or external is supported.
|
||||
*
|
||||
* <p>Up to 8 external triggers are currently supported.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param digitalSourceHandle the digital source handle (either a HAL_AnalogTriggerHandle or a
|
||||
* HAL_DigitalHandle)
|
||||
* @param analogTriggerType the analog trigger type if the source is an analog trigger
|
||||
* @param rising true to trigger on rising edge
|
||||
* @param falling true to trigger on falling edge
|
||||
* @return the index of the trigger
|
||||
* @see "HAL_SetDMAExternalTrigger"
|
||||
*/
|
||||
public static native int setExternalTrigger(
|
||||
int handle, int digitalSourceHandle, int analogTriggerType, boolean rising, boolean falling);
|
||||
|
||||
/**
|
||||
* Clear all sensors from the DMA collection list.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @see "HAL_ClearDMASensors"
|
||||
*/
|
||||
public static native void clearSensors(int handle);
|
||||
|
||||
/**
|
||||
* Clear all external triggers from the DMA trigger list.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @see "HAL_ClearDMAExternalTriggers"
|
||||
*/
|
||||
public static native void clearExternalTriggers(int handle);
|
||||
|
||||
/**
|
||||
* Starts DMA Collection.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param queueDepth the number of objects to be able to queue
|
||||
* @see "HAL_StartDMA"
|
||||
*/
|
||||
public static native void startDMA(int handle, int queueDepth);
|
||||
|
||||
/**
|
||||
* Stops DMA Collection.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @see "HAL_StopDMA"
|
||||
*/
|
||||
public static native void stopDMA(int handle);
|
||||
|
||||
/**
|
||||
* Reads a DMA sample from the queue.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @param timeoutSeconds the time to wait for data to be queued before timing out
|
||||
* @param buffer the sample object to place data into
|
||||
* @param sampleStore index 0-21 channelOffsets, index 22: capture size, index 23: triggerChannels
|
||||
* (bitflags), index 24: remaining, index 25: read status
|
||||
* @return timestamp of the DMA Sample
|
||||
*/
|
||||
public static native long readDMA(
|
||||
int handle, double timeoutSeconds, int[] buffer, int[] sampleStore);
|
||||
|
||||
/**
|
||||
* Get the sensor DMA sample.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
* @return The DMA sample
|
||||
*/
|
||||
public static native DMAJNISample.BaseStore getSensorReadData(int handle);
|
||||
|
||||
/** Utility class. */
|
||||
private DMAJNI() {}
|
||||
}
|
||||
@@ -1,147 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.hal;
|
||||
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
|
||||
public class DMAJNISample {
|
||||
private static final int kEnable_Accumulator0 = 8;
|
||||
private static final int kEnable_Accumulator1 = 9;
|
||||
|
||||
static class BaseStore {
|
||||
public final int m_valueType;
|
||||
public final int m_index;
|
||||
|
||||
BaseStore(int valueType, int index) {
|
||||
this.m_valueType = valueType;
|
||||
this.m_index = index;
|
||||
}
|
||||
}
|
||||
|
||||
private final int[] m_dataBuffer = new int[100];
|
||||
private final int[] m_storage = new int[100];
|
||||
private long m_timeStamp;
|
||||
private Map<Integer, BaseStore> m_propertyMap = new HashMap<>();
|
||||
|
||||
/** Default constructor. */
|
||||
public DMAJNISample() {}
|
||||
|
||||
public int update(int dmaHandle, double timeoutSeconds) {
|
||||
m_timeStamp = DMAJNI.readDMA(dmaHandle, timeoutSeconds, m_dataBuffer, m_storage);
|
||||
return m_storage[25];
|
||||
}
|
||||
|
||||
public int getCaptureSize() {
|
||||
return m_storage[22];
|
||||
}
|
||||
|
||||
public int getTriggerChannels() {
|
||||
return m_storage[23];
|
||||
}
|
||||
|
||||
public int getRemaining() {
|
||||
return m_storage[24];
|
||||
}
|
||||
|
||||
public long getTime() {
|
||||
return m_timeStamp;
|
||||
}
|
||||
|
||||
private BaseStore addSensorInternal(int handle) {
|
||||
BaseStore sensorData = DMAJNI.getSensorReadData(handle);
|
||||
m_propertyMap.put(handle, sensorData);
|
||||
return sensorData;
|
||||
}
|
||||
|
||||
public void addSensor(int handle) {
|
||||
addSensorInternal(handle);
|
||||
}
|
||||
|
||||
private int readValue(int valueType, int index) {
|
||||
int offset = m_storage[valueType];
|
||||
if (offset == -1) {
|
||||
throw new RuntimeException("Resource not found in DMA capture");
|
||||
}
|
||||
return m_dataBuffer[offset + index];
|
||||
}
|
||||
|
||||
public int getEncoder(int encoderHandle) {
|
||||
BaseStore data = m_propertyMap.get(encoderHandle);
|
||||
if (data == null) {
|
||||
data = addSensorInternal(encoderHandle);
|
||||
}
|
||||
return readValue(data.m_valueType, data.m_index);
|
||||
}
|
||||
|
||||
public int getEncoderPeriod(int encoderHandle) {
|
||||
BaseStore data = m_propertyMap.get(encoderHandle);
|
||||
if (data == null) {
|
||||
data = addSensorInternal(encoderHandle);
|
||||
}
|
||||
// + 2 Hack, but needed to not have to call into JNI
|
||||
return readValue(data.m_valueType + 2, data.m_index);
|
||||
}
|
||||
|
||||
public int getCounter(int counterHandle) {
|
||||
BaseStore data = m_propertyMap.get(counterHandle);
|
||||
if (data == null) {
|
||||
data = addSensorInternal(counterHandle);
|
||||
}
|
||||
return readValue(data.m_valueType, data.m_index);
|
||||
}
|
||||
|
||||
public int getCounterPeriod(int counterHandle) {
|
||||
BaseStore data = m_propertyMap.get(counterHandle);
|
||||
if (data == null) {
|
||||
data = addSensorInternal(counterHandle);
|
||||
}
|
||||
// Hack, but needed to not have to call into JNI
|
||||
return readValue(data.m_valueType + 2, data.m_index);
|
||||
}
|
||||
|
||||
public boolean getDigitalSource(int digitalSourceHandle) {
|
||||
BaseStore data = m_propertyMap.get(digitalSourceHandle);
|
||||
if (data == null) {
|
||||
data = addSensorInternal(digitalSourceHandle);
|
||||
}
|
||||
|
||||
int value = readValue(data.m_valueType, 0);
|
||||
|
||||
return ((value >> data.m_index) & 0x1) != 0;
|
||||
}
|
||||
|
||||
public int getAnalogInput(int analogInputHandle) {
|
||||
BaseStore data = m_propertyMap.get(analogInputHandle);
|
||||
if (data == null) {
|
||||
data = addSensorInternal(analogInputHandle);
|
||||
}
|
||||
|
||||
int value = readValue(data.m_valueType, data.m_index / 2);
|
||||
if ((data.m_index % 2) != 0) {
|
||||
return (value >>> 16) & 0xFFFF;
|
||||
} else {
|
||||
return value & 0xFFFF;
|
||||
}
|
||||
}
|
||||
|
||||
public int getAnalogInputAveraged(int analogInputHandle) {
|
||||
BaseStore data = m_propertyMap.get(analogInputHandle);
|
||||
if (data == null) {
|
||||
data = addSensorInternal(analogInputHandle);
|
||||
}
|
||||
|
||||
// + 2 Hack, but needed to not have to call into JNI
|
||||
return readValue(data.m_valueType + 2, data.m_index);
|
||||
}
|
||||
|
||||
public int getDutyCycleOutput(int dutyCycleHandle) {
|
||||
BaseStore data = m_propertyMap.get(dutyCycleHandle);
|
||||
if (data == null) {
|
||||
data = addSensorInternal(dutyCycleHandle);
|
||||
}
|
||||
return readValue(data.m_valueType, data.m_index);
|
||||
}
|
||||
}
|
||||
@@ -1,418 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
|
||||
#include <wpi/jni_util.h>
|
||||
|
||||
#include "HALUtil.h"
|
||||
#include "edu_wpi_first_hal_DMAJNI.h"
|
||||
#include "hal/DMA.h"
|
||||
#include "hal/handles/HandlesInternal.h"
|
||||
|
||||
using namespace hal;
|
||||
using namespace wpi::java;
|
||||
|
||||
namespace hal {
|
||||
bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
|
||||
HAL_FPGAEncoderHandle* fpgaEncoderHandle,
|
||||
HAL_CounterHandle* counterHandle);
|
||||
} // namespace hal
|
||||
|
||||
extern "C" {
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: initialize
|
||||
* Signature: ()I
|
||||
*/
|
||||
JNIEXPORT jint JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_initialize
|
||||
(JNIEnv* env, jclass)
|
||||
{
|
||||
int32_t status = 0;
|
||||
auto handle = HAL_InitializeDMA(&status);
|
||||
CheckStatusForceThrow(env, status);
|
||||
return handle;
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: free
|
||||
* Signature: (I)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_free
|
||||
(JNIEnv* env, jclass, jint handle)
|
||||
{
|
||||
if (handle != HAL_kInvalidHandle) {
|
||||
HAL_FreeDMA(handle);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: setPause
|
||||
* Signature: (IZ)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_setPause
|
||||
(JNIEnv* env, jclass, jint handle, jboolean pause)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_SetDMAPause(handle, pause, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: setTimedTrigger
|
||||
* Signature: (ID)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_setTimedTrigger
|
||||
(JNIEnv* env, jclass, jint handle, jdouble seconds)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_SetDMATimedTrigger(handle, seconds, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: setTimedTriggerCycles
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_setTimedTriggerCycles
|
||||
(JNIEnv* env, jclass, jint handle, jint cycles)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_SetDMATimedTriggerCycles(handle, static_cast<uint32_t>(cycles), &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: addEncoder
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_addEncoder
|
||||
(JNIEnv* env, jclass, jint handle, jint encoderHandle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAEncoder(handle, encoderHandle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: addEncoderPeriod
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_addEncoderPeriod
|
||||
(JNIEnv* env, jclass, jint handle, jint encoderHandle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAEncoderPeriod(handle, encoderHandle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: addCounter
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_addCounter
|
||||
(JNIEnv* env, jclass, jint handle, jint counterHandle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_AddDMACounter(handle, counterHandle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: addCounterPeriod
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_addCounterPeriod
|
||||
(JNIEnv* env, jclass, jint handle, jint counterHandle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_AddDMACounterPeriod(handle, counterHandle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: addDutyCycle
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_addDutyCycle
|
||||
(JNIEnv* env, jclass, jint handle, jint dutyCycleHandle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_AddDMADutyCycle(handle, dutyCycleHandle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: addDigitalSource
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_addDigitalSource
|
||||
(JNIEnv* env, jclass, jint handle, jint digitalSourceHandle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_AddDMADigitalSource(handle, digitalSourceHandle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: addAnalogInput
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_addAnalogInput
|
||||
(JNIEnv* env, jclass, jint handle, jint analogInputHandle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAAnalogInput(handle, analogInputHandle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: addAveragedAnalogInput
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_addAveragedAnalogInput
|
||||
(JNIEnv* env, jclass, jint handle, jint analogInputHandle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAAveragedAnalogInput(handle, analogInputHandle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: addAnalogAccumulator
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_addAnalogAccumulator
|
||||
(JNIEnv* env, jclass, jint handle, jint analogInputHandle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAAnalogAccumulator(handle, analogInputHandle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: setExternalTrigger
|
||||
* Signature: (IIIZZ)I
|
||||
*/
|
||||
JNIEXPORT jint JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_setExternalTrigger
|
||||
(JNIEnv* env, jclass, jint handle, jint digitalSourceHandle,
|
||||
jint analogTriggerType, jboolean rising, jboolean falling)
|
||||
{
|
||||
int32_t status = 0;
|
||||
int32_t idx = HAL_SetDMAExternalTrigger(
|
||||
handle, digitalSourceHandle,
|
||||
static_cast<HAL_AnalogTriggerType>(analogTriggerType), rising, falling,
|
||||
&status);
|
||||
CheckStatus(env, status);
|
||||
return idx;
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: clearSensors
|
||||
* Signature: (I)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_clearSensors
|
||||
(JNIEnv* env, jclass, jint handle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_ClearDMASensors(handle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: clearExternalTriggers
|
||||
* Signature: (I)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_clearExternalTriggers
|
||||
(JNIEnv* env, jclass, jint handle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_ClearDMAExternalTriggers(handle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: startDMA
|
||||
* Signature: (II)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_startDMA
|
||||
(JNIEnv* env, jclass, jint handle, jint queueDepth)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_StartDMA(handle, queueDepth, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: stopDMA
|
||||
* Signature: (I)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_stopDMA
|
||||
(JNIEnv* env, jclass, jint handle)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_StopDMA(handle, &status);
|
||||
CheckStatus(env, status);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: readDMA
|
||||
* Signature: (ID[I[I)J
|
||||
*/
|
||||
JNIEXPORT jlong JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_readDMA
|
||||
(JNIEnv* env, jclass, jint handle, jdouble timeoutSeconds, jintArray buf,
|
||||
jintArray store)
|
||||
{
|
||||
int32_t status = 0;
|
||||
HAL_DMASample dmaSample;
|
||||
std::memset(&dmaSample, 0, sizeof(dmaSample));
|
||||
int32_t remaining = 0;
|
||||
HAL_DMAReadStatus readStatus =
|
||||
HAL_ReadDMA(handle, &dmaSample, timeoutSeconds, &remaining, &status);
|
||||
CheckStatus(env, status);
|
||||
|
||||
static_assert(sizeof(uint32_t) == sizeof(jint), "Java ints must be 32 bits");
|
||||
|
||||
env->SetIntArrayRegion(buf, 0, dmaSample.captureSize,
|
||||
reinterpret_cast<jint*>(dmaSample.readBuffer));
|
||||
|
||||
CriticalJSpan<jint> nativeArr{env, store};
|
||||
|
||||
std::copy_n(
|
||||
dmaSample.channelOffsets,
|
||||
sizeof(dmaSample.channelOffsets) / sizeof(dmaSample.channelOffsets[0]),
|
||||
nativeArr.data());
|
||||
nativeArr[22] = static_cast<int32_t>(dmaSample.captureSize);
|
||||
nativeArr[23] = static_cast<int32_t>(dmaSample.triggerChannels);
|
||||
nativeArr[24] = remaining;
|
||||
nativeArr[25] = readStatus;
|
||||
|
||||
return dmaSample.timeStamp;
|
||||
}
|
||||
|
||||
// TODO sync these up
|
||||
enum DMAOffsetConstants {
|
||||
kEnable_AI0_Low = 0,
|
||||
kEnable_AI0_High = 1,
|
||||
kEnable_AIAveraged0_Low = 2,
|
||||
kEnable_AIAveraged0_High = 3,
|
||||
kEnable_AI1_Low = 4,
|
||||
kEnable_AI1_High = 5,
|
||||
kEnable_AIAveraged1_Low = 6,
|
||||
kEnable_AIAveraged1_High = 7,
|
||||
kEnable_Accumulator0 = 8,
|
||||
kEnable_Accumulator1 = 9,
|
||||
kEnable_DI = 10,
|
||||
kEnable_AnalogTriggers = 11,
|
||||
kEnable_Counters_Low = 12,
|
||||
kEnable_Counters_High = 13,
|
||||
kEnable_CounterTimers_Low = 14,
|
||||
kEnable_CounterTimers_High = 15,
|
||||
kEnable_Encoders_Low = 16,
|
||||
kEnable_Encoders_High = 17,
|
||||
kEnable_EncoderTimers_Low = 18,
|
||||
kEnable_EncoderTimers_High = 19,
|
||||
kEnable_DutyCycle_Low = 20,
|
||||
kEnable_DutyCycle_High = 21,
|
||||
};
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_hal_DMAJNI
|
||||
* Method: getSensorReadData
|
||||
* Signature: (I)Ljava/lang/Object;
|
||||
*/
|
||||
JNIEXPORT jobject JNICALL
|
||||
Java_edu_wpi_first_hal_DMAJNI_getSensorReadData
|
||||
(JNIEnv* env, jclass, jint handle)
|
||||
{
|
||||
HAL_Handle halHandle = static_cast<HAL_Handle>(handle);
|
||||
|
||||
// Check for encoder/counter handle
|
||||
HAL_FPGAEncoderHandle fpgaEncoderHandle = 0;
|
||||
HAL_CounterHandle counterHandle = 0;
|
||||
bool validEncoderHandle =
|
||||
hal::GetEncoderBaseHandle(halHandle, &fpgaEncoderHandle, &counterHandle);
|
||||
|
||||
if (validEncoderHandle) {
|
||||
if (counterHandle != HAL_kInvalidHandle) {
|
||||
int32_t cindex = getHandleIndex(counterHandle);
|
||||
if (cindex < 4) {
|
||||
return CreateDMABaseStore(env, kEnable_Counters_Low, cindex);
|
||||
} else {
|
||||
return CreateDMABaseStore(env, kEnable_Counters_High, cindex - 4);
|
||||
}
|
||||
} else {
|
||||
int32_t cindex = getHandleIndex(fpgaEncoderHandle);
|
||||
if (cindex < 4) {
|
||||
return CreateDMABaseStore(env, kEnable_Encoders_Low, cindex);
|
||||
} else {
|
||||
return CreateDMABaseStore(env, kEnable_Encoders_High, cindex - 4);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
HAL_HandleEnum handleType = getHandleType(halHandle);
|
||||
int32_t index = getHandleIndex(halHandle);
|
||||
if (handleType == HAL_HandleEnum::DIO) {
|
||||
return CreateDMABaseStore(env, kEnable_DI, index);
|
||||
} else if (handleType == HAL_HandleEnum::AnalogTrigger) {
|
||||
return CreateDMABaseStore(env, kEnable_AnalogTriggers, index);
|
||||
} else if (handleType == HAL_HandleEnum::AnalogInput) {
|
||||
if (index < 4) {
|
||||
return CreateDMABaseStore(env, kEnable_AI0_Low, index);
|
||||
} else {
|
||||
return CreateDMABaseStore(env, kEnable_AI0_High, index - 4);
|
||||
}
|
||||
} else if (handleType == HAL_HandleEnum::DutyCycle) {
|
||||
if (index < 4) {
|
||||
return CreateDMABaseStore(env, kEnable_DutyCycle_Low, index);
|
||||
} else {
|
||||
return CreateDMABaseStore(env, kEnable_DutyCycle_High, index - 4);
|
||||
}
|
||||
} else {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
} // extern "C"
|
||||
@@ -54,7 +54,6 @@ static JClass matchInfoDataCls;
|
||||
static JClass canDataCls;
|
||||
static JClass canStreamMessageCls;
|
||||
static JClass halValueCls;
|
||||
static JClass baseStoreCls;
|
||||
static JClass revPHVersionCls;
|
||||
static JClass canStreamOverflowExCls;
|
||||
|
||||
@@ -67,7 +66,6 @@ static const JClassInit classes[] = {
|
||||
{"edu/wpi/first/hal/CANData", &canDataCls},
|
||||
{"edu/wpi/first/hal/CANStreamMessage", &canStreamMessageCls},
|
||||
{"edu/wpi/first/hal/HALValue", &halValueCls},
|
||||
{"edu/wpi/first/hal/DMAJNISample$BaseStore", &baseStoreCls},
|
||||
{"edu/wpi/first/hal/REVPHVersion", &revPHVersionCls},
|
||||
{"edu/wpi/first/hal/can/CANStreamOverflowException",
|
||||
&canStreamOverflowExCls}};
|
||||
@@ -353,11 +351,6 @@ jobject CreateHALValue(JNIEnv* env, const HAL_Value& value) {
|
||||
halValueCls, fromNative, static_cast<jint>(value.type), value1, value2);
|
||||
}
|
||||
|
||||
jobject CreateDMABaseStore(JNIEnv* env, jint valueType, jint index) {
|
||||
static jmethodID ctor = env->GetMethodID(baseStoreCls, "<init>", "(II)V");
|
||||
return env->NewObject(baseStoreCls, ctor, valueType, index);
|
||||
}
|
||||
|
||||
jobject CreatePowerDistributionVersion(JNIEnv* env, uint32_t firmwareMajor,
|
||||
uint32_t firmwareMinor,
|
||||
uint32_t firmwareFix,
|
||||
|
||||
@@ -84,8 +84,6 @@ jbyteArray SetCANStreamObject(JNIEnv* env, jobject canStreamData,
|
||||
|
||||
jobject CreateHALValue(JNIEnv* env, const HAL_Value& value);
|
||||
|
||||
jobject CreateDMABaseStore(JNIEnv* env, jint valueType, jint index);
|
||||
|
||||
jobject CreatePowerDistributionVersion(JNIEnv* env, uint32_t firmwareMajor,
|
||||
uint32_t firmwareMinor,
|
||||
uint32_t firmwareFix,
|
||||
|
||||
@@ -1,458 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "hal/AnalogTrigger.h"
|
||||
#include "hal/Types.h"
|
||||
|
||||
/**
|
||||
* @defgroup hal_dma DMA Functions
|
||||
* @ingroup hal_capi
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* The DMA Read Status.
|
||||
*/
|
||||
HAL_ENUM(HAL_DMAReadStatus) {
|
||||
HAL_DMA_OK = 1,
|
||||
HAL_DMA_TIMEOUT = 2,
|
||||
HAL_DMA_ERROR = 3,
|
||||
};
|
||||
|
||||
/**
|
||||
* Buffer for containing all DMA data for a specific sample.
|
||||
*/
|
||||
struct HAL_DMASample {
|
||||
uint32_t readBuffer[74];
|
||||
int32_t channelOffsets[22];
|
||||
uint64_t timeStamp;
|
||||
uint32_t captureSize;
|
||||
uint8_t triggerChannels;
|
||||
};
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Initializes an object for performing DMA transfers.
|
||||
*
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return the created dma handle
|
||||
*/
|
||||
HAL_DMAHandle HAL_InitializeDMA(int32_t* status);
|
||||
|
||||
/**
|
||||
* Frees a DMA object.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
*/
|
||||
void HAL_FreeDMA(HAL_DMAHandle handle);
|
||||
|
||||
/**
|
||||
* Pauses or unpauses a DMA transfer.
|
||||
*
|
||||
* This can only be called while DMA is running.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] pause true to pause transfers, false to resume.
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status);
|
||||
|
||||
/**
|
||||
* Sets DMA transfers to occur at a specific timed interval.
|
||||
*
|
||||
* This will remove any external triggers. Only timed or external is supported.
|
||||
*
|
||||
* Only 1 timed period is supported.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] periodSeconds the period to trigger in seconds
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_SetDMATimedTrigger(HAL_DMAHandle handle, double periodSeconds,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Sets DMA transfers to occur at a specific timed interval in FPGA cycles.
|
||||
*
|
||||
* This will remove any external triggers. Only timed or external is supported.
|
||||
*
|
||||
* Only 1 timed period is supported
|
||||
*
|
||||
* The FPGA currently runs at 40 MHz, but this can change.
|
||||
* HAL_GetSystemClockTicksPerMicrosecond can be used to get a computable value
|
||||
* for this.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] cycles the period to trigger in FPGA cycles
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_SetDMATimedTriggerCycles(HAL_DMAHandle handle, uint32_t cycles,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Adds position data for an encoder to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] encoderHandle the encoder to add
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Adds timer data for an encoder to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] encoderHandle the encoder to add
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle,
|
||||
HAL_EncoderHandle encoderHandle, int32_t* status);
|
||||
|
||||
/**
|
||||
* Adds position data for an counter to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] counterHandle the counter to add
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Adds timer data for an counter to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] counterHandle the counter to add
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_AddDMACounterPeriod(HAL_DMAHandle handle,
|
||||
HAL_CounterHandle counterHandle, int32_t* status);
|
||||
|
||||
/**
|
||||
* Adds a digital source to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] digitalSourceHandle the digital source to add
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_AddDMADigitalSource(HAL_DMAHandle handle,
|
||||
HAL_Handle digitalSourceHandle, int32_t* status);
|
||||
|
||||
/**
|
||||
* Adds an analog input to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] aInHandle the analog input to add
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_AddDMAAnalogInput(HAL_DMAHandle handle,
|
||||
HAL_AnalogInputHandle aInHandle, int32_t* status);
|
||||
|
||||
/**
|
||||
* Adds averaged data of an analog input to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] aInHandle the analog input to add
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Adds accumulator data of an analog input to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] aInHandle the analog input to add
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Adds a duty cycle input to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] dutyCycleHandle the duty cycle input to add
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_AddDMADutyCycle(HAL_DMAHandle handle,
|
||||
HAL_DutyCycleHandle dutyCycleHandle, int32_t* status);
|
||||
|
||||
/**
|
||||
* Sets DMA transfers to occur on an external trigger.
|
||||
*
|
||||
* This will remove any timed trigger set. Only timed or external is supported.
|
||||
*
|
||||
* Up to 8 external triggers are currently supported.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] digitalSourceHandle the digital source handle (either a
|
||||
* HAL_AnalogTriggerHandle or a
|
||||
* HAL_DigitalHandle)
|
||||
* @param[in] analogTriggerType the analog trigger type if the source is an
|
||||
* analog trigger
|
||||
* @param[in] rising true to trigger on rising edge
|
||||
* @param[in] falling true to trigger on falling edge
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return the index of the trigger
|
||||
*/
|
||||
int32_t HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
|
||||
HAL_Handle digitalSourceHandle,
|
||||
HAL_AnalogTriggerType analogTriggerType,
|
||||
HAL_Bool rising, HAL_Bool falling,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Clear all sensors from the DMA collection list.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_ClearDMASensors(HAL_DMAHandle handle, int32_t* status);
|
||||
|
||||
/**
|
||||
* Clear all external triggers from the DMA trigger list.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_ClearDMAExternalTriggers(HAL_DMAHandle handle, int32_t* status);
|
||||
|
||||
/**
|
||||
* Starts DMA Collection.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] queueDepth the number of objects to be able to queue
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status);
|
||||
|
||||
/**
|
||||
* Stops DMA Collection.
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status);
|
||||
|
||||
/**
|
||||
* Gets the direct pointer to the DMA object.
|
||||
*
|
||||
* This is only to be used if absolute maximum performance is required. This
|
||||
* will only be valid until the handle is freed.
|
||||
*
|
||||
* @param handle the dma handle
|
||||
*/
|
||||
void* HAL_GetDMADirectPointer(HAL_DMAHandle handle);
|
||||
|
||||
/**
|
||||
* Reads a DMA sample using a direct DMA pointer.
|
||||
*
|
||||
* See HAL_ReadDMA for full documentation.
|
||||
*
|
||||
* @param[in] dmaPointer direct DMA pointer
|
||||
* @param[in] dmaSample the sample object to place data into
|
||||
* @param[in] timeoutSeconds the time to wait for data to be queued before
|
||||
* timing out
|
||||
* @param[in] remainingOut the number of samples remaining in the queue
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
|
||||
HAL_DMASample* dmaSample,
|
||||
double timeoutSeconds,
|
||||
int32_t* remainingOut,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Reads a DMA sample from the queue.
|
||||
*
|
||||
*
|
||||
* @param[in] handle the dma handle
|
||||
* @param[in] dmaSample the sample object to place data into
|
||||
* @param[in] timeoutSeconds the time to wait for data to be queued before
|
||||
* timing out
|
||||
* @param[in] remainingOut the number of samples remaining in the queue
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return the success result of the sample read
|
||||
*/
|
||||
enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
|
||||
HAL_DMASample* dmaSample,
|
||||
double timeoutSeconds, int32_t* remainingOut,
|
||||
int32_t* status);
|
||||
|
||||
// The following are helper functions for reading data from samples
|
||||
|
||||
/**
|
||||
* Returns the timestamp of the sample.
|
||||
* This is in the same time domain as HAL_GetFPGATime().
|
||||
*
|
||||
* @param[in] dmaSample the sample to read from
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return timestamp in microseconds since FPGA Initialization
|
||||
*/
|
||||
uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status);
|
||||
|
||||
/**
|
||||
* Returns the raw distance data for an encoder from the sample.
|
||||
*
|
||||
* This can be scaled with DistancePerPulse and DecodingScaleFactor to match the
|
||||
* result of GetDistance()
|
||||
*
|
||||
* @param[in] dmaSample the sample to read from
|
||||
* @param[in] encoderHandle the encoder handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return raw encoder ticks
|
||||
*/
|
||||
int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_EncoderHandle encoderHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Returns the distance data for an counter from the sample.
|
||||
*
|
||||
* @param[in] dmaSample the sample to read from
|
||||
* @param[in] counterHandle the counter handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return counter ticks
|
||||
*/
|
||||
int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample,
|
||||
HAL_CounterHandle counterHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Returns the raw period data for an encoder from the sample.
|
||||
*
|
||||
* This can be scaled with DistancePerPulse and DecodingScaleFactor to match the
|
||||
* result of GetRate()
|
||||
*
|
||||
* @param[in] dmaSample the sample to read from
|
||||
* @param[in] encoderHandle the encoder handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return raw encoder period
|
||||
*/
|
||||
int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_EncoderHandle encoderHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Returns the period data for an counter from the sample.
|
||||
*
|
||||
* @param[in] dmaSample the sample to read from
|
||||
* @param[in] counterHandle the counter handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return counter period
|
||||
*/
|
||||
int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample,
|
||||
HAL_CounterHandle counterHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Returns the state of a digital source from the sample.
|
||||
*
|
||||
* @param[in] dmaSample the sample to read from
|
||||
* @param[in] dSourceHandle the digital source handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return digital source state
|
||||
*/
|
||||
HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample,
|
||||
HAL_Handle dSourceHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Returns the raw analog data for an analog input from the sample.
|
||||
*
|
||||
* This can be scaled with HAL_GetAnalogValueToVolts to match GetVoltage().
|
||||
*
|
||||
* @param[in] dmaSample the sample to read from
|
||||
* @param[in] aInHandle the analog input handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return raw analog data
|
||||
*/
|
||||
int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Returns the raw averaged analog data for an analog input from the sample.
|
||||
*
|
||||
* This can be scaled with HAL_GetAnalogValueToVolts to match
|
||||
* GetAveragedVoltage().
|
||||
*
|
||||
* @param[in] dmaSample the sample to read from
|
||||
* @param[in] aInHandle the analog input handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return raw averaged analog data
|
||||
*/
|
||||
int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Returns the analog accumulator data for an analog input from the sample.
|
||||
*
|
||||
* @param[in] dmaSample the sample to read from
|
||||
* @param[in] aInHandle the analog input handle
|
||||
* @param[in] count the accumulator count
|
||||
* @param[in] value the accumulator value
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
*/
|
||||
void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int64_t* count, int64_t* value,
|
||||
int32_t* status);
|
||||
|
||||
/**
|
||||
* Returns the raw duty cycle input ratio data from the sample.
|
||||
*
|
||||
* Use HAL_GetDutyCycleOutputScaleFactor to scale this to a percentage.
|
||||
*
|
||||
* @param[in] dmaSample the sample to read from
|
||||
* @param[in] dutyCycleHandle the duty cycle handle
|
||||
* @param[out] status Error status variable. 0 on success.
|
||||
* @return raw duty cycle input data
|
||||
*/
|
||||
int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_DutyCycleHandle dutyCycleHandle,
|
||||
int32_t* status);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} // extern "C"
|
||||
#endif
|
||||
/** @} */
|
||||
@@ -1,134 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "hal/DMA.h"
|
||||
|
||||
extern "C" {
|
||||
HAL_DMAHandle HAL_InitializeDMA(int32_t* status) {
|
||||
return HAL_kInvalidHandle;
|
||||
}
|
||||
void HAL_FreeDMA(HAL_DMAHandle handle) {}
|
||||
|
||||
void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status) {}
|
||||
void HAL_SetDMATimedTrigger(HAL_DMAHandle handle, double periodSeconds,
|
||||
int32_t* status) {}
|
||||
void HAL_SetDMATimedTriggerCycles(HAL_DMAHandle handle, uint32_t cycles,
|
||||
int32_t* status) {}
|
||||
|
||||
void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
|
||||
int32_t* status) {}
|
||||
void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle,
|
||||
HAL_EncoderHandle encoderHandle, int32_t* status) {
|
||||
}
|
||||
void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle,
|
||||
int32_t* status) {}
|
||||
void HAL_AddDMACounterPeriod(HAL_DMAHandle handle,
|
||||
HAL_CounterHandle counterHandle, int32_t* status) {
|
||||
}
|
||||
void HAL_AddDMADigitalSource(HAL_DMAHandle handle,
|
||||
HAL_Handle digitalSourceHandle, int32_t* status) {}
|
||||
void HAL_AddDMAAnalogInput(HAL_DMAHandle handle,
|
||||
HAL_AnalogInputHandle aInHandle, int32_t* status) {}
|
||||
|
||||
void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status) {}
|
||||
|
||||
void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status) {}
|
||||
|
||||
void HAL_AddDMADutyCycle(HAL_DMAHandle handle,
|
||||
HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
|
||||
}
|
||||
|
||||
int32_t HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
|
||||
HAL_Handle digitalSourceHandle,
|
||||
HAL_AnalogTriggerType analogTriggerType,
|
||||
HAL_Bool rising, HAL_Bool falling,
|
||||
int32_t* status) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
void HAL_ClearDMASensors(HAL_DMAHandle handle, int32_t* status) {}
|
||||
void HAL_ClearDMAExternalTriggers(HAL_DMAHandle handle, int32_t* status) {}
|
||||
|
||||
void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) {}
|
||||
void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status) {}
|
||||
|
||||
void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
|
||||
HAL_DMASample* dmaSample,
|
||||
double timeoutSeconds,
|
||||
int32_t* remainingOut,
|
||||
int32_t* status) {
|
||||
return HAL_DMA_ERROR;
|
||||
}
|
||||
|
||||
enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
|
||||
HAL_DMASample* dmaSample,
|
||||
double timeoutSeconds, int32_t* remainingOut,
|
||||
int32_t* status) {
|
||||
return HAL_DMA_ERROR;
|
||||
}
|
||||
|
||||
// Sampling Code
|
||||
uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_EncoderHandle encoderHandle,
|
||||
int32_t* status) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample,
|
||||
HAL_CounterHandle counterHandle,
|
||||
int32_t* status) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_EncoderHandle encoderHandle,
|
||||
int32_t* status) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample,
|
||||
HAL_CounterHandle counterHandle,
|
||||
int32_t* status) {
|
||||
return 0;
|
||||
}
|
||||
HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample,
|
||||
HAL_Handle dSourceHandle,
|
||||
int32_t* status) {
|
||||
return 0;
|
||||
}
|
||||
int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int64_t* count, int64_t* value,
|
||||
int32_t* status) {}
|
||||
|
||||
int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_DutyCycleHandle dutyCycleHandle,
|
||||
int32_t* status) {
|
||||
return 0;
|
||||
}
|
||||
} // extern "C"
|
||||
@@ -1,230 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "hal/DMA.h"
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
#include <cstring>
|
||||
#include <memory>
|
||||
#include <type_traits>
|
||||
|
||||
#include <wpi/print.h>
|
||||
|
||||
#include "PortsInternal.h"
|
||||
#include "hal/AnalogInput.h"
|
||||
#include "hal/Errors.h"
|
||||
#include "hal/HALBase.h"
|
||||
#include "hal/handles/HandlesInternal.h"
|
||||
#include "hal/handles/LimitedHandleResource.h"
|
||||
#include "hal/handles/UnlimitedHandleResource.h"
|
||||
|
||||
using namespace hal;
|
||||
|
||||
static_assert(std::is_standard_layout_v<HAL_DMASample>,
|
||||
"HAL_DMASample must have standard layout");
|
||||
|
||||
namespace hal::init {
|
||||
void InitializeDMA() {}
|
||||
} // namespace hal::init
|
||||
|
||||
extern "C" {
|
||||
|
||||
HAL_DMAHandle HAL_InitializeDMA(int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return HAL_kInvalidHandle;
|
||||
}
|
||||
|
||||
void HAL_FreeDMA(HAL_DMAHandle handle) {}
|
||||
|
||||
void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_SetDMATimedTrigger(HAL_DMAHandle handle, double seconds,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_SetDMATimedTriggerCycles(HAL_DMAHandle handle, uint32_t cycles,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle,
|
||||
HAL_EncoderHandle encoderHandle, int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_AddDMACounterPeriod(HAL_DMAHandle handle,
|
||||
HAL_CounterHandle counterHandle, int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_AddDMADigitalSource(HAL_DMAHandle handle,
|
||||
HAL_Handle digitalSourceHandle, int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_AddDMAAnalogInput(HAL_DMAHandle handle,
|
||||
HAL_AnalogInputHandle aInHandle, int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_AddDMADutyCycle(HAL_DMAHandle handle,
|
||||
HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
int32_t HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
|
||||
HAL_Handle digitalSourceHandle,
|
||||
HAL_AnalogTriggerType analogTriggerType,
|
||||
HAL_Bool rising, HAL_Bool falling,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void HAL_ClearDMASensors(HAL_DMAHandle handle, int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_ClearDMAExternalTriggers(HAL_DMAHandle handle, int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
|
||||
HAL_DMASample* dmaSample,
|
||||
double timeoutSeconds,
|
||||
int32_t* remainingOut,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return HAL_DMAReadStatus::HAL_DMA_ERROR;
|
||||
}
|
||||
|
||||
enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
|
||||
HAL_DMASample* dmaSample,
|
||||
double timeoutSeconds, int32_t* remainingOut,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return HAL_DMAReadStatus::HAL_DMA_ERROR;
|
||||
}
|
||||
|
||||
uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status) {
|
||||
return dmaSample->timeStamp;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_EncoderHandle encoderHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_EncoderHandle encoderHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample,
|
||||
HAL_CounterHandle counterHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample,
|
||||
HAL_CounterHandle counterHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample,
|
||||
HAL_Handle dSourceHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return false;
|
||||
}
|
||||
int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample,
|
||||
HAL_AnalogInputHandle aInHandle,
|
||||
int64_t* count, int64_t* value,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample,
|
||||
HAL_DutyCycleHandle dutyCycleHandle,
|
||||
int32_t* status) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return 0;
|
||||
}
|
||||
} // extern "C"
|
||||
@@ -57,7 +57,6 @@ void InitializeHAL() {
|
||||
InitializeConstants();
|
||||
InitializeCounter();
|
||||
InitializeDIO();
|
||||
InitializeDMA();
|
||||
InitializeDutyCycle();
|
||||
InitializeEncoder();
|
||||
InitializeFRCDriverStation();
|
||||
|
||||
@@ -29,7 +29,6 @@ extern void InitializeConstants();
|
||||
extern void InitializeCounter();
|
||||
extern void InitializeDigitalInternal();
|
||||
extern void InitializeDIO();
|
||||
extern void InitializeDMA();
|
||||
extern void InitializeDutyCycle();
|
||||
extern void InitializeEncoder();
|
||||
extern void InitializeFPGAEncoder();
|
||||
|
||||
@@ -72,7 +72,7 @@ def tagList = [
|
||||
"LQR", "Pose Estimator",
|
||||
|
||||
/* --- Hardware --- */
|
||||
"Analog", "Ultrasonic", "Gyro", "Pneumatics", "I2C", "Duty Cycle", "PDP", "DMA",
|
||||
"Analog", "Ultrasonic", "Gyro", "Pneumatics", "I2C", "Duty Cycle", "PDP",
|
||||
"AddressableLEDs", "HAL", "Encoder", "Smart Motor Controller", "Digital Input",
|
||||
"Digital Output",
|
||||
|
||||
|
||||
@@ -1,153 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "frc/DMA.h"
|
||||
|
||||
#include <frc/AnalogInput.h>
|
||||
#include <frc/Counter.h>
|
||||
#include <frc/DigitalSource.h>
|
||||
#include <frc/DutyCycle.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/PWM.h>
|
||||
#include <frc/motorcontrol/PWMMotorController.h>
|
||||
|
||||
#include <hal/DMA.h>
|
||||
#include <hal/HALBase.h>
|
||||
|
||||
#include "frc/Errors.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
DMA::DMA() {
|
||||
int32_t status = 0;
|
||||
dmaHandle = HAL_InitializeDMA(&status);
|
||||
FRC_CheckErrorStatus(status, "InitializeDMA");
|
||||
}
|
||||
|
||||
void DMA::SetPause(bool pause) {
|
||||
int32_t status = 0;
|
||||
HAL_SetDMAPause(dmaHandle, pause, &status);
|
||||
FRC_CheckErrorStatus(status, "SetPause");
|
||||
}
|
||||
|
||||
void DMA::SetTimedTrigger(units::second_t period) {
|
||||
int32_t status = 0;
|
||||
HAL_SetDMATimedTrigger(dmaHandle, period.value(), &status);
|
||||
FRC_CheckErrorStatus(status, "SetTimedTrigger");
|
||||
}
|
||||
|
||||
void DMA::SetTimedTriggerCycles(int cycles) {
|
||||
int32_t status = 0;
|
||||
HAL_SetDMATimedTriggerCycles(dmaHandle, cycles, &status);
|
||||
FRC_CheckErrorStatus(status, "SetTimedTriggerCycles");
|
||||
}
|
||||
|
||||
void DMA::AddEncoder(const Encoder* encoder) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAEncoder(dmaHandle, encoder->m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "AddEncoder");
|
||||
}
|
||||
|
||||
void DMA::AddEncoderPeriod(const Encoder* encoder) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAEncoderPeriod(dmaHandle, encoder->m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "AddEncoderPeriod");
|
||||
}
|
||||
|
||||
void DMA::AddCounter(const Counter* counter) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMACounter(dmaHandle, counter->m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "AddCounter");
|
||||
}
|
||||
|
||||
void DMA::AddCounterPeriod(const Counter* counter) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMACounterPeriod(dmaHandle, counter->m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "AddCounterPeriod");
|
||||
}
|
||||
|
||||
void DMA::AddDigitalSource(const DigitalSource* digitalSource) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMADigitalSource(dmaHandle, digitalSource->GetPortHandleForRouting(),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "AddDigitalSource");
|
||||
}
|
||||
|
||||
void DMA::AddDutyCycle(const DutyCycle* dutyCycle) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMADutyCycle(dmaHandle, dutyCycle->m_handle, &status);
|
||||
FRC_CheckErrorStatus(status, "AddDutyCycle");
|
||||
}
|
||||
|
||||
void DMA::AddAnalogInput(const AnalogInput* analogInput) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAAnalogInput(dmaHandle, analogInput->m_port, &status);
|
||||
FRC_CheckErrorStatus(status, "AddAnalogInput");
|
||||
}
|
||||
|
||||
void DMA::AddAveragedAnalogInput(const AnalogInput* analogInput) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAAveragedAnalogInput(dmaHandle, analogInput->m_port, &status);
|
||||
FRC_CheckErrorStatus(status, "AddAveragedAnalogInput");
|
||||
}
|
||||
|
||||
void DMA::AddAnalogAccumulator(const AnalogInput* analogInput) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAAnalogAccumulator(dmaHandle, analogInput->m_port, &status);
|
||||
FRC_CheckErrorStatus(status, "AddAnalogAccumulator");
|
||||
}
|
||||
|
||||
int DMA::SetExternalTrigger(DigitalSource* source, bool rising, bool falling) {
|
||||
int32_t status = 0;
|
||||
int32_t idx =
|
||||
HAL_SetDMAExternalTrigger(dmaHandle, source->GetPortHandleForRouting(),
|
||||
static_cast<HAL_AnalogTriggerType>(
|
||||
source->GetAnalogTriggerTypeForRouting()),
|
||||
rising, falling, &status);
|
||||
FRC_CheckErrorStatus(status, "SetExternalTrigger");
|
||||
return idx;
|
||||
}
|
||||
|
||||
int DMA::SetPwmEdgeTrigger(PWMMotorController* source, bool rising,
|
||||
bool falling) {
|
||||
int32_t status = 0;
|
||||
int32_t idx = HAL_SetDMAExternalTrigger(
|
||||
dmaHandle, source->GetPwm()->m_handle,
|
||||
HAL_AnalogTriggerType::HAL_Trigger_kInWindow, rising, falling, &status);
|
||||
FRC_CheckErrorStatus(status, "SetPWmEdgeTrigger");
|
||||
return idx;
|
||||
}
|
||||
|
||||
int DMA::SetPwmEdgeTrigger(PWM* source, bool rising, bool falling) {
|
||||
int32_t status = 0;
|
||||
int32_t idx = HAL_SetDMAExternalTrigger(
|
||||
dmaHandle, source->m_handle, HAL_AnalogTriggerType::HAL_Trigger_kInWindow,
|
||||
rising, falling, &status);
|
||||
FRC_CheckErrorStatus(status, "SetPWmEdgeTrigger");
|
||||
return idx;
|
||||
}
|
||||
|
||||
void DMA::ClearSensors() {
|
||||
int32_t status = 0;
|
||||
HAL_ClearDMASensors(dmaHandle, &status);
|
||||
FRC_CheckErrorStatus(status, "ClearSensors");
|
||||
}
|
||||
|
||||
void DMA::ClearExternalTriggers() {
|
||||
int32_t status = 0;
|
||||
HAL_ClearDMAExternalTriggers(dmaHandle, &status);
|
||||
FRC_CheckErrorStatus(status, "ClearExternalTriggers");
|
||||
}
|
||||
|
||||
void DMA::Start(int queueDepth) {
|
||||
int32_t status = 0;
|
||||
HAL_StartDMA(dmaHandle, queueDepth, &status);
|
||||
FRC_CheckErrorStatus(status, "Start");
|
||||
}
|
||||
|
||||
void DMA::Stop() {
|
||||
int32_t status = 0;
|
||||
HAL_StopDMA(dmaHandle, &status);
|
||||
FRC_CheckErrorStatus(status, "Stop");
|
||||
}
|
||||
@@ -13,9 +13,6 @@
|
||||
|
||||
namespace frc {
|
||||
|
||||
class DMA;
|
||||
class DMASample;
|
||||
|
||||
/**
|
||||
* Analog input class.
|
||||
*
|
||||
@@ -31,8 +28,6 @@ class DMASample;
|
||||
class AnalogInput : public wpi::Sendable,
|
||||
public wpi::SendableHelper<AnalogInput> {
|
||||
friend class AnalogTrigger;
|
||||
friend class DMA;
|
||||
friend class DMASample;
|
||||
|
||||
public:
|
||||
/**
|
||||
|
||||
@@ -18,8 +18,6 @@
|
||||
namespace frc {
|
||||
|
||||
class DigitalGlitchFilter;
|
||||
class DMA;
|
||||
class DMASample;
|
||||
|
||||
/**
|
||||
* Class for counting the number of ticks on a digital input channel.
|
||||
@@ -34,9 +32,6 @@ class DMASample;
|
||||
class Counter : public CounterBase,
|
||||
public wpi::Sendable,
|
||||
public wpi::SendableHelper<Counter> {
|
||||
friend class DMA;
|
||||
friend class DMASample;
|
||||
|
||||
public:
|
||||
enum Mode {
|
||||
kTwoPulse = 0,
|
||||
|
||||
@@ -1,193 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <hal/DMA.h>
|
||||
#include <units/time.h>
|
||||
|
||||
namespace frc {
|
||||
class Encoder;
|
||||
class Counter;
|
||||
class DigitalSource;
|
||||
class DutyCycle;
|
||||
class AnalogInput;
|
||||
class DMASample;
|
||||
class PWM;
|
||||
class PWMMotorController;
|
||||
|
||||
/**
|
||||
* Class for configuring Direct Memory Access (DMA) of FPGA inputs.
|
||||
*/
|
||||
class DMA {
|
||||
friend class DMASample;
|
||||
|
||||
public:
|
||||
DMA();
|
||||
|
||||
DMA& operator=(DMA&& other) = default;
|
||||
DMA(DMA&& other) = default;
|
||||
|
||||
/**
|
||||
* Sets whether DMA is paused.
|
||||
*
|
||||
* @param pause True pauses DMA.
|
||||
*/
|
||||
void SetPause(bool pause);
|
||||
|
||||
/**
|
||||
* Sets DMA to trigger at an interval.
|
||||
*
|
||||
* @param period Period at which to trigger DMA.
|
||||
*/
|
||||
void SetTimedTrigger(units::second_t period);
|
||||
|
||||
/**
|
||||
* Sets number of DMA cycles to trigger.
|
||||
*
|
||||
* @param cycles Number of cycles.
|
||||
*/
|
||||
void SetTimedTriggerCycles(int cycles);
|
||||
|
||||
/**
|
||||
* Adds position data for an encoder to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param encoder Encoder to add to DMA.
|
||||
*/
|
||||
void AddEncoder(const Encoder* encoder);
|
||||
|
||||
/**
|
||||
* Adds timer data for an encoder to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param encoder Encoder to add to DMA.
|
||||
*/
|
||||
void AddEncoderPeriod(const Encoder* encoder);
|
||||
|
||||
/**
|
||||
* Adds position data for an counter to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param counter Counter to add to DMA.
|
||||
*/
|
||||
void AddCounter(const Counter* counter);
|
||||
|
||||
/**
|
||||
* Adds timer data for an counter to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param counter Counter to add to DMA.
|
||||
*/
|
||||
void AddCounterPeriod(const Counter* counter);
|
||||
|
||||
/**
|
||||
* Adds a digital source to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param digitalSource DigitalSource to add to DMA.
|
||||
*/
|
||||
void AddDigitalSource(const DigitalSource* digitalSource);
|
||||
|
||||
/**
|
||||
* Adds a digital source to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param digitalSource DigitalSource to add to DMA.
|
||||
*/
|
||||
void AddDutyCycle(const DutyCycle* digitalSource);
|
||||
|
||||
/**
|
||||
* Adds an analog input to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param analogInput AnalogInput to add to DMA.
|
||||
*/
|
||||
void AddAnalogInput(const AnalogInput* analogInput);
|
||||
|
||||
/**
|
||||
* Adds averaged data of an analog input to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param analogInput AnalogInput to add to DMA.
|
||||
*/
|
||||
void AddAveragedAnalogInput(const AnalogInput* analogInput);
|
||||
|
||||
/**
|
||||
* Adds accumulator data of an analog input to be collected by DMA.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*
|
||||
* @param analogInput AnalogInput to add to DMA.
|
||||
*/
|
||||
void AddAnalogAccumulator(const AnalogInput* analogInput);
|
||||
|
||||
/**
|
||||
* Sets an external DMA trigger.
|
||||
*
|
||||
* @param source the source to trigger from.
|
||||
* @param rising trigger on rising edge.
|
||||
* @param falling trigger on falling edge.
|
||||
* @return the index of the trigger
|
||||
*/
|
||||
int SetExternalTrigger(DigitalSource* source, bool rising, bool falling);
|
||||
|
||||
/**
|
||||
* Sets a DMA PWM edge trigger.
|
||||
*
|
||||
* @param pwm the PWM to trigger from.
|
||||
* @param rising trigger on rising edge.
|
||||
* @param falling trigger on falling edge.
|
||||
* @return the index of the trigger
|
||||
*/
|
||||
int SetPwmEdgeTrigger(PWM* pwm, bool rising, bool falling);
|
||||
|
||||
/**
|
||||
* Sets a DMA PWMMotorController edge trigger.
|
||||
*
|
||||
* @param pwm the PWMMotorController to trigger from.
|
||||
* @param rising trigger on rising edge.
|
||||
* @param falling trigger on falling edge.
|
||||
* @return the index of the trigger
|
||||
*/
|
||||
int SetPwmEdgeTrigger(PWMMotorController* pwm, bool rising, bool falling);
|
||||
|
||||
/**
|
||||
* Clear all sensors from the DMA collection list.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*/
|
||||
void ClearSensors();
|
||||
|
||||
/**
|
||||
* Clear all external triggers from the DMA trigger list.
|
||||
*
|
||||
* This can only be called if DMA is not started.
|
||||
*/
|
||||
void ClearExternalTriggers();
|
||||
|
||||
/**
|
||||
* Starts DMA Collection.
|
||||
*
|
||||
* @param queueDepth The number of objects to be able to queue.
|
||||
*/
|
||||
void Start(int queueDepth);
|
||||
|
||||
/**
|
||||
* Stops DMA Collection.
|
||||
*/
|
||||
void Stop();
|
||||
|
||||
private:
|
||||
hal::Handle<HAL_DMAHandle, HAL_FreeDMA> dmaHandle;
|
||||
};
|
||||
} // namespace frc
|
||||
@@ -1,232 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <type_traits>
|
||||
|
||||
#include <hal/AnalogInput.h>
|
||||
#include <hal/DMA.h>
|
||||
#include <units/time.h>
|
||||
|
||||
#include "frc/AnalogInput.h"
|
||||
#include "frc/Counter.h"
|
||||
#include "frc/DMA.h"
|
||||
#include "frc/DutyCycle.h"
|
||||
#include "frc/Encoder.h"
|
||||
|
||||
namespace frc {
|
||||
/**
|
||||
* DMA sample.
|
||||
*/
|
||||
class DMASample : public HAL_DMASample {
|
||||
public:
|
||||
/**
|
||||
* DMA read status.
|
||||
*/
|
||||
enum class DMAReadStatus {
|
||||
/// OK status.
|
||||
kOk = HAL_DMA_OK,
|
||||
/// Timeout status.
|
||||
kTimeout = HAL_DMA_TIMEOUT,
|
||||
/// Error status.
|
||||
kError = HAL_DMA_ERROR
|
||||
};
|
||||
|
||||
/**
|
||||
* Retrieves a new DMA sample.
|
||||
*
|
||||
* @param dma DMA object.
|
||||
* @param timeout Timeout for retrieval.
|
||||
* @param remaining Number of remaining samples.
|
||||
* @param status DMA read status.
|
||||
*/
|
||||
DMAReadStatus Update(const DMA* dma, units::second_t timeout,
|
||||
int32_t* remaining, int32_t* status) {
|
||||
return static_cast<DMAReadStatus>(
|
||||
HAL_ReadDMA(dma->dmaHandle, this, timeout.value(), remaining, status));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the DMA sample time in microseconds.
|
||||
*
|
||||
* @return The DMA sample time in microseconds.
|
||||
*/
|
||||
uint64_t GetTime() const { return timeStamp; }
|
||||
|
||||
/**
|
||||
* Returns the DMA sample timestamp.
|
||||
*
|
||||
* @return The DMA sample timestamp.
|
||||
*/
|
||||
units::second_t GetTimeStamp() const {
|
||||
return units::second_t{static_cast<double>(GetTime()) * 1.0e-6};
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns raw encoder value from DMA.
|
||||
*
|
||||
* @param encoder Encoder used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Raw encoder value from DMA.
|
||||
*/
|
||||
int32_t GetEncoderRaw(const Encoder* encoder, int32_t* status) const {
|
||||
return HAL_GetDMASampleEncoderRaw(this, encoder->m_encoder, status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns encoder distance from DMA.
|
||||
*
|
||||
* @param encoder Encoder used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Encoder distance from DMA.
|
||||
*/
|
||||
double GetEncoderDistance(const Encoder* encoder, int32_t* status) const {
|
||||
double val = GetEncoderRaw(encoder, status);
|
||||
val *= encoder->DecodingScaleFactor();
|
||||
val *= encoder->GetDistancePerPulse();
|
||||
return val;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns raw encoder period from DMA.
|
||||
*
|
||||
* @param encoder Encoder used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Raw encoder period from DMA.
|
||||
*/
|
||||
int32_t GetEncoderPeriodRaw(const Encoder* encoder, int32_t* status) const {
|
||||
return HAL_GetDMASampleEncoderPeriodRaw(this, encoder->m_encoder, status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns counter value from DMA.
|
||||
*
|
||||
* @param counter Counter used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Counter value from DMA.
|
||||
*/
|
||||
int32_t GetCounter(const Counter* counter, int32_t* status) const {
|
||||
return HAL_GetDMASampleCounter(this, counter->m_counter, status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns counter period from DMA.
|
||||
*
|
||||
* @param counter Counter used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Counter period from DMA.
|
||||
*/
|
||||
int32_t GetCounterPeriod(const Counter* counter, int32_t* status) const {
|
||||
return HAL_GetDMASampleCounterPeriod(this, counter->m_counter, status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns digital source value from DMA.
|
||||
*
|
||||
* @param digitalSource DigitalSource used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return DigitalSource value from DMA.
|
||||
*/
|
||||
bool GetDigitalSource(const DigitalSource* digitalSource,
|
||||
int32_t* status) const {
|
||||
return HAL_GetDMASampleDigitalSource(
|
||||
this, digitalSource->GetPortHandleForRouting(), status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns raw analog input value from DMA.
|
||||
*
|
||||
* @param analogInput AnalogInput used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Raw analog input value from DMA.
|
||||
*/
|
||||
int32_t GetAnalogInputRaw(const AnalogInput* analogInput,
|
||||
int32_t* status) const {
|
||||
return HAL_GetDMASampleAnalogInputRaw(this, analogInput->m_port, status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns analog input voltage from DMA.
|
||||
*
|
||||
* @param analogInput AnalogInput used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Analog input voltage from DMA.
|
||||
*/
|
||||
double GetAnalogInputVoltage(const AnalogInput* analogInput,
|
||||
int32_t* status) {
|
||||
return HAL_GetAnalogValueToVolts(
|
||||
analogInput->m_port, GetAnalogInputRaw(analogInput, status), status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns averaged analog input raw value from DMA.
|
||||
*
|
||||
* @param analogInput AnalogInput used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Averaged analog input raw value from DMA.
|
||||
*/
|
||||
int32_t GetAveragedAnalogInputRaw(const AnalogInput* analogInput,
|
||||
int32_t* status) const {
|
||||
return HAL_GetDMASampleAveragedAnalogInputRaw(this, analogInput->m_port,
|
||||
status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns averaged analog input voltage from DMA.
|
||||
*
|
||||
* @param analogInput AnalogInput used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Averaged analog input voltage from DMA.
|
||||
*/
|
||||
double GetAveragedAnalogInputVoltage(const AnalogInput* analogInput,
|
||||
int32_t* status) {
|
||||
return HAL_GetAnalogValueToVolts(
|
||||
analogInput->m_port, GetAveragedAnalogInputRaw(analogInput, status),
|
||||
status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns analog accumulator value from DMA.
|
||||
*
|
||||
* @param analogInput AnalogInput used for DMA.
|
||||
* @param count Accumulator sample count.
|
||||
* @param value Accumulator value.
|
||||
* @param status DMA read status.
|
||||
*/
|
||||
void GetAnalogAccumulator(const AnalogInput* analogInput, int64_t* count,
|
||||
int64_t* value, int32_t* status) const {
|
||||
return HAL_GetDMASampleAnalogAccumulator(this, analogInput->m_port, count,
|
||||
value, status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns raw duty cycle output from DMA.
|
||||
*
|
||||
* @param dutyCycle DutyCycle used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Raw duty cycle output from DMA.
|
||||
*/
|
||||
int32_t GetDutyCycleOutputRaw(const DutyCycle* dutyCycle,
|
||||
int32_t* status) const {
|
||||
return HAL_GetDMASampleDutyCycleOutputRaw(this, dutyCycle->m_handle,
|
||||
status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns duty cycle output (0-1) from DMA.
|
||||
*
|
||||
* @param dutyCycle DutyCycle used for DMA.
|
||||
* @param status DMA read status.
|
||||
* @return Duty cycle output (0-1) from DMA.
|
||||
*/
|
||||
double GetDutyCycleOutput(const DutyCycle* dutyCycle, int32_t* status) {
|
||||
return GetDutyCycleOutputRaw(dutyCycle, status) /
|
||||
static_cast<double>(dutyCycle->GetOutputScaleFactor());
|
||||
}
|
||||
};
|
||||
|
||||
static_assert(std::is_standard_layout_v<frc::DMASample>,
|
||||
"frc::DMASample must have standard layout");
|
||||
} // namespace frc
|
||||
@@ -15,8 +15,6 @@
|
||||
namespace frc {
|
||||
class DigitalSource;
|
||||
class AnalogTrigger;
|
||||
class DMA;
|
||||
class DMASample;
|
||||
|
||||
/**
|
||||
* Class to read a duty cycle PWM input.
|
||||
@@ -31,8 +29,6 @@ class DMASample;
|
||||
*/
|
||||
class DutyCycle : public wpi::Sendable, public wpi::SendableHelper<DutyCycle> {
|
||||
friend class AnalogTrigger;
|
||||
friend class DMA;
|
||||
friend class DMASample;
|
||||
|
||||
public:
|
||||
/**
|
||||
|
||||
@@ -18,8 +18,6 @@ namespace frc {
|
||||
|
||||
class DigitalSource;
|
||||
class DigitalGlitchFilter;
|
||||
class DMA;
|
||||
class DMASample;
|
||||
|
||||
/**
|
||||
* Class to read quad encoders.
|
||||
@@ -39,9 +37,6 @@ class DMASample;
|
||||
class Encoder : public CounterBase,
|
||||
public wpi::Sendable,
|
||||
public wpi::SendableHelper<Encoder> {
|
||||
friend class DMA;
|
||||
friend class DMASample;
|
||||
|
||||
public:
|
||||
/**
|
||||
* Encoder indexing types.
|
||||
|
||||
@@ -14,7 +14,6 @@
|
||||
|
||||
namespace frc {
|
||||
class AddressableLED;
|
||||
class DMA;
|
||||
|
||||
/**
|
||||
* Class implements the PWM generation in the FPGA.
|
||||
@@ -27,7 +26,6 @@ class DMA;
|
||||
class PWM : public wpi::Sendable, public wpi::SendableHelper<PWM> {
|
||||
public:
|
||||
friend class AddressableLED;
|
||||
friend class DMA;
|
||||
/**
|
||||
* Represents the amount to multiply the minimum servo-pulse pwm period by.
|
||||
*/
|
||||
|
||||
@@ -22,7 +22,6 @@
|
||||
#include "frc/motorcontrol/MotorController.h"
|
||||
|
||||
namespace frc {
|
||||
class DMA;
|
||||
|
||||
WPI_IGNORE_DEPRECATED
|
||||
|
||||
@@ -34,8 +33,6 @@ class PWMMotorController : public MotorController,
|
||||
public wpi::Sendable,
|
||||
public wpi::SendableHelper<PWMMotorController> {
|
||||
public:
|
||||
friend class DMA;
|
||||
|
||||
PWMMotorController(PWMMotorController&&) = default;
|
||||
PWMMotorController& operator=(PWMMotorController&&) = default;
|
||||
|
||||
|
||||
@@ -1,84 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <frc/AnalogInput.h>
|
||||
#include <frc/DMA.h>
|
||||
#include <frc/DMASample.h>
|
||||
#include <frc/DigitalOutput.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
frc::DMA m_dma; // DMA object
|
||||
|
||||
// DMA needs a trigger, can use an output as trigger.
|
||||
// 8 Triggers exist per DMA object, can be triggered on any
|
||||
// DigitalSource.
|
||||
frc::DigitalOutput m_dmaTrigger{2};
|
||||
|
||||
// Analog input to read with DMA
|
||||
frc::AnalogInput m_analogInput{0};
|
||||
|
||||
// Encoder to read with DMA
|
||||
frc::Encoder m_encoder{0, 1};
|
||||
|
||||
public:
|
||||
Robot() {
|
||||
// Trigger on falling edge of dma trigger output
|
||||
m_dma.SetExternalTrigger(&m_dmaTrigger, false, true);
|
||||
|
||||
// Add inputs we want to read via DMA
|
||||
m_dma.AddAnalogInput(&m_analogInput);
|
||||
m_dma.AddEncoder(&m_encoder);
|
||||
m_dma.AddEncoderPeriod(&m_encoder);
|
||||
|
||||
// Make sure trigger is set to off.
|
||||
m_dmaTrigger.Set(true);
|
||||
|
||||
// Start DMA. No triggers or inputs can be added after this call
|
||||
// unless DMA is stopped.
|
||||
m_dma.Start(1024);
|
||||
}
|
||||
|
||||
void RobotPeriodic() override {
|
||||
// Manually Trigger DMA read
|
||||
m_dmaTrigger.Set(false);
|
||||
|
||||
// Need to create a sample.
|
||||
frc::DMASample sample;
|
||||
int32_t remaining = 0;
|
||||
int32_t status = 0;
|
||||
// Update our sample. remaining is the number of samples remaining in the
|
||||
// buffer status is more specific error messages if readStatus is not OK.
|
||||
// Wait 1ms if buffer is empty
|
||||
frc::DMASample::DMAReadStatus readStatus =
|
||||
sample.Update(&m_dma, 1_ms, &remaining, &status);
|
||||
|
||||
// Unset trigger
|
||||
m_dmaTrigger.Set(true);
|
||||
|
||||
if (readStatus == frc::DMASample::DMAReadStatus::kOk) {
|
||||
// Status value in all these reads should be checked, a non 0 value means
|
||||
// value could not be read
|
||||
|
||||
// If DMA is good, values exist
|
||||
auto encoderDistance = sample.GetEncoderDistance(&m_encoder, &status);
|
||||
// Period is not scaled, and is a raw value
|
||||
auto encoderPeriod = sample.GetEncoderPeriodRaw(&m_encoder, &status);
|
||||
auto analogVoltage =
|
||||
sample.GetAnalogInputVoltage(&m_analogInput, &status);
|
||||
|
||||
frc::SmartDashboard::PutNumber("Distance", encoderDistance);
|
||||
frc::SmartDashboard::PutNumber("Period", encoderPeriod);
|
||||
frc::SmartDashboard::PutNumber("Input", analogVoltage);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -503,18 +503,6 @@
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "DMA",
|
||||
"description": "Read various sensors using DMA.",
|
||||
"tags": [
|
||||
"Hardware",
|
||||
"DMA",
|
||||
"SmartDashboard"
|
||||
],
|
||||
"foldername": "DMA",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "MecanumControllerCommand",
|
||||
"description": "Follow a pre-generated trajectory with a mecanum drive using MecanumControllerCommand.",
|
||||
|
||||
@@ -1,222 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj;
|
||||
|
||||
import edu.wpi.first.hal.DMAJNI;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
|
||||
|
||||
/** Class for configuring Direct Memory Access (DMA) of FPGA inputs. */
|
||||
public class DMA implements AutoCloseable {
|
||||
final int m_dmaHandle;
|
||||
|
||||
/** Default constructor. */
|
||||
public DMA() {
|
||||
m_dmaHandle = DMAJNI.initialize();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void close() {
|
||||
DMAJNI.free(m_dmaHandle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets whether DMA is paused.
|
||||
*
|
||||
* @param pause True pauses DMA.
|
||||
*/
|
||||
public void setPause(boolean pause) {
|
||||
DMAJNI.setPause(m_dmaHandle, pause);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets DMA to trigger at an interval.
|
||||
*
|
||||
* @param periodSeconds Period at which to trigger DMA in seconds.
|
||||
*/
|
||||
public void setTimedTrigger(double periodSeconds) {
|
||||
DMAJNI.setTimedTrigger(m_dmaHandle, periodSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets number of DMA cycles to trigger.
|
||||
*
|
||||
* @param cycles Number of cycles.
|
||||
*/
|
||||
public void setTimedTriggerCycles(int cycles) {
|
||||
DMAJNI.setTimedTriggerCycles(m_dmaHandle, cycles);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds position data for an encoder to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param encoder Encoder to add to DMA.
|
||||
*/
|
||||
public void addEncoder(Encoder encoder) {
|
||||
DMAJNI.addEncoder(m_dmaHandle, encoder.m_encoder);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds timer data for an encoder to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param encoder Encoder to add to DMA.
|
||||
*/
|
||||
public void addEncoderPeriod(Encoder encoder) {
|
||||
DMAJNI.addEncoderPeriod(m_dmaHandle, encoder.m_encoder);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds position data for an counter to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param counter Counter to add to DMA.
|
||||
*/
|
||||
public void addCounter(Counter counter) {
|
||||
DMAJNI.addCounter(m_dmaHandle, counter.m_counter);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds timer data for an counter to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param counter Counter to add to DMA.
|
||||
*/
|
||||
public void addCounterPeriod(Counter counter) {
|
||||
DMAJNI.addCounterPeriod(m_dmaHandle, counter.m_counter);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds a digital source to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param digitalSource DigitalSource to add to DMA.
|
||||
*/
|
||||
public void addDigitalSource(DigitalSource digitalSource) {
|
||||
DMAJNI.addDigitalSource(m_dmaHandle, digitalSource.getPortHandleForRouting());
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds a duty cycle input to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param dutyCycle DutyCycle to add to DMA.
|
||||
*/
|
||||
public void addDutyCycle(DutyCycle dutyCycle) {
|
||||
DMAJNI.addDutyCycle(m_dmaHandle, dutyCycle.m_handle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds an analog input to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param analogInput AnalogInput to add to DMA.
|
||||
*/
|
||||
public void addAnalogInput(AnalogInput analogInput) {
|
||||
DMAJNI.addAnalogInput(m_dmaHandle, analogInput.m_port);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds averaged data of an analog input to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param analogInput AnalogInput to add to DMA.
|
||||
*/
|
||||
public void addAveragedAnalogInput(AnalogInput analogInput) {
|
||||
DMAJNI.addAveragedAnalogInput(m_dmaHandle, analogInput.m_port);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds accumulator data of an analog input to be collected by DMA.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*
|
||||
* @param analogInput AnalogInput to add to DMA.
|
||||
*/
|
||||
public void addAnalogAccumulator(AnalogInput analogInput) {
|
||||
DMAJNI.addAnalogAccumulator(m_dmaHandle, analogInput.m_port);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets an external DMA trigger.
|
||||
*
|
||||
* @param source the source to trigger from.
|
||||
* @param rising trigger on rising edge.
|
||||
* @param falling trigger on falling edge.
|
||||
* @return the index of the trigger
|
||||
*/
|
||||
public int setExternalTrigger(DigitalSource source, boolean rising, boolean falling) {
|
||||
return DMAJNI.setExternalTrigger(
|
||||
m_dmaHandle,
|
||||
source.getPortHandleForRouting(),
|
||||
source.getAnalogTriggerTypeForRouting(),
|
||||
rising,
|
||||
falling);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets a DMA PWM edge trigger.
|
||||
*
|
||||
* @param pwm the PWM to trigger from.
|
||||
* @param rising trigger on rising edge.
|
||||
* @param falling trigger on falling edge.
|
||||
* @return the index of the trigger
|
||||
*/
|
||||
public int setPwmEdgeTrigger(PWM pwm, boolean rising, boolean falling) {
|
||||
return DMAJNI.setExternalTrigger(m_dmaHandle, pwm.getHandle(), 0, rising, falling);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets a DMA PWMMotorController edge trigger.
|
||||
*
|
||||
* @param pwm the PWMMotorController to trigger from.
|
||||
* @param rising trigger on rising edge.
|
||||
* @param falling trigger on falling edge.
|
||||
* @return the index of the trigger
|
||||
*/
|
||||
public int setPwmEdgeTrigger(PWMMotorController pwm, boolean rising, boolean falling) {
|
||||
return DMAJNI.setExternalTrigger(m_dmaHandle, pwm.getPwmHandle(), 0, rising, falling);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clear all sensors from the DMA collection list.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*/
|
||||
public void clearSensors() {
|
||||
DMAJNI.clearSensors(m_dmaHandle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clear all external triggers from the DMA trigger list.
|
||||
*
|
||||
* <p>This can only be called if DMA is not started.
|
||||
*/
|
||||
public void clearExternalTriggers() {
|
||||
DMAJNI.clearExternalTriggers(m_dmaHandle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts DMA Collection.
|
||||
*
|
||||
* @param queueDepth The number of objects to be able to queue.
|
||||
*/
|
||||
public void start(int queueDepth) {
|
||||
DMAJNI.startDMA(m_dmaHandle, queueDepth);
|
||||
}
|
||||
|
||||
/** Stops DMA Collection. */
|
||||
public void stop() {
|
||||
DMAJNI.stopDMA(m_dmaHandle);
|
||||
}
|
||||
}
|
||||
@@ -1,237 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj;
|
||||
|
||||
import edu.wpi.first.hal.AnalogJNI;
|
||||
import edu.wpi.first.hal.DMAJNISample;
|
||||
|
||||
/** DMA sample. */
|
||||
public class DMASample {
|
||||
/** DMA read status. */
|
||||
public enum DMAReadStatus {
|
||||
/** OK status. */
|
||||
kOk(1),
|
||||
/** Timeout status. */
|
||||
kTimeout(2),
|
||||
/** Error status. */
|
||||
kError(3);
|
||||
|
||||
private final int value;
|
||||
|
||||
DMAReadStatus(int value) {
|
||||
this.value = value;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the DMAReadStatus value.
|
||||
*
|
||||
* @return The DMAReadStatus value.
|
||||
*/
|
||||
public int getValue() {
|
||||
return value;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a DMAReadStatus from a raw value.
|
||||
*
|
||||
* @param value raw value
|
||||
* @return enum value
|
||||
*/
|
||||
public static DMAReadStatus getValue(int value) {
|
||||
if (value == 1) {
|
||||
return kOk;
|
||||
} else if (value == 2) {
|
||||
return kTimeout;
|
||||
}
|
||||
return kError;
|
||||
}
|
||||
}
|
||||
|
||||
private final DMAJNISample m_dmaSample = new DMAJNISample();
|
||||
|
||||
/** Default constructor. */
|
||||
public DMASample() {}
|
||||
|
||||
/**
|
||||
* Retrieves a new DMA sample.
|
||||
*
|
||||
* @param dma DMA object.
|
||||
* @param timeoutSeconds Timeout in seconds for retrieval.
|
||||
* @return DMA read status.
|
||||
*/
|
||||
public DMAReadStatus update(DMA dma, double timeoutSeconds) {
|
||||
return DMAReadStatus.getValue(m_dmaSample.update(dma.m_dmaHandle, timeoutSeconds));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the DMA sample time in microseconds.
|
||||
*
|
||||
* @return The DMA sample time in microseconds.
|
||||
*/
|
||||
public long getTime() {
|
||||
return m_dmaSample.getTime();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the DMA sample timestamp in seconds.
|
||||
*
|
||||
* @return The DMA sample timestamp in seconds.
|
||||
*/
|
||||
public double getTimeStamp() {
|
||||
return getTime() * 1.0e-6;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the DMA sample capture size.
|
||||
*
|
||||
* @return The DMA sample capture size.
|
||||
*/
|
||||
public int getCaptureSize() {
|
||||
return m_dmaSample.getCaptureSize();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the number of DMA trigger channels.
|
||||
*
|
||||
* @return The number of DMA trigger channels.
|
||||
*/
|
||||
public int getTriggerChannels() {
|
||||
return m_dmaSample.getTriggerChannels();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the number of remaining samples.
|
||||
*
|
||||
* @return The number of remaining samples.
|
||||
*/
|
||||
public int getRemaining() {
|
||||
return m_dmaSample.getRemaining();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns raw encoder value from DMA.
|
||||
*
|
||||
* @param encoder Encoder used for DMA.
|
||||
* @return Raw encoder value from DMA.
|
||||
*/
|
||||
public int getEncoderRaw(Encoder encoder) {
|
||||
return m_dmaSample.getEncoder(encoder.m_encoder);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns encoder distance from DMA.
|
||||
*
|
||||
* @param encoder Encoder used for DMA.
|
||||
* @return Encoder distance from DMA.
|
||||
*/
|
||||
public double getEncoderDistance(Encoder encoder) {
|
||||
double val = getEncoderRaw(encoder);
|
||||
val *= encoder.getDecodingScaleFactor();
|
||||
val *= encoder.getDistancePerPulse();
|
||||
return val;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns raw encoder period from DMA.
|
||||
*
|
||||
* @param encoder Encoder used for DMA.
|
||||
* @return Raw encoder period from DMA.
|
||||
*/
|
||||
public int getEncoderPeriodRaw(Encoder encoder) {
|
||||
return m_dmaSample.getEncoderPeriod(encoder.m_encoder);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns counter value from DMA.
|
||||
*
|
||||
* @param counter Counter used for DMA.
|
||||
* @return Counter value from DMA.
|
||||
*/
|
||||
public int getCounter(Counter counter) {
|
||||
return m_dmaSample.getCounter(counter.m_counter);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns counter period from DMA.
|
||||
*
|
||||
* @param counter Counter used for DMA.
|
||||
* @return Counter period from DMA.
|
||||
*/
|
||||
public int getCounterPeriod(Counter counter) {
|
||||
return m_dmaSample.getCounterPeriod(counter.m_counter);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns digital source value from DMA.
|
||||
*
|
||||
* @param digitalSource DigitalSource used for DMA.
|
||||
* @return DigitalSource value from DMA.
|
||||
*/
|
||||
public boolean getDigitalSource(DigitalSource digitalSource) {
|
||||
return m_dmaSample.getDigitalSource(digitalSource.getPortHandleForRouting());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns raw analog input value from DMA.
|
||||
*
|
||||
* @param analogInput AnalogInput used for DMA.
|
||||
* @return Raw analog input value from DMA.
|
||||
*/
|
||||
public int getAnalogInputRaw(AnalogInput analogInput) {
|
||||
return m_dmaSample.getAnalogInput(analogInput.m_port);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns analog input voltage from DMA.
|
||||
*
|
||||
* @param analogInput AnalogInput used for DMA.
|
||||
* @return Analog input voltage from DMA.
|
||||
*/
|
||||
public double getAnalogInputVoltage(AnalogInput analogInput) {
|
||||
return AnalogJNI.getAnalogValueToVolts(analogInput.m_port, getAnalogInputRaw(analogInput));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns averaged raw analog input value from DMA.
|
||||
*
|
||||
* @param analogInput AnalogInput used for DMA.
|
||||
* @return Averaged raw analog input value from DMA.
|
||||
*/
|
||||
public int getAveragedAnalogInputRaw(AnalogInput analogInput) {
|
||||
return m_dmaSample.getAnalogInputAveraged(analogInput.m_port);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns averaged analog input voltage from DMA.
|
||||
*
|
||||
* @param analogInput AnalogInput used for DMA.
|
||||
* @return Averaged analog input voltage from DMA.
|
||||
*/
|
||||
public double getAveragedAnalogInputVoltage(AnalogInput analogInput) {
|
||||
return AnalogJNI.getAnalogValueToVolts(
|
||||
analogInput.m_port, getAveragedAnalogInputRaw(analogInput));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns raw duty cycle output from DMA.
|
||||
*
|
||||
* @param dutyCycle DutyCycle used for DMA.
|
||||
* @return Raw duty cycle output from DMA.
|
||||
*/
|
||||
public int getDutyCycleOutputRaw(DutyCycle dutyCycle) {
|
||||
return m_dmaSample.getDutyCycleOutput(dutyCycle.m_handle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns duty cycle output (0-1) from DMA.
|
||||
*
|
||||
* @param dutyCycle DutyCycle used for DMA.
|
||||
* @return Duty cycle output (0-1) from DMA.
|
||||
*/
|
||||
public double getDutyCycleOutput(DutyCycle dutyCycle) {
|
||||
return m_dmaSample.getDutyCycleOutput(dutyCycle.m_handle)
|
||||
/ (double) dutyCycle.getOutputScaleFactor();
|
||||
}
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.dma;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -1,85 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.dma;
|
||||
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.AnalogInput;
|
||||
import edu.wpi.first.wpilibj.DMA;
|
||||
import edu.wpi.first.wpilibj.DMASample;
|
||||
import edu.wpi.first.wpilibj.DigitalOutput;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
/** This is a sample program showing how to to use DMA to read a sensor. */
|
||||
public class Robot extends TimedRobot {
|
||||
private final DMA m_dma;
|
||||
private final DMASample m_dmaSample;
|
||||
|
||||
// DMA needs a trigger, can use an output as trigger.
|
||||
// 8 Triggers exist per DMA object, can be triggered on any
|
||||
// DigitalSource.
|
||||
private final DigitalOutput m_dmaTrigger;
|
||||
|
||||
// Analog input to read with DMA
|
||||
private final AnalogInput m_analogInput;
|
||||
|
||||
// Encoder to read with DMA
|
||||
private final Encoder m_encoder;
|
||||
|
||||
/** Called once at the beginning of the robot program. */
|
||||
public Robot() {
|
||||
m_dma = new DMA();
|
||||
m_dmaSample = new DMASample();
|
||||
m_dmaTrigger = new DigitalOutput(2);
|
||||
m_analogInput = new AnalogInput(0);
|
||||
m_encoder = new Encoder(0, 1);
|
||||
|
||||
// Trigger on falling edge of dma trigger output
|
||||
m_dma.setExternalTrigger(m_dmaTrigger, false, true);
|
||||
|
||||
// Add inputs we want to read via DMA
|
||||
m_dma.addAnalogInput(m_analogInput);
|
||||
m_dma.addEncoder(m_encoder);
|
||||
m_dma.addEncoderPeriod(m_encoder);
|
||||
|
||||
// Make sure trigger is set to off.
|
||||
m_dmaTrigger.set(true);
|
||||
|
||||
// Start DMA. No triggers or inputs can be added after this call
|
||||
// unless DMA is stopped.
|
||||
m_dma.start(1024);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
// Manually Trigger DMA read
|
||||
m_dmaTrigger.set(false);
|
||||
|
||||
// Update our sample. remaining is the number of samples remaining in the
|
||||
// buffer status is more specific error messages if readStatus is not OK.
|
||||
// Wait 1ms if buffer is empty
|
||||
DMASample.DMAReadStatus readStatus = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1));
|
||||
|
||||
// Unset trigger
|
||||
m_dmaTrigger.set(true);
|
||||
|
||||
if (readStatus == DMASample.DMAReadStatus.kOk) {
|
||||
// Status value in all these reads should be checked, a non 0 value means
|
||||
// value could not be read
|
||||
|
||||
// If DMA is good, values exist
|
||||
double encoderDistance = m_dmaSample.getEncoderDistance(m_encoder);
|
||||
// Period is not scaled, and is a raw value
|
||||
int encoderPeriod = m_dmaSample.getEncoderPeriodRaw(m_encoder);
|
||||
double analogVoltage = m_dmaSample.getAnalogInputVoltage(m_analogInput);
|
||||
|
||||
SmartDashboard.putNumber("Distance", encoderDistance);
|
||||
SmartDashboard.putNumber("Period", encoderPeriod);
|
||||
SmartDashboard.putNumber("Input", analogVoltage);
|
||||
SmartDashboard.putNumber("Timestamp", m_dmaSample.getTimeStamp());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -506,19 +506,6 @@
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "DMA",
|
||||
"description": "Read various sensors using DMA.",
|
||||
"tags": [
|
||||
"Hardware",
|
||||
"DMA",
|
||||
"SmartDashboard"
|
||||
],
|
||||
"foldername": "dma",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "DriveDistanceOffboard",
|
||||
"description": "Drive a differential drivetrain a set distance using TrapezoidProfileCommand and smart motor controller PID.",
|
||||
|
||||
Reference in New Issue
Block a user