diff --git a/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java b/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java index 72fa951511..e2deaddb35 100644 --- a/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java @@ -58,6 +58,8 @@ public class AnalogJNI extends JNIWrapper { public static native int getAnalogVoltsToValue(int analogPortHandle, double voltage); + public static native double getAnalogValueToVolts(int analogPortHandle, int value); + public static native double getAnalogVoltage(int analogPortHandle); public static native double getAnalogAverageVoltage(int analogPortHandle); diff --git a/hal/src/main/java/edu/wpi/first/hal/DMAJNI.java b/hal/src/main/java/edu/wpi/first/hal/DMAJNI.java new file mode 100644 index 0000000000..21c06f1497 --- /dev/null +++ b/hal/src/main/java/edu/wpi/first/hal/DMAJNI.java @@ -0,0 +1,57 @@ +// 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; + +@SuppressWarnings("AbbreviationAsWordInName") +public class DMAJNI extends JNIWrapper { + public static native int initialize(); + + public static native void free(int handle); + + public static native void setPause(int handle, boolean pause); + + public static native void setTimedTrigger(int handle, double periodSeconds); + + public static native void setTimedTriggerCycles(int handle, int cycles); + + public static native void addEncoder(int handle, int encoderHandle); + + public static native void addEncoderPeriod(int handle, int encoderHandle); + + public static native void addCounter(int handle, int counterHandle); + + public static native void addCounterPeriod(int handle, int counterHandle); + + public static native void addDigitalSource(int handle, int digitalSourceHandle); + + public static native void addDutyCycle(int handle, int dutyCycleHandle); + + public static native void addAnalogInput(int handle, int analogInputHandle); + + public static native void addAveragedAnalogInput(int handle, int analogInputHandle); + + public static native void addAnalogAccumulator(int handle, int analogInputHandle); + + public static native int setExternalTrigger( + int handle, int digitalSourceHandle, int analogTriggerType, boolean rising, boolean falling); + + public static native void clearSensors(int handle); + + public static native void clearExternalTriggers(int handle); + + public static native void startDMA(int handle, int queueDepth); + + public static native void stopDMA(int handle); + + // 0-21 channelOffsets + // 22: capture size + // 23: triggerChannels (bitflags) + // 24: remaining + // 25: read status + public static native long readDMA( + int handle, double timeoutSeconds, int[] buffer, int[] sampleStore); + + public static native DMAJNISample.BaseStore getSensorReadData(int handle); +} diff --git a/hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java b/hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java new file mode 100644 index 0000000000..78a0e99a9a --- /dev/null +++ b/hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java @@ -0,0 +1,169 @@ +// 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; + +@SuppressWarnings("AbbreviationAsWordInName") +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 m_propertyMap = new HashMap<>(); + + 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 + int value = readValue(data.m_valueType + 2, data.m_index); + return value; + } + + public void getAnalogAccumulator(int analogInputHandle, AccumulatorResult result) { + BaseStore data = m_propertyMap.get(analogInputHandle); + if (data == null) { + data = addSensorInternal(analogInputHandle); + } + + if (data.m_index == 0) { + int val0 = readValue(kEnable_Accumulator0, 0); + int val1 = readValue(kEnable_Accumulator0, 1); + int val2 = readValue(kEnable_Accumulator0, 2); + result.count = val2; + result.value = ((long) val1 << 32) | val0; + } else if (data.m_index == 1) { + int val0 = readValue(kEnable_Accumulator1, 0); + int val1 = readValue(kEnable_Accumulator1, 1); + int val2 = readValue(kEnable_Accumulator1, 2); + result.count = val2; + result.value = ((long) val1 << 32) | val0; + } else { + throw new RuntimeException("Resource not found in DMA capture"); + } + } + + 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); + } +} diff --git a/hal/src/main/native/athena/DMA.cpp b/hal/src/main/native/athena/DMA.cpp index 40eaca6f4b..f7d8b08720 100644 --- a/hal/src/main/native/athena/DMA.cpp +++ b/hal/src/main/native/athena/DMA.cpp @@ -11,6 +11,7 @@ #include #include "AnalogInternal.h" +#include "ConstantsInternal.h" #include "DigitalInternal.h" #include "EncoderInternal.h" #include "HALInternal.h" @@ -103,19 +104,25 @@ HAL_DMAHandle HAL_InitializeDMA(int32_t* status) { return HAL_kInvalidHandle; } - dma->aDMA->writeConfig_ExternalClock(false, status); - if (*status != 0) { - dmaHandles->Free(handle); - return HAL_kInvalidHandle; + std::memset(&dma->captureStore, 0, sizeof(dma->captureStore)); + + tDMA::tConfig config; + std::memset(&config, 0, sizeof(config)); + config.Pause = true; + dma->aDMA->writeConfig(config, status); + + dma->aDMA->writeRate(1, status); + + tDMA::tExternalTriggers newTrigger; + std::memset(&newTrigger, 0, sizeof(newTrigger)); + for (unsigned char reg = 0; reg < tDMA::kNumExternalTriggersRegisters; + reg++) { + for (unsigned char bit = 0; bit < tDMA::kNumExternalTriggersElements; + bit++) { + dma->aDMA->writeExternalTriggers(reg, bit, newTrigger, status); + } } - HAL_SetDMARate(handle, 1, status); - if (*status != 0) { - dmaHandles->Free(handle); - return HAL_kInvalidHandle; - } - - HAL_SetDMAPause(handle, false, status); return handle; } @@ -139,20 +146,44 @@ void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status) { return; } + if (!dma->manager) { + *status = HAL_INVALID_DMA_STATE; + return; + } + dma->aDMA->writeConfig_Pause(pause, status); } -void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, int32_t* status) { + +void HAL_SetDMATimedTrigger(HAL_DMAHandle handle, double seconds, + int32_t* status) { + constexpr double baseMultipler = kSystemClockTicksPerMicrosecond * 1000000; + uint32_t cycles = static_cast(baseMultipler * seconds); + HAL_SetDMATimedTriggerCycles(handle, cycles, status); +} + +void HAL_SetDMATimedTriggerCycles(HAL_DMAHandle handle, uint32_t cycles, + int32_t* status) { auto dma = dmaHandles->Get(handle); if (!dma) { *status = HAL_HANDLE_ERROR; return; } + if (dma->manager) { + *status = HAL_INVALID_DMA_ADDITION; + return; + } + if (cycles < 1) { cycles = 1; } - dma->aDMA->writeRate(static_cast(cycles), status); + dma->aDMA->writeConfig_ExternalClock(false, status); + if (*status != 0) { + return; + } + + dma->aDMA->writeRate(cycles, status); } void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle, @@ -477,20 +508,20 @@ void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle, } } -void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle, - HAL_Handle digitalSourceHandle, - HAL_AnalogTriggerType analogTriggerType, - HAL_Bool rising, HAL_Bool falling, - 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) { auto dma = dmaHandles->Get(handle); if (!dma) { *status = HAL_HANDLE_ERROR; - return; + return 0; } if (dma->manager) { *status = HAL_INVALID_DMA_ADDITION; - return; + return 0; } int index = 0; @@ -504,21 +535,21 @@ void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle, if (index == 8) { *status = NO_AVAILABLE_RESOURCES; - return; + return 0; } dma->captureStore.triggerChannels |= (1 << index); auto channelIndex = index; - auto isExternalClock = dma->aDMA->readConfig_ExternalClock(status); - if (*status == 0 && !isExternalClock) { - dma->aDMA->writeConfig_ExternalClock(true, status); - if (*status != 0) { - return; - } - } else if (*status != 0) { - return; + dma->aDMA->writeConfig_ExternalClock(true, status); + if (*status != 0) { + return 0; + } + + dma->aDMA->writeRate(1, status); + if (*status != 0) { + return 0; } uint8_t pin = 0; @@ -532,7 +563,7 @@ void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle, hal::SetLastError(status, "Digital Source unabled to be mapped properly. Likely " "invalid handle passed."); - return; + return 0; } tDMA::tExternalTriggers newTrigger; @@ -544,6 +575,55 @@ void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle, dma->aDMA->writeExternalTriggers(channelIndex / 4, channelIndex % 4, newTrigger, status); + return index; +} + +void HAL_ClearDMASensors(HAL_DMAHandle handle, int32_t* status) { + auto dma = dmaHandles->Get(handle); + if (!dma) { + *status = HAL_HANDLE_ERROR; + return; + } + + if (dma->manager) { + *status = HAL_INVALID_DMA_STATE; + return; + } + + bool existingExternal = dma->aDMA->readConfig_ExternalClock(status); + if (*status != 0) { + return; + } + + tDMA::tConfig config; + std::memset(&config, 0, sizeof(config)); + config.Pause = true; + config.ExternalClock = existingExternal; + dma->aDMA->writeConfig(config, status); +} + +void HAL_ClearDMAExternalTriggers(HAL_DMAHandle handle, int32_t* status) { + auto dma = dmaHandles->Get(handle); + if (!dma) { + *status = HAL_HANDLE_ERROR; + return; + } + + if (dma->manager) { + *status = HAL_INVALID_DMA_STATE; + return; + } + + dma->captureStore.triggerChannels = 0; + tDMA::tExternalTriggers newTrigger; + std::memset(&newTrigger, 0, sizeof(newTrigger)); + for (unsigned char reg = 0; reg < tDMA::kNumExternalTriggersRegisters; + reg++) { + for (unsigned char bit = 0; bit < tDMA::kNumExternalTriggersElements; + bit++) { + dma->aDMA->writeExternalTriggers(reg, bit, newTrigger, status); + } + } } void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) { @@ -554,7 +634,7 @@ void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) { } if (dma->manager) { - *status = INCOMPATIBLE_STATE; + *status = HAL_INVALID_DMA_STATE; return; } @@ -598,12 +678,15 @@ void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) { dma->captureStore.captureSize = accum_size + 1; } - dma->manager = std::make_unique( - g_DMA_index, queueDepth * dma->captureStore.captureSize, status); + uint32_t byteDepth = queueDepth * dma->captureStore.captureSize; + + dma->manager = std::make_unique(g_DMA_index, byteDepth, status); if (*status != 0) { return; } + dma->aDMA->writeConfig_Pause(false, status); + dma->manager->start(status); dma->manager->stop(status); dma->manager->start(status); @@ -617,6 +700,8 @@ void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status) { } if (dma->manager) { + dma->aDMA->writeConfig_Pause(true, status); + *status = 0; dma->manager->stop(status); dma->manager = nullptr; } @@ -629,7 +714,7 @@ void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) { enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer, HAL_DMASample* dmaSample, - int32_t timeoutMs, + double timeoutSeconds, int32_t* remainingOut, int32_t* status) { DMA* dma = static_cast(dmaPointer); @@ -637,12 +722,13 @@ enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer, size_t remainingBytes = 0; if (!dma->manager) { - *status = INCOMPATIBLE_STATE; + *status = HAL_INVALID_DMA_STATE; return HAL_DMA_ERROR; } dma->manager->read(dmaSample->readBuffer, dma->captureStore.captureSize, - timeoutMs, &remainingBytes, status); + static_cast(timeoutSeconds * 1000), + &remainingBytes, status); *remainingOut = remainingBytes / dma->captureStore.captureSize; @@ -667,15 +753,16 @@ enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer, } enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle, - HAL_DMASample* dmaSample, int32_t timeoutMs, - int32_t* remainingOut, int32_t* status) { + HAL_DMASample* dmaSample, + double timeoutSeconds, int32_t* remainingOut, + int32_t* status) { auto dma = dmaHandles->Get(handle); if (!dma) { *status = HAL_HANDLE_ERROR; return HAL_DMA_ERROR; } - return HAL_ReadDMADirect(dma.get(), dmaSample, timeoutMs, remainingOut, + return HAL_ReadDMADirect(dma.get(), dmaSample, timeoutSeconds, remainingOut, status); } diff --git a/hal/src/main/native/athena/DigitalInternal.cpp b/hal/src/main/native/athena/DigitalInternal.cpp index c75c66b3ba..63ea0aa25f 100644 --- a/hal/src/main/native/athena/DigitalInternal.cpp +++ b/hal/src/main/native/athena/DigitalInternal.cpp @@ -165,9 +165,17 @@ bool remapDigitalSource(HAL_Handle digitalSourceHandle, } analogTrigger = false; return true; - } else { - return false; + } else if (isHandleType(digitalSourceHandle, HAL_HandleEnum::PWM)) { + // PWM's on MXP port are supported as a digital source + int32_t index = getHandleIndex(digitalSourceHandle); + if (index >= kNumPWMHeaders) { + channel = remapMXPPWMChannel(index); + module = 1; + analogTrigger = false; + return true; + } } + return false; } int32_t remapMXPChannel(int32_t channel) { diff --git a/hal/src/main/native/athena/Encoder.cpp b/hal/src/main/native/athena/Encoder.cpp index d5a3d38c94..6edfea3e1e 100644 --- a/hal/src/main/native/athena/Encoder.cpp +++ b/hal/src/main/native/athena/Encoder.cpp @@ -254,7 +254,7 @@ bool GetEncoderBaseHandle(HAL_EncoderHandle handle, HAL_FPGAEncoderHandle* fpgaHandle, HAL_CounterHandle* counterHandle) { auto encoder = encoderHandles->Get(handle); - if (!handle) { + if (!encoder) { return false; } diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index e99aa0f6a7..03cefc52d6 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -219,6 +219,10 @@ const char* HAL_GetErrorMessage(int32_t code) { return HAL_CAN_BUFFER_OVERRUN_MESSAGE; case HAL_LED_CHANNEL_ERROR: return HAL_LED_CHANNEL_ERROR_MESSAGE; + case HAL_INVALID_DMA_STATE: + return HAL_INVALID_DMA_STATE_MESSAGE; + case HAL_INVALID_DMA_ADDITION: + return HAL_INVALID_DMA_ADDITION_MESSAGE; case HAL_USE_LAST_ERROR: return HAL_USE_LAST_ERROR_MESSAGE; default: diff --git a/hal/src/main/native/cpp/jni/AnalogJNI.cpp b/hal/src/main/native/cpp/jni/AnalogJNI.cpp index 5f0abce6be..a2cccb67e9 100644 --- a/hal/src/main/native/cpp/jni/AnalogJNI.cpp +++ b/hal/src/main/native/cpp/jni/AnalogJNI.cpp @@ -296,6 +296,22 @@ Java_edu_wpi_first_hal_AnalogJNI_getAnalogVoltsToValue return returnValue; } +/* + * Class: edu_wpi_first_hal_AnalogJNI + * Method: getAnalogValueToVolts + * Signature: (II)D + */ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_hal_AnalogJNI_getAnalogValueToVolts + (JNIEnv* env, jclass, jint id, jint rawValue) +{ + int32_t status = 0; + jdouble returnValue = + HAL_GetAnalogValueToVolts((HAL_AnalogInputHandle)id, rawValue, &status); + CheckStatus(env, status); + return returnValue; +} + /* * Class: edu_wpi_first_hal_AnalogJNI * Method: getAnalogVoltage diff --git a/hal/src/main/native/cpp/jni/DMAJNI.cpp b/hal/src/main/native/cpp/jni/DMAJNI.cpp new file mode 100644 index 0000000000..3e95c7b6ac --- /dev/null +++ b/hal/src/main/native/cpp/jni/DMAJNI.cpp @@ -0,0 +1,416 @@ +// 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 +#include + +#include "HALUtil.h" +#include "edu_wpi_first_hal_DMAJNI.h" +#include "hal/DMA.h" +#include "hal/handles/HandlesInternal.h" + +using namespace hal; + +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) +{ + 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(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(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(dmaSample.readBuffer)); + + int32_t* nativeArr = + static_cast(env->GetPrimitiveArrayCritical(store, nullptr)); + + std::copy_n( + dmaSample.channelOffsets, + sizeof(dmaSample.channelOffsets) / sizeof(dmaSample.channelOffsets[0]), + nativeArr); + nativeArr[22] = static_cast(dmaSample.captureSize); + nativeArr[23] = static_cast(dmaSample.triggerChannels); + nativeArr[24] = remaining; + nativeArr[25] = readStatus; + + env->ReleasePrimitiveArrayCritical(store, nativeArr, JNI_ABORT); + + 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(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" diff --git a/hal/src/main/native/cpp/jni/HALUtil.cpp b/hal/src/main/native/cpp/jni/HALUtil.cpp index 1cf7cbb3d0..b8ca4e2554 100644 --- a/hal/src/main/native/cpp/jni/HALUtil.cpp +++ b/hal/src/main/native/cpp/jni/HALUtil.cpp @@ -46,6 +46,7 @@ static JClass matchInfoDataCls; static JClass accumulatorResultCls; static JClass canDataCls; static JClass halValueCls; +static JClass baseStoreCls; static const JClassInit classes[] = { {"edu/wpi/first/hal/PWMConfigDataResult", &pwmConfigDataResultCls}, @@ -53,7 +54,8 @@ static const JClassInit classes[] = { {"edu/wpi/first/hal/MatchInfoData", &matchInfoDataCls}, {"edu/wpi/first/hal/AccumulatorResult", &accumulatorResultCls}, {"edu/wpi/first/hal/CANData", &canDataCls}, - {"edu/wpi/first/hal/HALValue", &halValueCls}}; + {"edu/wpi/first/hal/HALValue", &halValueCls}, + {"edu/wpi/first/hal/DMAJNISample$BaseStore", &baseStoreCls}}; static const JExceptionInit exceptions[] = { {"java/lang/IllegalArgumentException", &illegalArgExCls}, @@ -305,6 +307,11 @@ jobject CreateHALValue(JNIEnv* env, const HAL_Value& value) { halValueCls, fromNative, static_cast(value.type), value1, value2); } +jobject CreateDMABaseStore(JNIEnv* env, jint valueType, jint index) { + static jmethodID ctor = env->GetMethodID(baseStoreCls, "", "(II)V"); + return env->NewObject(baseStoreCls, ctor, valueType, index); +} + JavaVM* GetJVM() { return jvm; } diff --git a/hal/src/main/native/cpp/jni/HALUtil.h b/hal/src/main/native/cpp/jni/HALUtil.h index 56faa88f1c..2afe595d62 100644 --- a/hal/src/main/native/cpp/jni/HALUtil.h +++ b/hal/src/main/native/cpp/jni/HALUtil.h @@ -75,6 +75,8 @@ jbyteArray SetCANDataObject(JNIEnv* env, jobject canData, int32_t length, jobject CreateHALValue(JNIEnv* env, const HAL_Value& value); +jobject CreateDMABaseStore(JNIEnv* env, jint valueType, jint index); + JavaVM* GetJVM(); } // namespace hal diff --git a/hal/src/main/native/include/hal/Counter.h b/hal/src/main/native/include/hal/Counter.h index 578201a13f..df681062e7 100644 --- a/hal/src/main/native/include/hal/Counter.h +++ b/hal/src/main/native/include/hal/Counter.h @@ -62,7 +62,7 @@ void HAL_SetCounterAverageSize(HAL_CounterHandle counterHandle, int32_t size, * * @param counterHandle the counter handle * @param digitalSourceHandle the digital source handle (either a - * HAL_AnalogTriggerHandle of a HAL_DigitalHandle) + * HAL_AnalogTriggerHandle or a HAL_DigitalHandle) * @param analogTriggerType the analog trigger type if the source is an analog * trigger */ @@ -96,7 +96,7 @@ void HAL_ClearCounterUpSource(HAL_CounterHandle counterHandle, int32_t* status); * * @param counterHandle the counter handle * @param digitalSourceHandle the digital source handle (either a - * HAL_AnalogTriggerHandle of a HAL_DigitalHandle) + * HAL_AnalogTriggerHandle or a HAL_DigitalHandle) * @param analogTriggerType the analog trigger type if the source is an analog * trigger */ diff --git a/hal/src/main/native/include/hal/DMA.h b/hal/src/main/native/include/hal/DMA.h index 8313f3f23e..49eb017810 100644 --- a/hal/src/main/native/include/hal/DMA.h +++ b/hal/src/main/native/include/hal/DMA.h @@ -4,22 +4,29 @@ #pragma once -#include - #include "hal/AnalogTrigger.h" #include "hal/Types.h" +/** + * @defgroup hal_dma DMA Functions + * @ingroup hal_capi + * @{ + */ + // clang-format off /** * The DMA Read Status. */ -HAL_ENUM(HAL_DMAReadStatus ) { +HAL_ENUM(HAL_DMAReadStatus) { HAL_DMA_OK = 1, HAL_DMA_TIMEOUT = 2, HAL_DMA_ERROR = 3, }; // clang-format on +/** + * Buffer for containing all DMA data for a specific sample. + */ struct HAL_DMASample { uint32_t readBuffer[74]; int32_t channelOffsets[22]; @@ -32,91 +39,380 @@ struct HAL_DMASample { extern "C" { #endif +/** + * Initializes an object for peforming DMA transfers. + * + * @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 handle the dma handle + * @param pause true to pause transfers, false to resume. + */ void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status); -void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, 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 handle the dma handle + * @param periodSeconds the period to trigger in seconds + */ +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 handle the dma handle + * @param cycles the period to trigger in FPGA cycles + */ +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 handle the dma handle + * @param encoderHandle the encoder to add + */ 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 handle the dma handle + * @param encoderHandle the encoder to add + */ 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 handle the dma handle + * @param counterHandle the counter to add + */ void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle, int32_t* status); + +/** + * Adds timer data for an counter to be collected by DMA. + * + * @param handle the dma handle + * @param counterHandle the counter to add + */ 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 handle the dma handle + * @param digitalSourceHandle the digital source to add + */ 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 handle the dma handle + * @param aInHandle the analog input to add + */ 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 handle the dma handle + * @param aInHandle the analog input to add + */ void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle, HAL_AnalogInputHandle aInHandle, int32_t* status); +/** + * Adds acuumulator data of an analog input to be collected by DMA. + * + * This can only be called if DMA is not started. + * + * @param handle the dma handle + * @param aInHandle the analog input to add + */ 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 handle the dma handle + * @param dutyCycleHandle the duty cycle input to add + */ void HAL_AddDMADutyCycle(HAL_DMAHandle handle, HAL_DutyCycleHandle dutyCycleHandle, int32_t* status); -void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle, - HAL_Handle digitalSourceHandle, - HAL_AnalogTriggerType analogTriggerType, - HAL_Bool rising, HAL_Bool falling, - 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 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 risingEdge true to trigger on rising + * @param fallingEdge true to trigger on falling + * @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 handle the dma handle + */ +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 handle the dma handle + */ +void HAL_ClearDMAExternalTriggers(HAL_DMAHandle handle, int32_t* status); + +/** + * Starts DMA Collection. + * + * @param handle the dma handle + * @param queueDepth the number of objects to be able to queue + */ void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status); + +/** + * Stops DMA Collection. + * + * @param handle the dma handle + */ 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 handle the dma handle + */ enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer, HAL_DMASample* dmaSample, - int32_t timeoutMs, + double timeoutSeconds, int32_t* remainingOut, int32_t* status); +/** + * Reads a DMA sample from the queue. + * + * + * @param handle the dma handle + * @param dmaSample the sample object to place data into + * @param timeoutSeconds the time to wait for data to be queued before timing + * out + * @param remainingOut the number of samples remaining in the queue + * @return the succes result of the sample read + */ enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle, - HAL_DMASample* dmaSample, int32_t timeoutMs, - int32_t* remainingOut, int32_t* status); + HAL_DMASample* dmaSample, + double timeoutSeconds, int32_t* remainingOut, + int32_t* status); -// Sampling Code +// 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 dmaSample the sample to read from + * @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 dmaSample the sample to read from + * @param encoderHandle the encoder handle + * @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 dmaSample the sample to read from + * @param counterHandle the counter handle + * @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 dmaSample the sample to read from + * @param encoderHandle the encoder handle + * @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 dmaSample the sample to read from + * @param counterHandle the counter handle + * @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 dmaSample the sample to read from + * @param dSourceHandle the digital source handle + * @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 dmaSample the sample to read from + * @param aInHandle the analog input handle + * @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 dmaSample the sample to read from + * @param aInHandle the analog input handle + * @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 dmaSample the sample to read from + * @param aInHandle the analog input handle + * @param count the accumulator count + * @param value the accumulator value + */ 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 dmaSample the sample to read from + * @param dutyCycleHandle the duty cycle handle + * @return raw duty cycle input data + */ int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample, HAL_DutyCycleHandle dutyCycleHandle, int32_t* status); @@ -124,3 +420,4 @@ int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample, #ifdef __cplusplus } // extern "C" #endif +/** @} */ diff --git a/hal/src/main/native/include/hal/Encoder.h b/hal/src/main/native/include/hal/Encoder.h index 26da006a27..8996257605 100644 --- a/hal/src/main/native/include/hal/Encoder.h +++ b/hal/src/main/native/include/hal/Encoder.h @@ -254,7 +254,7 @@ int32_t HAL_GetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle, * * @param encoderHandle the encoder handle * @param digitalSourceHandle the index source handle (either a - * HAL_AnalogTriggerHandle of a HAL_DigitalHandle) + * HAL_AnalogTriggerHandle or a HAL_DigitalHandle) * @param analogTriggerType the analog trigger type if the source is an analog * trigger * @param type the index triggering type diff --git a/hal/src/main/native/include/hal/Errors.h b/hal/src/main/native/include/hal/Errors.h index 6444ea6c9e..ab54576945 100644 --- a/hal/src/main/native/include/hal/Errors.h +++ b/hal/src/main/native/include/hal/Errors.h @@ -101,6 +101,10 @@ #define HAL_INVALID_DMA_ADDITION_MESSAGE \ "HAL_AddDMA() only works before HAL_StartDMA()" +#define HAL_INVALID_DMA_STATE -1103 +#define HAL_INVALID_DMA_STATE_MESSAGE \ + "HAL_SetPause() only works before HAL_StartDMA()" + #define HAL_SERIAL_PORT_NOT_FOUND -1123 #define HAL_SERIAL_PORT_NOT_FOUND_MESSAGE \ "HAL: The specified serial port device was not found" diff --git a/hal/src/main/native/include/hal/Interrupts.h b/hal/src/main/native/include/hal/Interrupts.h index 948859a569..4df327b829 100644 --- a/hal/src/main/native/include/hal/Interrupts.h +++ b/hal/src/main/native/include/hal/Interrupts.h @@ -78,7 +78,7 @@ int64_t HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle, * * @param interruptHandle the interrupt handle * @param digitalSourceHandle the digital source handle (either a - * HAL_AnalogTriggerHandle of a HAL_DigitalHandle) + * HAL_AnalogTriggerHandle or a HAL_DigitalHandle) * @param analogTriggerType the trigger type if the source is an AnalogTrigger */ void HAL_RequestInterrupts(HAL_InterruptHandle interruptHandle, diff --git a/hal/src/main/native/sim/DMA.cpp b/hal/src/main/native/sim/DMA.cpp index b749e9611a..c000ac5302 100644 --- a/hal/src/main/native/sim/DMA.cpp +++ b/hal/src/main/native/sim/DMA.cpp @@ -11,7 +11,10 @@ HAL_DMAHandle HAL_InitializeDMA(int32_t* status) { void HAL_FreeDMA(HAL_DMAHandle handle) {} void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status) {} -void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, 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) {} @@ -40,11 +43,16 @@ void HAL_AddDMADutyCycle(HAL_DMAHandle handle, HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) { } -void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle, - HAL_Handle digitalSourceHandle, - HAL_AnalogTriggerType analogTriggerType, - HAL_Bool rising, HAL_Bool falling, - 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) {} @@ -55,15 +63,16 @@ void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) { enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer, HAL_DMASample* dmaSample, - int32_t timeoutMs, + double timeoutSeconds, int32_t* remainingOut, int32_t* status) { return HAL_DMA_ERROR; } enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle, - HAL_DMASample* dmaSample, int32_t timeoutMs, - int32_t* remainingOut, int32_t* status) { + HAL_DMASample* dmaSample, + double timeoutSeconds, int32_t* remainingOut, + int32_t* status) { return HAL_DMA_ERROR; } diff --git a/hal/src/main/native/sim/Encoder.cpp b/hal/src/main/native/sim/Encoder.cpp index f34f651686..78aa28123c 100644 --- a/hal/src/main/native/sim/Encoder.cpp +++ b/hal/src/main/native/sim/Encoder.cpp @@ -18,6 +18,8 @@ using namespace hal; namespace { struct Encoder { HAL_Handle nativeHandle; + HAL_FPGAEncoderHandle fpgaHandle; + HAL_CounterHandle counterHandle; HAL_EncoderEncodingType encodingType; double distancePerPulse; uint8_t index; @@ -46,6 +48,21 @@ void InitializeEncoder() { } } // namespace hal::init +namespace hal { +bool GetEncoderBaseHandle(HAL_EncoderHandle handle, + HAL_FPGAEncoderHandle* fpgaHandle, + HAL_CounterHandle* counterHandle) { + auto encoder = encoderHandles->Get(handle); + if (!handle) { + return false; + } + + *fpgaHandle = encoder->fpgaHandle; + *counterHandle = encoder->counterHandle; + return true; +} +} // namespace hal + extern "C" { HAL_EncoderHandle HAL_InitializeEncoder( HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA, @@ -86,6 +103,13 @@ HAL_EncoderHandle HAL_InitializeEncoder( encoder->nativeHandle = nativeHandle; encoder->encodingType = encodingType; encoder->distancePerPulse = 1.0; + if (encodingType == HAL_EncoderEncodingType::HAL_Encoder_k4X) { + encoder->fpgaHandle = nativeHandle; + encoder->counterHandle = HAL_kInvalidHandle; + } else { + encoder->fpgaHandle = HAL_kInvalidHandle; + encoder->counterHandle = nativeHandle; + } return handle; } diff --git a/wpilibc/src/main/native/cpp/DMA.cpp b/wpilibc/src/main/native/cpp/DMA.cpp index be057383ab..5eeaa60d02 100644 --- a/wpilibc/src/main/native/cpp/DMA.cpp +++ b/wpilibc/src/main/native/cpp/DMA.cpp @@ -4,6 +4,14 @@ #include "frc/DMA.h" +#include +#include +#include +#include +#include +#include +#include + #include #include @@ -32,10 +40,16 @@ void DMA::SetPause(bool pause) { FRC_CheckErrorStatus(status, "{}", "SetPause"); } -void DMA::SetRate(int cycles) { +void DMA::SetTimedTrigger(units::second_t seconds) { int32_t status = 0; - HAL_SetDMARate(dmaHandle, cycles, &status); - FRC_CheckErrorStatus(status, "{}", "SetRate"); + HAL_SetDMATimedTrigger(dmaHandle, seconds.to(), &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) { @@ -93,23 +107,56 @@ void DMA::AddAnalogAccumulator(const AnalogInput* analogInput) { FRC_CheckErrorStatus(status, "{}", "AddAnalogAccumulator"); } -void DMA::SetExternalTrigger(DigitalSource* source, bool rising, bool falling) { +int DMA::SetExternalTrigger(DigitalSource* source, bool rising, bool falling) { int32_t status = 0; - HAL_SetDMAExternalTrigger(dmaHandle, source->GetPortHandleForRouting(), - static_cast( - source->GetAnalogTriggerTypeForRouting()), - rising, falling, &status); + int32_t idx = + HAL_SetDMAExternalTrigger(dmaHandle, source->GetPortHandleForRouting(), + static_cast( + source->GetAnalogTriggerTypeForRouting()), + rising, falling, &status); FRC_CheckErrorStatus(status, "{}", "SetExternalTrigger"); + return idx; } -void DMA::StartDMA(int queueDepth) { +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, "{}", "StartDMA"); + FRC_CheckErrorStatus(status, "{}", "Start"); } -void DMA::StopDMA() { +void DMA::Stop() { int32_t status = 0; HAL_StopDMA(dmaHandle, &status); - FRC_CheckErrorStatus(status, "{}", "StopDMA"); + FRC_CheckErrorStatus(status, "{}", "Stop"); } diff --git a/wpilibc/src/main/native/include/frc/DMA.h b/wpilibc/src/main/native/include/frc/DMA.h index 72c3829215..1bbf268f77 100644 --- a/wpilibc/src/main/native/include/frc/DMA.h +++ b/wpilibc/src/main/native/include/frc/DMA.h @@ -5,6 +5,7 @@ #pragma once #include +#include namespace frc { class Encoder; @@ -13,6 +14,8 @@ class DigitalSource; class DutyCycle; class AnalogInput; class DMASample; +class PWM; +class PWMMotorController; class DMA { friend class DMASample; @@ -25,7 +28,8 @@ class DMA { DMA(DMA&& other) = default; void SetPause(bool pause); - void SetRate(int cycles); + void SetTimedTrigger(units::second_t seconds); + void SetTimedTriggerCycles(int cycles); void AddEncoder(const Encoder* encoder); void AddEncoderPeriod(const Encoder* encoder); @@ -41,10 +45,15 @@ class DMA { void AddAveragedAnalogInput(const AnalogInput* analogInput); void AddAnalogAccumulator(const AnalogInput* analogInput); - void SetExternalTrigger(DigitalSource* source, bool rising, bool falling); + int SetExternalTrigger(DigitalSource* source, bool rising, bool falling); + int SetPwmEdgeTrigger(PWM* pwm, bool rising, bool falling); + int SetPwmEdgeTrigger(PWMMotorController* pwm, bool rising, bool falling); - void StartDMA(int queueDepth); - void StopDMA(); + void ClearSensors(); + void ClearExternalTriggers(); + + void Start(int queueDepth); + void Stop(); private: hal::Handle dmaHandle; diff --git a/wpilibc/src/main/native/include/frc/DMASample.h b/wpilibc/src/main/native/include/frc/DMASample.h index bd25251e8d..9d78cca6d5 100644 --- a/wpilibc/src/main/native/include/frc/DMASample.h +++ b/wpilibc/src/main/native/include/frc/DMASample.h @@ -19,11 +19,16 @@ namespace frc { class DMASample : public HAL_DMASample { public: - HAL_DMAReadStatus Update(const DMA* dma, units::second_t timeout, - int32_t* remaining, int32_t* status) { - units::millisecond_t ms = timeout; - auto timeoutMs = ms.to(); - return HAL_ReadDMA(dma->dmaHandle, this, timeoutMs, remaining, status); + enum class DMAReadStatus { + kOk = HAL_DMA_OK, + kTimeout = HAL_DMA_TIMEOUT, + kError = HAL_DMA_ERROR + }; + + DMAReadStatus Update(const DMA* dma, units::second_t timeout, + int32_t* remaining, int32_t* status) { + return static_cast(HAL_ReadDMA( + dma->dmaHandle, this, timeout.to(), remaining, status)); } uint64_t GetTime() const { return timeStamp; } diff --git a/wpilibc/src/main/native/include/frc/PWM.h b/wpilibc/src/main/native/include/frc/PWM.h index aaf92f5d39..cde5cdc2a6 100644 --- a/wpilibc/src/main/native/include/frc/PWM.h +++ b/wpilibc/src/main/native/include/frc/PWM.h @@ -12,6 +12,7 @@ namespace frc { class AddressableLED; +class DMA; /** * Class implements the PWM generation in the FPGA. @@ -33,6 +34,7 @@ class AddressableLED; class PWM : public wpi::Sendable, public wpi::SendableHelper { public: friend class AddressableLED; + friend class DMA; /** * Represents the amount to multiply the minimum servo-pulse pwm period by. */ diff --git a/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h b/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h index 3fe67f518d..c3e192de3a 100644 --- a/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h +++ b/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h @@ -15,6 +15,7 @@ #include "frc/motorcontrol/MotorController.h" namespace frc { +class DMA; /** * Common base class for all PWM Motor Controllers. @@ -24,6 +25,8 @@ class PWMMotorController : public MotorController, public wpi::Sendable, public wpi::SendableHelper { public: + friend class DMA; + PWMMotorController(PWMMotorController&&) = default; PWMMotorController& operator=(PWMMotorController&&) = default; @@ -74,6 +77,8 @@ class PWMMotorController : public MotorController, private: bool m_isInverted = false; + + PWM* GetPwm() { return &m_pwm; } }; } // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp index 3eff035898..9097d94456 100644 --- a/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp @@ -39,7 +39,7 @@ class Robot : public frc::TimedRobot { // Start DMA. No triggers or inputs can be added after this call // unless DMA is stopped. - m_dma.StartDMA(1024); + m_dma.Start(1024); } void RobotPeriodic() override { @@ -53,10 +53,13 @@ class Robot : public frc::TimedRobot { // 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 - HAL_DMAReadStatus readStatus = + frc::DMASample::DMAReadStatus readStatus = sample.Update(&m_dma, 1_ms, &remaining, &status); - if (readStatus == HAL_DMA_OK) { + // 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 diff --git a/wpilibcIntegrationTests/src/main/native/cpp/DMATest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/DMATest.cpp new file mode 100644 index 0000000000..841b55dcb0 --- /dev/null +++ b/wpilibcIntegrationTests/src/main/native/cpp/DMATest.cpp @@ -0,0 +1,155 @@ +// 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 +#include +#include +#include +#include + +#include "TestBench.h" +#include "frc/DMA.h" +#include "frc/DMASample.h" +#include "gtest/gtest.h" + +using namespace frc; + +static constexpr auto kDelayTime = 100_ms; + +class DMATest : public testing::Test { + protected: + AnalogInput m_analogInput{TestBench::kAnalogOutputChannel}; + AnalogOutput m_analogOutput{TestBench::kFakeAnalogOutputChannel}; + DigitalOutput m_manualTrigger{TestBench::kLoop1InputChannel}; + Jaguar m_pwm{TestBench::kFakePwmOutput}; + DMA m_dma; + + void SetUp() override { + m_dma.AddAnalogInput(&m_analogInput); + m_dma.SetExternalTrigger(&m_manualTrigger, false, true); + m_manualTrigger.Set(true); + } +}; + +TEST_F(DMATest, PausingWorks_DISABLED) { + m_dma.Start(1024); + m_dma.SetPause(true); + m_manualTrigger.Set(false); + + frc::DMASample sample; + int32_t remaining = 0; + int32_t status = 0; + + auto timedOut = sample.Update(&m_dma, 5_ms, &remaining, &status); + + ASSERT_EQ(DMASample::DMAReadStatus::kTimeout, timedOut); +} + +TEST_F(DMATest, RemovingTriggersWorks_DISABLED) { + m_dma.ClearExternalTriggers(); + m_dma.Start(1024); + m_manualTrigger.Set(false); + + frc::DMASample sample; + int32_t remaining = 0; + int32_t status = 0; + + auto timedOut = sample.Update(&m_dma, 5_ms, &remaining, &status); + + ASSERT_EQ(DMASample::DMAReadStatus::kTimeout, timedOut); +} + +TEST_F(DMATest, ManualTriggerOnlyHappensOnce_DISABLED) { + m_dma.Start(1024); + m_manualTrigger.Set(false); + + frc::DMASample sample; + int32_t remaining = 0; + int32_t status = 0; + + auto timedOut = sample.Update(&m_dma, 5_ms, &remaining, &status); + + ASSERT_EQ(DMASample::DMAReadStatus::kOk, timedOut); + ASSERT_EQ(0, remaining); + timedOut = sample.Update(&m_dma, 5_ms, &remaining, &status); + ASSERT_EQ(DMASample::DMAReadStatus::kTimeout, timedOut); +} + +TEST_F(DMATest, AnalogIndividualTriggers_DISABLED) { + m_dma.Start(1024); + for (double i = 0; i < 5; i += 0.5) { + frc::DMASample sample; + int32_t remaining = 0; + int32_t status = 0; + + m_analogOutput.SetVoltage(i); + frc::Wait(kDelayTime); + m_manualTrigger.Set(false); + auto timedOut = sample.Update(&m_dma, 1_ms, &remaining, &status); + m_manualTrigger.Set(true); + ASSERT_EQ(DMASample::DMAReadStatus::kOk, timedOut); + ASSERT_EQ(0, status); + ASSERT_EQ(0, remaining); + ASSERT_DOUBLE_EQ(m_analogInput.GetVoltage(), + sample.GetAnalogInputVoltage(&m_analogInput, &status)); + } +} + +TEST_F(DMATest, AnalogMultipleTriggers_DISABLED) { + m_dma.Start(1024); + std::vector values; + for (double i = 0; i < 5; i += 0.5) { + values.push_back(i); + m_analogOutput.SetVoltage(i); + frc::Wait(kDelayTime); + m_manualTrigger.Set(false); + frc::Wait(kDelayTime); + m_manualTrigger.Set(true); + } + + for (size_t i = 0; i < values.size(); i++) { + frc::DMASample sample; + int32_t remaining = 0; + int32_t status = 0; + auto timedOut = sample.Update(&m_dma, 1_ms, &remaining, &status); + ASSERT_EQ(DMASample::DMAReadStatus::kOk, timedOut); + ASSERT_EQ(0, status); + ASSERT_EQ(values.size() - i - 1, (uint32_t)remaining); + ASSERT_DOUBLE_EQ(values[i], + sample.GetAnalogInputVoltage(&m_analogInput, &status)); + } +} + +TEST_F(DMATest, TimedTriggers_DISABLED) { + m_dma.SetTimedTrigger(10_ms); + m_dma.Start(1024); + frc::Wait(kDelayTime); + m_dma.SetPause(true); + + frc::DMASample sample; + int32_t remaining = 0; + int32_t status = 0; + + auto timedOut = sample.Update(&m_dma, 1_ms, &remaining, &status); + ASSERT_EQ(DMASample::DMAReadStatus::kOk, timedOut); + ASSERT_EQ(0, status); + ASSERT_GT(remaining, 5); +} + +TEST_F(DMATest, PWMTimedTriggers_DISABLED) { + m_dma.ClearExternalTriggers(); + m_dma.SetPwmEdgeTrigger(&m_pwm, true, false); + m_dma.Start(1024); + frc::Wait(kDelayTime); + m_dma.SetPause(true); + + frc::DMASample sample; + int32_t remaining = 0; + int32_t status = 0; + + auto timedOut = sample.Update(&m_dma, 1_ms, &remaining, &status); + ASSERT_EQ(DMASample::DMAReadStatus::kOk, timedOut); + ASSERT_EQ(0, status); + ASSERT_GT(remaining, 5); +} diff --git a/wpilibcIntegrationTests/src/main/native/include/TestBench.h b/wpilibcIntegrationTests/src/main/native/include/TestBench.h index 28f2046d22..0b6cf33a65 100644 --- a/wpilibcIntegrationTests/src/main/native/include/TestBench.h +++ b/wpilibcIntegrationTests/src/main/native/include/TestBench.h @@ -41,6 +41,7 @@ class TestBench { static constexpr uint32_t kFakeSolenoid2Channel = 13; static constexpr uint32_t kFakeRelayForward = 18; static constexpr uint32_t kFakeRelayReverse = 19; + static constexpr uint32_t kFakePwmOutput = 14; /* Relay channels */ static constexpr uint32_t kRelayChannel = 0; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java index feb33d6135..d445e9f537 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java @@ -50,7 +50,7 @@ public class Counter implements CounterBase, Sendable, AutoCloseable { protected DigitalSource m_downSource; // /< What makes the counter count down. private boolean m_allocatedUpSource; private boolean m_allocatedDownSource; - private int m_counter; // /< The FPGA counter object. + int m_counter; // /< The FPGA counter object. private int m_index; // /< The index of this counter. private double m_distancePerPulse; // distance of travel for each tick diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMA.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMA.java new file mode 100644 index 0000000000..a919da55f0 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMA.java @@ -0,0 +1,110 @@ +// 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; + +public class DMA implements AutoCloseable { + final int m_dmaHandle; + + public DMA() { + m_dmaHandle = DMAJNI.initialize(); + } + + @Override + public void close() { + DMAJNI.free(m_dmaHandle); + } + + public void setPause(boolean pause) { + DMAJNI.setPause(m_dmaHandle, pause); + } + + public void setTimedTrigger(double periodSeconds) { + DMAJNI.setTimedTrigger(m_dmaHandle, periodSeconds); + } + + public void setTimedTriggerCycles(int cycles) { + DMAJNI.setTimedTriggerCycles(m_dmaHandle, cycles); + } + + public void addEncoder(Encoder encoder) { + DMAJNI.addEncoder(m_dmaHandle, encoder.m_encoder); + } + + public void addEncoderPeriod(Encoder encoder) { + DMAJNI.addEncoderPeriod(m_dmaHandle, encoder.m_encoder); + } + + public void addCounter(Counter counter) { + DMAJNI.addCounter(m_dmaHandle, counter.m_counter); + } + + public void addCounterPeriod(Counter counter) { + DMAJNI.addCounterPeriod(m_dmaHandle, counter.m_counter); + } + + public void addDigitalSource(DigitalSource digitalSource) { + DMAJNI.addDigitalSource(m_dmaHandle, digitalSource.getPortHandleForRouting()); + } + + public void addDutyCycle(DutyCycle dutyCycle) { + DMAJNI.addDutyCycle(m_dmaHandle, dutyCycle.m_handle); + } + + public void addAnalogInput(AnalogInput analogInput) { + DMAJNI.addAnalogInput(m_dmaHandle, analogInput.m_port); + } + + public void addAveragedAnalogInput(AnalogInput analogInput) { + DMAJNI.addAveragedAnalogInput(m_dmaHandle, analogInput.m_port); + } + + 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); + } + + public int setPwmEdgeTrigger(PWMMotorController pwm, boolean rising, boolean falling) { + return DMAJNI.setExternalTrigger(m_dmaHandle, pwm.getPwmHandle(), 0, rising, falling); + } + + public int setPwmEdgeTrigger(PWM pwm, boolean rising, boolean falling) { + return DMAJNI.setExternalTrigger(m_dmaHandle, pwm.getHandle(), 0, rising, falling); + } + + public void clearSensors() { + DMAJNI.clearSensors(m_dmaHandle); + } + + public void clearExternalTriggers() { + DMAJNI.clearExternalTriggers(m_dmaHandle); + } + + public void start(int queueDepth) { + DMAJNI.startDMA(m_dmaHandle, queueDepth); + } + + public void stop() { + DMAJNI.stopDMA(m_dmaHandle); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMASample.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMASample.java new file mode 100644 index 0000000000..1055dbcc40 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMASample.java @@ -0,0 +1,126 @@ +// 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; + +public class DMASample { + public enum DMAReadStatus { + kOk(1), + kTimeout(2), + kError(3); + + private final int value; + + DMAReadStatus(int value) { + this.value = 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(); + + public DMAReadStatus update(DMA dma, double timeoutSeconds) { + return DMAReadStatus.getValue(m_dmaSample.update(dma.m_dmaHandle, timeoutSeconds)); + } + + public int getCaptureSize() { + return m_dmaSample.getCaptureSize(); + } + + public int getTriggerChannels() { + return m_dmaSample.getTriggerChannels(); + } + + public int getRemaining() { + return m_dmaSample.getRemaining(); + } + + public long getTime() { + return m_dmaSample.getTime(); + } + + public double getTimeStamp() { + return getTime() * 1.0e-6; + } + + public int getEncoderRaw(Encoder encoder) { + return m_dmaSample.getEncoder(encoder.m_encoder); + } + + /** + * Gets the scaled encoder distance for this sample. + * + * @param encoder the encoder to use to read + * @return the distance + */ + public double getEncoderDistance(Encoder encoder) { + double val = getEncoderRaw(encoder); + val *= encoder.getDecodingScaleFactor(); + val *= encoder.getDistancePerPulse(); + return val; + } + + public int getEncoderPeriodRaw(Encoder encoder) { + return m_dmaSample.getEncoderPeriod(encoder.m_encoder); + } + + public int getCounter(Counter counter) { + return m_dmaSample.getCounter(counter.m_counter); + } + + public int getCounterPeriod(Counter counter) { + return m_dmaSample.getCounterPeriod(counter.m_counter); + } + + public boolean getDigitalSource(DigitalSource digitalSource) { + return m_dmaSample.getDigitalSource(digitalSource.getPortHandleForRouting()); + } + + public int getAnalogInputRaw(AnalogInput analogInput) { + return m_dmaSample.getAnalogInput(analogInput.m_port); + } + + public double getAnalogInputVoltage(AnalogInput analogInput) { + return AnalogJNI.getAnalogValueToVolts(analogInput.m_port, getAnalogInputRaw(analogInput)); + } + + public int getAveragedAnalogInputRaw(AnalogInput analogInput) { + return m_dmaSample.getAnalogInputAveraged(analogInput.m_port); + } + + public double getAveragedAnalogInputVoltage(AnalogInput analogInput) { + return AnalogJNI.getAnalogValueToVolts( + analogInput.m_port, getAveragedAnalogInputRaw(analogInput)); + } + + public int getDutyCycleOutputRaw(DutyCycle dutyCycle) { + return m_dmaSample.getDutyCycleOutput(dutyCycle.m_handle); + } + + public double getDutyCycleOutput(DutyCycle dutyCycle) { + return m_dmaSample.getDutyCycleOutput(dutyCycle.m_handle) + / (double) dutyCycle.getOutputScaleFactor(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java index 6f7aacdabd..200d713065 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java @@ -52,8 +52,9 @@ public class Encoder implements CounterBase, Sendable, AutoCloseable { private boolean m_allocatedA; private boolean m_allocatedB; private boolean m_allocatedI; + private final EncodingType m_encodingType; - private int m_encoder; // the HAL encoder object + int m_encoder; // the HAL encoder object /** * Common initialization code for Encoders. This code allocates resources for Encoders and is @@ -131,6 +132,7 @@ public class Encoder implements CounterBase, Sendable, AutoCloseable { m_allocatedI = false; m_aSource = new DigitalInput(channelA); m_bSource = new DigitalInput(channelB); + m_encodingType = encodingType; SendableRegistry.addChild(this, m_aSource); SendableRegistry.addChild(this, m_bSource); initEncoder(reverseDirection, encodingType); @@ -230,6 +232,7 @@ public class Encoder implements CounterBase, Sendable, AutoCloseable { m_allocatedA = false; m_allocatedB = false; m_allocatedI = false; + m_encodingType = encodingType; m_aSource = sourceA; m_bSource = sourceB; initEncoder(reverseDirection, encodingType); @@ -540,6 +543,24 @@ public class Encoder implements CounterBase, Sendable, AutoCloseable { EncoderJNI.setEncoderSimDevice(m_encoder, device.getNativeHandle()); } + /** + * Gets the decoding scale factor for scaling raw values to full counts. + * + * @return decoding scale factor + */ + public double getDecodingScaleFactor() { + switch (m_encodingType) { + case k1X: + return 1.0; + case k2X: + return 0.5; + case k4X: + return 0.25; + default: + return 0.0; + } + } + @Override public void initSendable(SendableBuilder builder) { if (EncoderJNI.getEncoderEncodingType(m_encoder) == EncodingType.k4X.value) { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java index 133e5af399..f594d39fd5 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java @@ -37,8 +37,7 @@ public class PWM implements Sendable, AutoCloseable { private final int m_channel; - // Package private to use from AddressableLED - int m_handle; + private int m_handle; /** * Allocate a PWM given a channel. @@ -246,6 +245,15 @@ public class PWM implements Sendable, AutoCloseable { PWMJNI.latchPWMZero(m_handle); } + /** + * Get the underlying handle. + * + * @return Underlying PWM handle + */ + public int getHandle() { + return m_handle; + } + @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("PWM"); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java index 378e78ac9c..d33fb0133b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java @@ -86,6 +86,15 @@ public abstract class PWMMotorController extends MotorSafety return "PWM " + getChannel(); } + /** + * Gets the backing PWM handle. + * + * @return The pwm handle. + */ + public int getPwmHandle() { + return m_pwm.getHandle(); + } + /** * Gets the PWM channel number. * diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Main.java new file mode 100644 index 0000000000..97f67d1de2 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Main.java @@ -0,0 +1,25 @@ +// 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. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Robot.java new file mode 100644 index 0000000000..a7b088c735 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Robot.java @@ -0,0 +1,85 @@ +// 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 DMA m_dma; + private 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 DigitalOutput m_dmaTrigger; + + // Analog input to read with DMA + private AnalogInput m_analogInput; + + // Encoder to read with DMA + private Encoder m_encoder; + + @Override + public void robotInit() { + 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()); + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index bf45489497..7f9a365acd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -491,6 +491,17 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "DMA", + "description": "Demonstrates the use of the DMA class", + "tags": [ + "Advanced Java" + ], + "foldername": "dma", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "ArmBot", "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.", diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DMATest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DMATest.java new file mode 100644 index 0000000000..1da5c6fe81 --- /dev/null +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DMATest.java @@ -0,0 +1,161 @@ +// 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 static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertTrue; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture; +import edu.wpi.first.wpilibj.motorcontrol.Jaguar; +import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; +import edu.wpi.first.wpilibj.test.AbstractComsSetup; +import edu.wpi.first.wpilibj.test.TestBench; +import java.util.ArrayList; +import java.util.List; +import java.util.logging.Logger; +import org.junit.After; +import org.junit.Before; +import org.junit.Ignore; +import org.junit.Test; + +@Ignore +public class DMATest extends AbstractComsSetup { + private static final Logger logger = Logger.getLogger(DMATest.class.getName()); + + @Override + protected Logger getClassLogger() { + return logger; + } + + private AnalogCrossConnectFixture m_analogIO; + private DigitalOutput m_manualTrigger; + private PWMMotorController m_pwm; + private DMA m_dma; + private DMASample m_dmaSample; + + @Before + public void setUp() { + m_analogIO = TestBench.getAnalogCrossConnectFixture(); + m_manualTrigger = new DigitalOutput(7); + m_pwm = new Jaguar(14); + m_dma = new DMA(); + m_dmaSample = new DMASample(); + + m_dma.addAnalogInput(m_analogIO.getInput()); + m_dma.setExternalTrigger(m_manualTrigger, false, true); + m_manualTrigger.set(true); + } + + @After + public void tearDown() { + m_dma.close(); + m_manualTrigger.close(); + m_analogIO.teardown(); + m_pwm.close(); + } + + @Test + public void testPausingWorks() { + m_dma.start(1024); + m_dma.setPause(true); + m_manualTrigger.set(false); + + var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(5)); + + assertEquals(DMASample.DMAReadStatus.kTimeout, timedOut); + } + + @Test + public void testRemovingTriggersWorks() { + m_dma.clearExternalTriggers(); + m_dma.start(1024); + m_manualTrigger.set(false); + + var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(5)); + + assertEquals(DMASample.DMAReadStatus.kTimeout, timedOut); + } + + @Test + public void testManualTriggerOnlyHappensOnce() { + m_dma.start(1024); + m_manualTrigger.set(false); + var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(5)); + m_manualTrigger.set(true); + + assertEquals(DMASample.DMAReadStatus.kOk, timedOut); + assertEquals(0, m_dmaSample.getRemaining()); + timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(5)); + assertEquals(DMASample.DMAReadStatus.kTimeout, timedOut); + } + + @Test + public void testAnalogIndividualTriggers() { + m_dma.start(1024); + for (double i = 0; i < 5; i += 0.5) { + m_analogIO.getOutput().setVoltage(i); + // Need to sleep to ensure value sets + Timer.delay(AnalogCrossConnectTest.kDelayTime); + m_manualTrigger.set(false); + var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1)); + m_manualTrigger.set(true); + + assertEquals(DMASample.DMAReadStatus.kOk, timedOut); + assertEquals(0, m_dmaSample.getRemaining()); + assertEquals( + m_analogIO.getInput().getVoltage(), + m_dmaSample.getAnalogInputVoltage(m_analogIO.getInput()), + 0.01); + } + } + + @Test + public void testAnalogMultipleTriggers() { + m_dma.start(1024); + List values = new ArrayList<>(); + for (double i = 0; i < 5; i += 0.5) { + m_analogIO.getOutput().setVoltage(i); + values.add(i); + // Need to sleep to ensure value sets + Timer.delay(AnalogCrossConnectTest.kDelayTime); + m_manualTrigger.set(false); + Timer.delay(AnalogCrossConnectTest.kDelayTime); + m_manualTrigger.set(true); + } + + for (int i = 0; i < values.size(); i++) { + var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1)); + assertEquals(DMASample.DMAReadStatus.kOk, timedOut); + assertEquals(values.size() - i - 1, m_dmaSample.getRemaining()); + assertEquals(values.get(i), m_dmaSample.getAnalogInputVoltage(m_analogIO.getInput()), 0.01); + } + } + + @Test + public void testTimedTriggers() { + m_dma.setTimedTrigger(Units.millisecondsToSeconds(10)); + m_dma.start(1024); + Timer.delay(Units.millisecondsToSeconds(100)); + m_dma.setPause(true); + + var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1)); + assertEquals(DMASample.DMAReadStatus.kOk, timedOut); + assertTrue("Received more then 5 samples in 100 ms", m_dmaSample.getRemaining() > 5); + } + + @Test + public void testPwmTimedTriggers() { + m_dma.clearExternalTriggers(); + m_dma.setPwmEdgeTrigger(m_pwm, true, false); + m_dma.start(1024); + Timer.delay(Units.millisecondsToSeconds(100)); + m_dma.setPause(true); + + var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1)); + assertEquals(DMASample.DMAReadStatus.kOk, timedOut); + assertTrue("Received more then 5 samples in 100 ms", m_dmaSample.getRemaining() > 5); + } +} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java index 56aaef26f8..e7d968f9e2 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java @@ -22,6 +22,7 @@ import org.junit.runners.Suite.SuiteClasses; CounterTest.class, DigitalGlitchFilterTest.class, DIOCrossConnectTest.class, + DMATest.class, DriverStationTest.class, EncoderTest.class, GyroTest.class, diff --git a/wpimath/src/main/java/edu/wpi/first/math/util/Units.java b/wpimath/src/main/java/edu/wpi/first/math/util/Units.java index 204a95a570..13adcd8930 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/util/Units.java +++ b/wpimath/src/main/java/edu/wpi/first/math/util/Units.java @@ -9,6 +9,7 @@ public final class Units { private static final double kInchesPerFoot = 12.0; private static final double kMetersPerInch = 0.0254; private static final double kSecondsPerMinute = 60; + private static final double kMillisecondsPerSecond = 1000; private static final double kKilogramsPerLb = 0.453592; /** Utility class, so constructor is private. */ @@ -96,6 +97,26 @@ public final class Units { return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI; } + /** + * Converts given milliseconds to seconds. + * + * @param milliseconds The milliseconds to convert to seconds. + * @return Seconds converted from milliseconds. + */ + public static double millisecondsToSeconds(double milliseconds) { + return milliseconds / kMillisecondsPerSecond; + } + + /** + * Converts given seconds to milliseconds. + * + * @param seconds The seconds to convert to milliseconds. + * @return Milliseconds converted from seconds. + */ + public static double secondsToMilliseconds(double seconds) { + return seconds * kMillisecondsPerSecond; + } + /** * Converts kilograms into lbs (pound-mass). * diff --git a/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java b/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java index 89484039a2..99a8b5aa10 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java @@ -55,6 +55,15 @@ class UnitsTest extends UtilityClassTest { } @Test + void millisecondsToSeconds() { + assertEquals(0.5, Units.millisecondsToSeconds(500), 1e-2); + } + + @Test + void secondsToMilliseconds() { + assertEquals(1500, Units.secondsToMilliseconds(1.5), 1e-2); + } + void kilogramsToLbsTest() { assertEquals(2.20462, Units.kilogramsToLbs(1), 1e-2); }