From 0a983eeeb81349bd7a5ea6bd6ad71832a0d43809 Mon Sep 17 00:00:00 2001 From: Thad House Date: Thu, 7 Jul 2016 21:31:45 -0700 Subject: [PATCH] Moves Gyros to the HAL (#131) --- hal/include/HAL/AnalogGyro.h | 32 +++ hal/include/HAL/HAL.h | 1 + hal/include/HAL/Handles.h | 2 + hal/lib/athena/AnalogGyro.cpp | 232 ++++++++++++++++++ hal/lib/athena/handles/HandlesInternal.h | 3 +- wpilibc/athena/include/AnalogGyro.h | 8 +- wpilibc/athena/include/AnalogInput.h | 1 + wpilibc/athena/src/AnalogGyro.cpp | 146 ++++++----- wpilibj/athena.gradle | 1 + wpilibj/src/athena/cpp/lib/AnalogGyroJNI.cpp | 214 ++++++++++++++++ .../edu/wpi/first/wpilibj/AnalogGyro.java | 82 ++----- .../wpi/first/wpilibj/hal/AnalogGyroJNI.java | 37 +++ 12 files changed, 632 insertions(+), 127 deletions(-) create mode 100644 hal/include/HAL/AnalogGyro.h create mode 100644 hal/lib/athena/AnalogGyro.cpp create mode 100644 wpilibj/src/athena/cpp/lib/AnalogGyroJNI.cpp create mode 100644 wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/AnalogGyroJNI.java diff --git a/hal/include/HAL/AnalogGyro.h b/hal/include/HAL/AnalogGyro.h new file mode 100644 index 0000000000..c33b82ac7a --- /dev/null +++ b/hal/include/HAL/AnalogGyro.h @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. 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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "HAL/Handles.h" + +extern "C" { +HalGyroHandle initializeAnalogGyro(HalAnalogInputHandle handle, + int32_t* status); +void setupAnalogGyro(HalGyroHandle handle, int32_t* status); +void freeAnalogGyro(HalGyroHandle handle); +void setAnalogGyroParameters(HalGyroHandle handle, + float voltsPerDegreePerSecond, float offset, + uint32_t center, int32_t* status); +void setAnalogGyroVoltsPerDegreePerSecond(HalGyroHandle handle, + float voltsPerDegreePerSecond, + int32_t* status); +void resetAnalogGyro(HalGyroHandle handle, int32_t* status); +void calibrateAnalogGyro(HalGyroHandle handle, int32_t* status); +void setAnalogGyroDeadband(HalGyroHandle handle, float volts, int32_t* status); +float getAnalogGyroAngle(HalGyroHandle handle, int32_t* status); +double getAnalogGyroRate(HalGyroHandle handle, int32_t* status); +float getAnalogGyroOffset(HalGyroHandle handle, int32_t* status); +uint32_t getAnalogGyroCenter(HalGyroHandle handle, int32_t* status); +} diff --git a/hal/include/HAL/HAL.h b/hal/include/HAL/HAL.h index 11dd48f58b..c60b3f917d 100644 --- a/hal/include/HAL/HAL.h +++ b/hal/include/HAL/HAL.h @@ -11,6 +11,7 @@ #include "Accelerometer.h" #include "AnalogAccumulator.h" +#include "AnalogGyro.h" #include "AnalogInput.h" #include "AnalogOutput.h" #include "AnalogTrigger.h" diff --git a/hal/include/HAL/Handles.h b/hal/include/HAL/Handles.h index e01508837a..d1e6ada962 100644 --- a/hal/include/HAL/Handles.h +++ b/hal/include/HAL/Handles.h @@ -40,3 +40,5 @@ typedef HalHandle HalSolenoidHandle; typedef HalHandle HalFPGAEncoderHandle; typedef HalHandle HalEncoderHandle; + +typedef HalHandle HalGyroHandle; diff --git a/hal/lib/athena/AnalogGyro.cpp b/hal/lib/athena/AnalogGyro.cpp new file mode 100644 index 0000000000..5765536a0e --- /dev/null +++ b/hal/lib/athena/AnalogGyro.cpp @@ -0,0 +1,232 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. 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. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/AnalogGyro.h" + +#include +#include + +#include "AnalogInternal.h" +#include "HAL/AnalogAccumulator.h" +#include "HAL/AnalogInput.h" +#include "handles/IndexedHandleResource.h" + +namespace { +struct AnalogGyro { + HalAnalogInputHandle handle; + float voltsPerDegreePerSecond; + float offset; + uint32_t center; +}; +} + +static constexpr uint32_t kOversampleBits = 10; +static constexpr uint32_t kAverageBits = 0; +static constexpr float kSamplesPerSecond = 50.0; +static constexpr float kCalibrationSampleTime = 5.0; +static constexpr float kDefaultVoltsPerDegreePerSecond = 0.007; + +using namespace hal; + +static IndexedHandleResource + analogGyroHandles; + +static void Wait(double seconds) { + if (seconds < 0.0) return; + std::this_thread::sleep_for(std::chrono::duration(seconds)); +} + +extern "C" { +HalGyroHandle initializeAnalogGyro(HalAnalogInputHandle analog_handle, + int32_t* status) { + if (!isAccumulatorChannel(analog_handle, status)) { + if (*status == 0) { + *status = PARAMETER_OUT_OF_RANGE; + } + return HAL_INVALID_HANDLE; + } + + // handle known to be correct, so no need to type check + int16_t channel = getHandleIndex(analog_handle); + + auto handle = analogGyroHandles.Allocate(channel, status); + + if (*status != 0) + return HAL_INVALID_HANDLE; // failed to allocate. Pass error back. + + // Initialize port structure + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { // would only error on thread issue + *status = HAL_HANDLE_ERROR; + return HAL_INVALID_HANDLE; + } + + gyro->handle = analog_handle; + gyro->voltsPerDegreePerSecond = 0; + gyro->offset = 0; + gyro->center = 0; + + return handle; +} + +void setupAnalogGyro(HalGyroHandle handle, int32_t* status) { + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + gyro->voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond; + + setAnalogAverageBits(gyro->handle, kAverageBits, status); + if (*status != 0) return; + setAnalogOversampleBits(gyro->handle, kOversampleBits, status); + if (*status != 0) return; + float sampleRate = + kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits)); + setAnalogSampleRate(sampleRate, status); + if (*status != 0) return; + Wait(0.1); + + setAnalogGyroDeadband(handle, 0.0f, status); + if (*status != 0) return; +} + +void freeAnalogGyro(HalGyroHandle handle) { analogGyroHandles.Free(handle); } + +void setAnalogGyroParameters(HalGyroHandle handle, + float voltsPerDegreePerSecond, float offset, + uint32_t center, int32_t* status) { + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + gyro->voltsPerDegreePerSecond = voltsPerDegreePerSecond; + gyro->offset = offset; + gyro->center = center; + setAccumulatorCenter(gyro->handle, center, status); +} + +void setAnalogGyroVoltsPerDegreePerSecond(HalGyroHandle handle, + float voltsPerDegreePerSecond, + int32_t* status) { + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + gyro->voltsPerDegreePerSecond = voltsPerDegreePerSecond; +} + +void resetAnalogGyro(HalGyroHandle handle, int32_t* status) { + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + resetAccumulator(gyro->handle, status); + if (*status != 0) return; + + const float sampleTime = 1.0f / getAnalogSampleRate(status); + const float overSamples = 1 << getAnalogOversampleBits(gyro->handle, status); + const float averageSamples = 1 << getAnalogAverageBits(gyro->handle, status); + if (*status != 0) return; + Wait(sampleTime * overSamples * averageSamples); +} + +void calibrateAnalogGyro(HalGyroHandle handle, int32_t* status) { + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + initAccumulator(gyro->handle, status); + if (*status != 0) return; + Wait(kCalibrationSampleTime); + + int64_t value; + uint32_t count; + getAccumulatorOutput(gyro->handle, &value, &count, status); + if (*status != 0) return; + + gyro->center = (uint32_t)((float)value / (float)count + .5); + + gyro->offset = ((float)value / (float)count) - (float)gyro->center; + setAccumulatorCenter(gyro->handle, gyro->center, status); + if (*status != 0) return; + resetAnalogGyro(handle, status); +} + +void setAnalogGyroDeadband(HalGyroHandle handle, float volts, int32_t* status) { + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + int32_t deadband = volts * 1e9 / getAnalogLSBWeight(gyro->handle, status) * + (1 << getAnalogOversampleBits(gyro->handle, status)); + if (*status != 0) return; + setAccumulatorDeadband(gyro->handle, deadband, status); +} + +float getAnalogGyroAngle(HalGyroHandle handle, int32_t* status) { + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + int64_t rawValue = 0; + uint32_t count = 0; + getAccumulatorOutput(gyro->handle, &rawValue, &count, status); + + int64_t value = rawValue - (int64_t)((float)count * gyro->offset); + + double scaledValue = + value * 1e-9 * (double)getAnalogLSBWeight(gyro->handle, status) * + (double)(1 << getAnalogAverageBits(gyro->handle, status)) / + (getAnalogSampleRate(status) * gyro->voltsPerDegreePerSecond); + + return (float)scaledValue; +} + +double getAnalogGyroRate(HalGyroHandle handle, int32_t* status) { + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + return (getAnalogAverageValue(gyro->handle, status) - + ((double)gyro->center + gyro->offset)) * + 1e-9 * getAnalogLSBWeight(gyro->handle, status) / + ((1 << getAnalogOversampleBits(gyro->handle, status)) * + gyro->voltsPerDegreePerSecond); +} + +float getAnalogGyroOffset(HalGyroHandle handle, int32_t* status) { + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + return gyro->offset; +} + +uint32_t getAnalogGyroCenter(HalGyroHandle handle, int32_t* status) { + auto gyro = analogGyroHandles.Get(handle); + if (gyro == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + return gyro->center; +} +} diff --git a/hal/lib/athena/handles/HandlesInternal.h b/hal/lib/athena/handles/HandlesInternal.h index 37a48984a6..babcb8758c 100644 --- a/hal/lib/athena/handles/HandlesInternal.h +++ b/hal/lib/athena/handles/HandlesInternal.h @@ -41,7 +41,8 @@ enum class HalHandleEnum { FPGAEncoder = 12, Encoder = 13, Compressor = 14, - Solenoid = 15 + Solenoid = 15, + AnalogGyro = 16 }; static inline int16_t getHandleIndex(HalHandle handle) { diff --git a/wpilibc/athena/include/AnalogGyro.h b/wpilibc/athena/include/AnalogGyro.h index 0f45c44a66..28a2ca624f 100644 --- a/wpilibc/athena/include/AnalogGyro.h +++ b/wpilibc/athena/include/AnalogGyro.h @@ -9,6 +9,8 @@ #include "GyroBase.h" +#include "HAL/Handles.h" + class AnalogInput; /** @@ -38,7 +40,7 @@ class AnalogGyro : public GyroBase { AnalogGyro(int32_t channel, uint32_t center, float offset); AnalogGyro(std::shared_ptr channel, uint32_t center, float offset); - virtual ~AnalogGyro() = default; + virtual ~AnalogGyro(); float GetAngle() const override; double GetRate() const override; @@ -54,7 +56,5 @@ class AnalogGyro : public GyroBase { std::shared_ptr m_analog; private: - float m_voltsPerDegreePerSecond; - float m_offset; - uint32_t m_center; + HalGyroHandle m_gyroHandle = HAL_INVALID_HANDLE; }; diff --git a/wpilibc/athena/include/AnalogInput.h b/wpilibc/athena/include/AnalogInput.h index e7922da9ad..3aedbfdbdd 100644 --- a/wpilibc/athena/include/AnalogInput.h +++ b/wpilibc/athena/include/AnalogInput.h @@ -30,6 +30,7 @@ class AnalogInput : public SensorBase, public PIDSource, public LiveWindowSendable { friend class AnalogTrigger; + friend class AnalogGyro; public: static const uint8_t kAccumulatorModuleNumber = 1; diff --git a/wpilibc/athena/src/AnalogGyro.cpp b/wpilibc/athena/src/AnalogGyro.cpp index 8e193f1745..7e68a88347 100644 --- a/wpilibc/athena/src/AnalogGyro.cpp +++ b/wpilibc/athena/src/AnalogGyro.cpp @@ -79,10 +79,15 @@ AnalogGyro::AnalogGyro(std::shared_ptr channel) AnalogGyro::AnalogGyro(int32_t channel, uint32_t center, float offset) { m_analog = std::make_shared(channel); InitGyro(); - m_center = center; - m_offset = offset; - m_analog->SetAccumulatorCenter(m_center); - m_analog->ResetAccumulator(); + int32_t status = 0; + setAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond, offset, + center, &status); + if (status != 0) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + m_gyroHandle = HAL_INVALID_HANDLE; + return; + } + Reset(); } /** @@ -103,13 +108,24 @@ AnalogGyro::AnalogGyro(std::shared_ptr channel, uint32_t center, wpi_setWPIError(NullParameter); } else { InitGyro(); - m_center = center; - m_offset = offset; - m_analog->SetAccumulatorCenter(m_center); - m_analog->ResetAccumulator(); + int32_t status = 0; + setAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond, + offset, center, &status); + if (status != 0) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + m_gyroHandle = HAL_INVALID_HANDLE; + return; + } + Reset(); } } +/** + * AnalogGyro Destructor + * + */ +AnalogGyro::~AnalogGyro() { freeAnalogGyro(m_gyroHandle); } + /** * Reset the gyro. * @@ -119,7 +135,9 @@ AnalogGyro::AnalogGyro(std::shared_ptr channel, uint32_t center, */ void AnalogGyro::Reset() { if (StatusIsFatal()) return; - m_analog->ResetAccumulator(); + int32_t status = 0; + resetAnalogGyro(m_gyroHandle, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); } /** @@ -127,25 +145,32 @@ void AnalogGyro::Reset() { */ void AnalogGyro::InitGyro() { if (StatusIsFatal()) return; - - if (!m_analog->IsAccumulatorChannel()) { - wpi_setWPIErrorWithContext(ParameterOutOfRange, - " channel (must be accumulator channel)"); - m_analog = nullptr; - return; + if (m_gyroHandle == HAL_INVALID_HANDLE) { + int32_t status = 0; + m_gyroHandle = initializeAnalogGyro(m_analog->m_port, &status); + if (status == PARAMETER_OUT_OF_RANGE) { + wpi_setWPIErrorWithContext(ParameterOutOfRange, + " channel (must be accumulator channel)"); + m_analog = nullptr; + m_gyroHandle = HAL_INVALID_HANDLE; + return; + } + if (status != 0) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + m_analog = nullptr; + m_gyroHandle = HAL_INVALID_HANDLE; + return; + } } - m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond; - m_analog->SetAverageBits(kAverageBits); - m_analog->SetOversampleBits(kOversampleBits); - float sampleRate = - kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits)); - m_analog->SetSampleRate(sampleRate); - Wait(0.1); - - SetDeadband(0.0f); - - SetPIDSourceType(PIDSourceType::kDisplacement); + int32_t status = 0; + setupAnalogGyro(m_gyroHandle, &status); + if (status != 0) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + m_analog = nullptr; + m_gyroHandle = HAL_INVALID_HANDLE; + return; + } HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel()); LiveWindow::GetInstance()->AddSensor("AnalogGyro", m_analog->GetChannel(), @@ -154,20 +179,9 @@ void AnalogGyro::InitGyro() { void AnalogGyro::Calibrate() { if (StatusIsFatal()) return; - - m_analog->InitAccumulator(); - - Wait(kCalibrationSampleTime); - - int64_t value; - uint32_t count; - m_analog->GetAccumulatorOutput(value, count); - - m_center = (uint32_t)((float)value / (float)count + .5); - - m_offset = ((float)value / (float)count) - (float)m_center; - m_analog->SetAccumulatorCenter(m_center); - m_analog->ResetAccumulator(); + int32_t status = 0; + calibrateAnalogGyro(m_gyroHandle, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); } /** @@ -184,18 +198,10 @@ void AnalogGyro::Calibrate() { */ float AnalogGyro::GetAngle() const { if (StatusIsFatal()) return 0.f; - - int64_t rawValue; - uint32_t count; - m_analog->GetAccumulatorOutput(rawValue, count); - - int64_t value = rawValue - (int64_t)((float)count * m_offset); - - double scaledValue = value * 1e-9 * (double)m_analog->GetLSBWeight() * - (double)(1 << m_analog->GetAverageBits()) / - (m_analog->GetSampleRate() * m_voltsPerDegreePerSecond); - - return (float)scaledValue; + int32_t status = 0; + float value = getAnalogGyroAngle(m_gyroHandle, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + return value; } /** @@ -207,10 +213,10 @@ float AnalogGyro::GetAngle() const { */ double AnalogGyro::GetRate() const { if (StatusIsFatal()) return 0.0; - - return (m_analog->GetAverageValue() - ((double)m_center + m_offset)) * 1e-9 * - m_analog->GetLSBWeight() / - ((1 << m_analog->GetOversampleBits()) * m_voltsPerDegreePerSecond); + int32_t status = 0; + double value = getAnalogGyroRate(m_gyroHandle, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + return value; } /** @@ -219,7 +225,13 @@ double AnalogGyro::GetRate() const { * * @return the current offset value */ -float AnalogGyro::GetOffset() const { return m_offset; } +float AnalogGyro::GetOffset() const { + if (StatusIsFatal()) return 0.0; + int32_t status = 0; + float value = getAnalogGyroOffset(m_gyroHandle, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + return value; +} /** * Return the gyro center value. If run after calibration, @@ -227,7 +239,13 @@ float AnalogGyro::GetOffset() const { return m_offset; } * * @return the current center value */ -uint32_t AnalogGyro::GetCenter() const { return m_center; } +uint32_t AnalogGyro::GetCenter() const { + if (StatusIsFatal()) return 0; + int32_t status = 0; + uint32_t value = getAnalogGyroCenter(m_gyroHandle, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + return value; +} /** * Set the gyro sensitivity. @@ -239,7 +257,10 @@ uint32_t AnalogGyro::GetCenter() const { return m_center; } * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second */ void AnalogGyro::SetSensitivity(float voltsPerDegreePerSecond) { - m_voltsPerDegreePerSecond = voltsPerDegreePerSecond; + int32_t status = 0; + setAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle, voltsPerDegreePerSecond, + &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); } /** @@ -253,8 +274,7 @@ void AnalogGyro::SetSensitivity(float voltsPerDegreePerSecond) { */ void AnalogGyro::SetDeadband(float volts) { if (StatusIsFatal()) return; - - int32_t deadband = volts * 1e9 / m_analog->GetLSBWeight() * - (1 << m_analog->GetOversampleBits()); - m_analog->SetAccumulatorDeadband(deadband); + int32_t status = 0; + setAnalogGyroDeadband(m_gyroHandle, volts, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); } diff --git a/wpilibj/athena.gradle b/wpilibj/athena.gradle index 9fc4dd4972..659587dd0b 100644 --- a/wpilibj/athena.gradle +++ b/wpilibj/athena.gradle @@ -158,6 +158,7 @@ task jniHeaders { args 'edu.wpi.first.wpilibj.hal.JNIWrapper' args 'edu.wpi.first.wpilibj.hal.AccelerometerJNI' args 'edu.wpi.first.wpilibj.hal.AnalogJNI' + args 'edu.wpi.first.wpilibj.hal.AnalogGyroJNI' args 'edu.wpi.first.wpilibj.hal.CanTalonJNI' args 'edu.wpi.first.wpilibj.hal.CounterJNI' args 'edu.wpi.first.wpilibj.hal.DigitalGlitchFilterJNI' diff --git a/wpilibj/src/athena/cpp/lib/AnalogGyroJNI.cpp b/wpilibj/src/athena/cpp/lib/AnalogGyroJNI.cpp new file mode 100644 index 0000000000..e0e8637f7d --- /dev/null +++ b/wpilibj/src/athena/cpp/lib/AnalogGyroJNI.cpp @@ -0,0 +1,214 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. 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. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include "Log.h" + +#include "edu_wpi_first_wpilibj_hal_AnalogGyroJNI.h" + +#include "HAL/AnalogGyro.h" +#include "HALUtil.h" + +// set the logging level +TLogLevel analogGyroJNILogLevel = logWARNING; + +#define ANALOGGYROJNI_LOG(level) \ + if (level > analogGyroJNILogLevel) \ + ; \ + else \ + Log().Get(level) + +extern "C" { +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: initializeAnalogGyro + * Signature: (I)I + */ +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_initializeAnalogGyro( + JNIEnv* env, jclass, jint id) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI initializeAnalogGyro"; + ANALOGGYROJNI_LOG(logDEBUG) << "Analog Input Handle = " << (HalAnalogInputHandle)id; + int32_t status = 0; + HalGyroHandle handle = initializeAnalogGyro((HalAnalogInputHandle)id, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << handle; + return (jint) handle; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: setupAnalogGyro + * Signature: (I)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_setupAnalogGyro( + JNIEnv* env, jclass, jint id) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI setupAnalogGyro"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + int32_t status = 0; + setupAnalogGyro((HalGyroHandle)id, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: freeAnalogGyro + * Signature: (I)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_freeAnalogGyro( + JNIEnv* env, jclass, jint id) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI freeAnalogGyro"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + freeAnalogGyro((HalGyroHandle)id); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: setAnalogGyroParameters + * Signature: (IFFI)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_setAnalogGyroParameters( + JNIEnv* env, jclass, jint id, jfloat vPDPS, jfloat offset, jint center) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI setAnalogGyroParameters"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + int32_t status = 0; + setAnalogGyroParameters((HalGyroHandle)id, vPDPS, offset, center, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: setAnalogGyroVoltsPerDegreePerSecond + * Signature: (IF)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_setAnalogGyroVoltsPerDegreePerSecond( + JNIEnv* env, jclass, jint id, jfloat vPDPS) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI setAnalogGyroVoltsPerDegreePerSecond"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + ANALOGGYROJNI_LOG(logDEBUG) << "vPDPS = " << vPDPS; + int32_t status = 0; + setAnalogGyroVoltsPerDegreePerSecond((HalGyroHandle)id, vPDPS, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: resetAnalogGyro + * Signature: (I)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_resetAnalogGyro( + JNIEnv* env, jclass, jint id) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI resetAnalogGyro"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + int32_t status = 0; + resetAnalogGyro((HalGyroHandle)id, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: calibrateAnalogGyro + * Signature: (I)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_calibrateAnalogGyro( + JNIEnv* env, jclass, jint id) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI calibrateAnalogGyro"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + int32_t status = 0; + calibrateAnalogGyro((HalGyroHandle)id, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: setAnalogGyroDeadband + * Signature: (IF)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_setAnalogGyroDeadband( + JNIEnv* env, jclass, jint id, jfloat deadband) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI setAnalogGyroDeadband"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + int32_t status = 0; + setAnalogGyroDeadband((HalGyroHandle)id, deadband, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: getAnalogGyroAngle + * Signature: (I)F + */ +JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_getAnalogGyroAngle( + JNIEnv* env, jclass, jint id) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI getAnalogGyroAngle"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + int32_t status = 0; + jfloat value = getAnalogGyroAngle((HalGyroHandle)id, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGGYROJNI_LOG(logDEBUG) << "Result = " << value; + CheckStatus(env, status); + return value; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: getAnalogGyroRate + * Signature: (I)D + */ +JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_getAnalogGyroRate( + JNIEnv* env, jclass, jint id) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI getAnalogGyroRate"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + int32_t status = 0; + jdouble value = getAnalogGyroRate((HalGyroHandle)id, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGGYROJNI_LOG(logDEBUG) << "Result = " << value; + CheckStatus(env, status); + return value; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: getAnalogGyroOffset + * Signature: (I)F + */ +JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_getAnalogGyroOffset( + JNIEnv* env, jclass, jint id) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI getAnalogGyroOffset"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + int32_t status = 0; + jfloat value = getAnalogGyroOffset((HalGyroHandle)id, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGGYROJNI_LOG(logDEBUG) << "Result = " << value; + CheckStatus(env, status); + return value; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_AnalogGyroJNI + * Method: getAnalogGyroCenter + * Signature: (I)I + */ +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_getAnalogGyroCenter( + JNIEnv* env, jclass, jint id) { + ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI getAnalogGyroCenter"; + ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id; + int32_t status = 0; + jint value = getAnalogGyroCenter((HalGyroHandle)id, &status); + ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGGYROJNI_LOG(logDEBUG) << "Result = " << value; + CheckStatus(env, status); + return value; +} + +} // extern "C" diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java index e643a55d9b..89acff3d51 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java @@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType; import edu.wpi.first.wpilibj.communication.UsageReporting; +import edu.wpi.first.wpilibj.hal.AnalogGyroJNI; import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; @@ -24,34 +25,22 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; */ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowSendable { - private static final int kOversampleBits = 10; - private static final int kAverageBits = 0; - private static final double kSamplesPerSecond = 50.0; - private static final double kCalibrationSampleTime = 5.0; - private static final double kDefaultVoltsPerDegreePerSecond = 0.007; + private static final float kDefaultVoltsPerDegreePerSecond = 0.007f; protected AnalogInput m_analog; - private double m_voltsPerDegreePerSecond; - private double m_offset; - private int m_center; private boolean m_channelAllocated = false; - private AccumulatorResult m_result; + + private int m_gyroHandle = 0; /** * Initialize the gyro. Calibration is handled by calibrate(). */ public void initGyro() { - m_result = new AccumulatorResult(); + + if (m_gyroHandle == 0) { + m_gyroHandle = AnalogGyroJNI.initializeAnalogGyro(m_analog.m_port); + } - m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond; - m_analog.setAverageBits(kAverageBits); - m_analog.setOversampleBits(kOversampleBits); - double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits)); - AnalogInput.setGlobalSampleRate(sampleRate); - Timer.delay(0.1); - - setDeadband(0.0); - - setPIDSourceType(PIDSourceType.kDisplacement); + AnalogGyroJNI.setupAnalogGyro(m_gyroHandle); UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel()); LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this); @@ -59,19 +48,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS @Override public void calibrate() { - m_analog.initAccumulator(); - m_analog.resetAccumulator(); - - Timer.delay(kCalibrationSampleTime); - - m_analog.getAccumulatorOutput(m_result); - - m_center = (int) ((double) m_result.value / (double) m_result.count + .5); - - m_offset = ((double) m_result.value / (double) m_result.count) - m_center; - - m_analog.setAccumulatorCenter(m_center); - m_analog.resetAccumulator(); + AnalogGyroJNI.calibrateAnalogGyro(m_gyroHandle); } /** @@ -130,18 +107,14 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS throw new NullPointerException("AnalogInput supplied to Gyro constructor is null"); } initGyro(); - m_offset = offset; - m_center = center; - - m_analog.setAccumulatorCenter(m_center); - m_analog.resetAccumulator(); + AnalogGyroJNI.setAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond, + (float)offset, center); + reset(); } @Override public void reset() { - if (m_analog != null) { - m_analog.resetAccumulator(); - } + AnalogGyroJNI.resetAnalogGyro(m_gyroHandle); } /** @@ -153,22 +126,15 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS m_analog.free(); } m_analog = null; + AnalogGyroJNI.freeAnalogGyro(m_gyroHandle); } @Override - public synchronized double getAngle() { + public double getAngle() { if (m_analog == null) { return 0.0; } else { - m_analog.getAccumulatorOutput(m_result); - - long value = m_result.value - (long) (m_result.count * m_offset); - - double scaledValue = - value * 1e-9 * m_analog.getLSBWeight() * (1 << m_analog.getAverageBits()) - / (AnalogInput.getGlobalSampleRate() * m_voltsPerDegreePerSecond); - - return scaledValue; + return AnalogGyroJNI.getAnalogGyroAngle(m_gyroHandle); } } @@ -177,8 +143,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS if (m_analog == null) { return 0.0; } else { - return (m_analog.getAverageValue() - (m_center + m_offset)) * 1e-9 * m_analog.getLSBWeight() - / ((1 << m_analog.getOversampleBits()) * m_voltsPerDegreePerSecond); + return AnalogGyroJNI.getAnalogGyroRate(m_gyroHandle); } } @@ -188,7 +153,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS * @return the current offset value */ public double getOffset() { - return m_offset; + return AnalogGyroJNI.getAnalogGyroOffset(m_gyroHandle); } /** @@ -197,7 +162,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS * @return the current center value */ public int getCenter() { - return m_center; + return AnalogGyroJNI.getAnalogGyroCenter(m_gyroHandle); } /** @@ -208,7 +173,8 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second. */ public void setSensitivity(double voltsPerDegreePerSecond) { - m_voltsPerDegreePerSecond = voltsPerDegreePerSecond; + AnalogGyroJNI.setAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle, + (float)voltsPerDegreePerSecond); } /** @@ -219,8 +185,6 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS * @param volts The size of the deadband in volts */ void setDeadband(double volts) { - int deadband = - (int) (volts * 1e9 / m_analog.getLSBWeight() * (1 << m_analog.getOversampleBits())); - m_analog.setAccumulatorDeadband(deadband); + AnalogGyroJNI.setAnalogGyroDeadband(m_gyroHandle, (float)volts); } } diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/AnalogGyroJNI.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/AnalogGyroJNI.java new file mode 100644 index 0000000000..fe3a771fae --- /dev/null +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/AnalogGyroJNI.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.hal; + +public class AnalogGyroJNI extends JNIWrapper { + public static native int initializeAnalogGyro(int halAnalogInputHandle); + + public static native void setupAnalogGyro(int handle); + + public static native void freeAnalogGyro(int handle); + + public static native void setAnalogGyroParameters(int handle, + float voltsPerDegreePerSecond, + float offset, int center); + + public static native void setAnalogGyroVoltsPerDegreePerSecond(int handle, + float voltsPerDegreePerSecond); + + public static native void resetAnalogGyro(int handle); + + public static native void calibrateAnalogGyro(int handle); + + public static native void setAnalogGyroDeadband(int handle, float volts); + + public static native float getAnalogGyroAngle(int handle); + + public static native double getAnalogGyroRate(int handle); + + public static native float getAnalogGyroOffset(int handle); + + public static native int getAnalogGyroCenter(int handle); +}