diff --git a/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java b/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java index c45a053c9d..e2d7fccc59 100644 --- a/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -21,6 +21,8 @@ public class PDPJNI extends JNIWrapper { public static native double getPDPChannelCurrent(byte channel, int handle); + public static native void getPDPAllCurrents(int handle, double[] currents); + public static native double getPDPTotalCurrent(int handle); public static native double getPDPTotalPower(int handle); diff --git a/hal/src/main/native/athena/PDP.cpp b/hal/src/main/native/athena/PDP.cpp index f27f5da82d..709a4d1535 100644 --- a/hal/src/main/native/athena/PDP.cpp +++ b/hal/src/main/native/athena/PDP.cpp @@ -289,6 +289,75 @@ double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel, return raw * 0.125; /* 7.3 fixed pt value in Amps */ } +void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents, + int32_t* status) { + int32_t length = 0; + uint64_t receivedTimestamp = 0; + PdpStatus1 pdpStatus; + HAL_ReadCANPacketTimeout(handle, Status1, pdpStatus.data, &length, + &receivedTimestamp, TimeoutMs, status); + if (*status != 0) return; + PdpStatus2 pdpStatus2; + HAL_ReadCANPacketTimeout(handle, Status2, pdpStatus2.data, &length, + &receivedTimestamp, TimeoutMs, status); + if (*status != 0) return; + PdpStatus3 pdpStatus3; + HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus3.data, &length, + &receivedTimestamp, TimeoutMs, status); + if (*status != 0) return; + + currents[0] = ((static_cast(pdpStatus.bits.chan1_h8) << 2) | + pdpStatus.bits.chan1_l2) * + .125; + currents[1] = ((static_cast(pdpStatus.bits.chan2_h6) << 4) | + pdpStatus.bits.chan2_l4) * + .125; + currents[2] = ((static_cast(pdpStatus.bits.chan3_h4) << 6) | + pdpStatus.bits.chan3_l6) * + .125; + currents[3] = ((static_cast(pdpStatus.bits.chan4_h2) << 8) | + pdpStatus.bits.chan4_l8) * + .125; + currents[4] = ((static_cast(pdpStatus.bits.chan5_h8) << 2) | + pdpStatus.bits.chan5_l2) * + .125; + currents[5] = ((static_cast(pdpStatus.bits.chan6_h6) << 4) | + pdpStatus.bits.chan6_l4) * + .125; + + currents[6] = ((static_cast(pdpStatus2.bits.chan7_h8) << 2) | + pdpStatus2.bits.chan7_l2) * + .125; + currents[7] = ((static_cast(pdpStatus2.bits.chan8_h6) << 4) | + pdpStatus2.bits.chan8_l4) * + .125; + currents[8] = ((static_cast(pdpStatus2.bits.chan9_h4) << 6) | + pdpStatus2.bits.chan9_l6) * + .125; + currents[9] = ((static_cast(pdpStatus2.bits.chan10_h2) << 8) | + pdpStatus2.bits.chan10_l8) * + .125; + currents[10] = ((static_cast(pdpStatus2.bits.chan11_h8) << 2) | + pdpStatus2.bits.chan11_l2) * + .125; + currents[11] = ((static_cast(pdpStatus2.bits.chan12_h6) << 4) | + pdpStatus2.bits.chan12_l4) * + .125; + + currents[12] = ((static_cast(pdpStatus3.bits.chan13_h8) << 2) | + pdpStatus3.bits.chan13_l2) * + .125; + currents[13] = ((static_cast(pdpStatus3.bits.chan14_h6) << 4) | + pdpStatus3.bits.chan14_l4) * + .125; + currents[14] = ((static_cast(pdpStatus3.bits.chan15_h4) << 6) | + pdpStatus3.bits.chan15_l6) * + .125; + currents[15] = ((static_cast(pdpStatus3.bits.chan16_h2) << 8) | + pdpStatus3.bits.chan16_l8) * + .125; +} + double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) { PdpStatusEnergy pdpStatus; int32_t length = 0; diff --git a/hal/src/main/native/cpp/jni/PDPJNI.cpp b/hal/src/main/native/cpp/jni/PDPJNI.cpp index e8173be3d3..f649a8ddc4 100644 --- a/hal/src/main/native/cpp/jni/PDPJNI.cpp +++ b/hal/src/main/native/cpp/jni/PDPJNI.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -98,6 +98,25 @@ Java_edu_wpi_first_hal_PDPJNI_getPDPChannelCurrent return current; } +/* + * Class: edu_wpi_first_hal_PDPJNI + * Method: getPDPAllCurrents + * Signature: (I[D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_PDPJNI_getPDPAllCurrents + (JNIEnv* env, jclass, jint handle, jdoubleArray jarr) +{ + double storage[16]; + int32_t status = 0; + HAL_GetPDPAllChannelCurrents(handle, storage, &status); + if (!CheckStatus(env, status, false)) { + return; + } + + env->SetDoubleArrayRegion(jarr, 0, 16, storage); +} + /* * Class: edu_wpi_first_hal_PDPJNI * Method: getPDPTotalCurrent diff --git a/hal/src/main/native/include/hal/PDP.h b/hal/src/main/native/include/hal/PDP.h index 50873f8dbd..c80e8b69a0 100644 --- a/hal/src/main/native/include/hal/PDP.h +++ b/hal/src/main/native/include/hal/PDP.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -79,6 +79,17 @@ double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status); double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel, int32_t* status); +/** + * Gets the current of all 16 channels on the PDP. + * + * The array must be large enough to hold all channels. + * + * @param handle the module handle + * @param current the currents (output) + */ +void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents, + int32_t* status); + /** * Gets the total current of the PDP. * diff --git a/hal/src/main/native/include/mockdata/PCMData.h b/hal/src/main/native/include/mockdata/PCMData.h index 74a591ae00..66b1ec2d59 100644 --- a/hal/src/main/native/include/mockdata/PCMData.h +++ b/hal/src/main/native/include/mockdata/PCMData.h @@ -74,6 +74,9 @@ void HALSIM_CancelPCMCompressorCurrentCallback(int32_t index, int32_t uid); double HALSIM_GetPCMCompressorCurrent(int32_t index); void HALSIM_SetPCMCompressorCurrent(int32_t index, double compressorCurrent); +void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values); +void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values); + void HALSIM_RegisterPCMAllNonSolenoidCallbacks(int32_t index, HAL_NotifyCallback callback, void* param, diff --git a/hal/src/main/native/include/mockdata/PDPData.h b/hal/src/main/native/include/mockdata/PDPData.h index a25b66d2e5..8315e3c6aa 100644 --- a/hal/src/main/native/include/mockdata/PDPData.h +++ b/hal/src/main/native/include/mockdata/PDPData.h @@ -46,6 +46,9 @@ void HALSIM_CancelPDPCurrentCallback(int32_t index, int32_t channel, double HALSIM_GetPDPCurrent(int32_t index, int32_t channel); void HALSIM_SetPDPCurrent(int32_t index, int32_t channel, double current); +void HALSIM_GetPDPAllCurrents(int32_t index, double* currents); +void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents); + void HALSIM_RegisterPDPAllNonCurrentCallbacks(int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param, diff --git a/hal/src/main/native/include/simulation/PCMSim.h b/hal/src/main/native/include/simulation/PCMSim.h index 3f3ca9b1fc..b6fcd5a2bb 100644 --- a/hal/src/main/native/include/simulation/PCMSim.h +++ b/hal/src/main/native/include/simulation/PCMSim.h @@ -138,6 +138,16 @@ class PCMSim { HALSIM_SetPCMCompressorCurrent(m_index, compressorCurrent); } + uint8_t GetAllSolenoidOutputs() { + uint8_t ret = 0; + HALSIM_GetPCMAllSolenoids(m_index, &ret); + return ret; + } + + void SetAllSolenoidOutputs(uint8_t outputs) { + HALSIM_SetPCMAllSolenoids(m_index, outputs); + } + void ResetData() { HALSIM_ResetPCMData(m_index); } private: diff --git a/hal/src/main/native/include/simulation/PDPSim.h b/hal/src/main/native/include/simulation/PDPSim.h index e3ffd4b59a..72d8233f99 100644 --- a/hal/src/main/native/include/simulation/PDPSim.h +++ b/hal/src/main/native/include/simulation/PDPSim.h @@ -79,6 +79,14 @@ class PDPSim { HALSIM_SetPDPCurrent(m_index, channel, current); } + void GetAllCurrents(double* currents) { + HALSIM_GetPDPAllCurrents(m_index, currents); + } + + void SetAllCurrents(const double* currents) { + HALSIM_SetPDPAllCurrents(m_index, currents); + } + void ResetData() { HALSIM_ResetPDPData(m_index); } private: diff --git a/hal/src/main/native/sim/PDP.cpp b/hal/src/main/native/sim/PDP.cpp index 07f4dcd239..dbe09d4b2d 100644 --- a/hal/src/main/native/sim/PDP.cpp +++ b/hal/src/main/native/sim/PDP.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -78,6 +78,18 @@ double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel, } return SimPDPData[module].current[channel]; } +void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents, + int32_t* status) { + auto module = hal::can::GetCANModuleFromHandle(handle, status); + if (*status != 0) { + return; + } + + auto& data = SimPDPData[module]; + for (int i = 0; i < kNumPDPChannels; i++) { + currents[i] = data.current[i]; + } +} double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) { return 0.0; } diff --git a/hal/src/main/native/sim/mockdata/PCMData.cpp b/hal/src/main/native/sim/mockdata/PCMData.cpp index df68e04e8e..6193b05e0e 100644 --- a/hal/src/main/native/sim/mockdata/PCMData.cpp +++ b/hal/src/main/native/sim/mockdata/PCMData.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -49,6 +49,23 @@ DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, closedLoopEnabled) DEFINE_CAPI(HAL_Bool, PressureSwitch, pressureSwitch) DEFINE_CAPI(double, CompressorCurrent, compressorCurrent) +void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values) { + auto& data = SimPCMData[index].solenoidOutput; + uint8_t ret = 0; + for (int i = 0; i < kNumSolenoidChannels; i++) { + ret |= (data[i] << i); + } + *values = ret; +} + +void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values) { + auto& data = SimPCMData[index].solenoidOutput; + for (int i = 0; i < kNumSolenoidChannels; i++) { + data[i] = (values & 0x1) != 0; + values >>= 1; + } +} + #define REGISTER(NAME) \ SimPCMData[index].NAME.RegisterCallback(callback, param, initialNotify) diff --git a/hal/src/main/native/sim/mockdata/PDPData.cpp b/hal/src/main/native/sim/mockdata/PDPData.cpp index 9c6143eb88..1c150bb180 100644 --- a/hal/src/main/native/sim/mockdata/PDPData.cpp +++ b/hal/src/main/native/sim/mockdata/PDPData.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -42,6 +42,20 @@ DEFINE_CAPI(double, Voltage, voltage) HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(double, HALSIM, PDPCurrent, SimPDPData, current) +void HALSIM_GetPDPAllCurrents(int32_t index, double* currents) { + auto& data = SimPDPData[index].current; + for (int i = 0; i < kNumPDPChannels; i++) { + currents[i] = data[i]; + } +} + +void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents) { + auto& data = SimPDPData[index].current; + for (int i = 0; i < kNumPDPChannels; i++) { + data[i] = currents[i]; + } +} + #define REGISTER(NAME) \ SimPDPData[index].NAME.RegisterCallback(callback, param, initialNotify)