diff --git a/hal/include/HAL/Errors.h b/hal/include/HAL/Errors.h index 46a8e79e51..6b7d09f8d9 100644 --- a/hal/include/HAL/Errors.h +++ b/hal/include/HAL/Errors.h @@ -70,6 +70,9 @@ #define HAL_COUNTER_NOT_SUPPORTED -1058 #define HAL_COUNTER_NOT_SUPPORTED_MESSAGE \ "HAL: Counter mode not supported for encoder method" +#define HAL_PWM_SCALE_ERROR -1072 +#define HAL_PWM_SCALE_ERROR_MESSAGE \ + "HAL: The PWM Scale Factors are out of range" #define HAL_HANDLE_ERROR -1098 #define HAL_HANDLE_ERROR_MESSAGE \ "HAL: A handle parameter was passed incorrectly" diff --git a/hal/include/HAL/PWM.h b/hal/include/HAL/PWM.h index 018222a36f..893fd94140 100644 --- a/hal/include/HAL/PWM.h +++ b/hal/include/HAL/PWM.h @@ -17,9 +17,29 @@ void freePWMPort(HalDigitalHandle pwm_port_handle, int32_t* status); bool checkPWMChannel(uint8_t pin); -void setPWM(HalDigitalHandle pwm_port_handle, unsigned short value, - int32_t* status); -unsigned short getPWM(HalDigitalHandle pwm_port_handle, int32_t* status); +void setPWMConfig(HalDigitalHandle pwm_port_handle, double maxPwm, + double deadbandMaxPwm, double centerPwm, + double deadbandMinPwm, double minPwm, int32_t* status); +void setPWMConfigRaw(HalDigitalHandle pwm_port_handle, int32_t maxPwm, + int32_t deadbandMaxPwm, int32_t centerPwm, + int32_t deadbandMinPwm, int32_t minPwm, int32_t* status); +void getPWMConfigRaw(HalDigitalHandle pwm_port_handle, int32_t* maxPwm, + int32_t* deadbandMaxPwm, int32_t* centerPwm, + int32_t* deadbandMinPwm, int32_t* minPwm, int32_t* status); +void setPWMEliminateDeadband(HalDigitalHandle pwm_port_handle, + uint8_t eliminateDeadband, int32_t* status); +uint8_t getPWMEliminateDeadband(HalDigitalHandle pwm_port_handle, + int32_t* status); +void setPWMRaw(HalDigitalHandle pwm_port_handle, uint16_t value, + int32_t* status); +void setPWMSpeed(HalDigitalHandle pwm_port_handle, float speed, + int32_t* status); +void setPWMPosition(HalDigitalHandle pwm_port_handle, float position, + int32_t* status); +void setPWMDisabled(HalDigitalHandle pwm_port_handle, int32_t* status); +uint16_t getPWMRaw(HalDigitalHandle pwm_port_handle, int32_t* status); +float getPWMSpeed(HalDigitalHandle pwm_port_handle, int32_t* status); +float getPWMPosition(HalDigitalHandle pwm_port_handle, int32_t* status); void latchPWMZero(HalDigitalHandle pwm_port_handle, int32_t* status); void setPWMPeriodScale(HalDigitalHandle pwm_port_handle, uint32_t squelchMask, int32_t* status); diff --git a/hal/lib/athena/DigitalInternal.h b/hal/lib/athena/DigitalInternal.h index cc7e2d29ae..a9fa640bfd 100644 --- a/hal/lib/athena/DigitalInternal.h +++ b/hal/lib/athena/DigitalInternal.h @@ -63,6 +63,13 @@ extern bool digitalSystemsInitialized; struct DigitalPort { uint8_t pin; + bool configSet = false; + bool eliminateDeadband = false; + int32_t maxPwm = 0; + int32_t deadbandMaxPwm = 0; + int32_t centerPwm = 0; + int32_t deadbandMinPwm = 0; + int32_t minPwm = 0; }; extern DigitalHandleResourcemaxPwm; +} +static inline int32_t GetMinPositivePwm(DigitalPort* port) { + return port->eliminateDeadband ? port->deadbandMaxPwm : port->centerPwm + 1; +} +static inline int32_t GetCenterPwm(DigitalPort* port) { + return port->centerPwm; +} +static inline int32_t GetMaxNegativePwm(DigitalPort* port) { + return port->eliminateDeadband ? port->deadbandMinPwm : port->centerPwm - 1; +} +static inline int32_t GetMinNegativePwm(DigitalPort* port) { + return port->minPwm; +} +static inline int32_t GetPositiveScaleFactor(DigitalPort* port) { + return GetMaxPositivePwm(port) - GetMinPositivePwm(port); +} ///< The scale for positive speeds. +static inline int32_t GetNegativeScaleFactor(DigitalPort* port) { + return GetMaxNegativePwm(port) - GetMinNegativePwm(port); +} ///< The scale for negative speeds. +static inline int32_t GetFullRangeScaleFactor(DigitalPort* port) { + return GetMaxPositivePwm(port) - GetMinNegativePwm(port); +} ///< The scale for positions. + extern "C" { HalDigitalHandle initializePWMPort(HalPortHandle port_handle, int32_t* status) { @@ -74,6 +100,91 @@ void freePWMPort(HalDigitalHandle pwm_port_handle, int32_t* status) { bool checkPWMChannel(uint8_t pin) { return pin < kNumPWMPins; } +void setPWMConfig(HalDigitalHandle pwm_port_handle, double max, + double deadbandMax, double center, double deadbandMin, + double min, int32_t* status) { + auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + // calculate the loop time in milliseconds + double loopTime = + getLoopTiming(status) / (kSystemClockTicksPerMicrosecond * 1e3); + if (*status != 0) return; + + int32_t maxPwm = (int32_t)((max - kDefaultPwmCenter) / loopTime + + kDefaultPwmStepsDown - 1); + int32_t deadbandMaxPwm = (int32_t)( + (deadbandMax - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1); + int32_t centerPwm = (int32_t)((center - kDefaultPwmCenter) / loopTime + + kDefaultPwmStepsDown - 1); + int32_t deadbandMinPwm = (int32_t)( + (deadbandMin - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1); + int32_t minPwm = (int32_t)((min - kDefaultPwmCenter) / loopTime + + kDefaultPwmStepsDown - 1); + + port->maxPwm = maxPwm; + port->deadbandMaxPwm = deadbandMaxPwm; + port->deadbandMinPwm = deadbandMinPwm; + port->centerPwm = centerPwm; + port->minPwm = minPwm; + port->configSet = true; +} + +void setPWMConfigRaw(HalDigitalHandle pwm_port_handle, int32_t maxPwm, + int32_t deadbandMaxPwm, int32_t centerPwm, + int32_t deadbandMinPwm, int32_t minPwm, int32_t* status) { + auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + port->maxPwm = maxPwm; + port->deadbandMaxPwm = deadbandMaxPwm; + port->deadbandMinPwm = deadbandMinPwm; + port->centerPwm = centerPwm; + port->minPwm = minPwm; +} + +void getPWMConfigRaw(HalDigitalHandle pwm_port_handle, int32_t* maxPwm, + int32_t* deadbandMaxPwm, int32_t* centerPwm, + int32_t* deadbandMinPwm, int32_t* minPwm, + int32_t* status) { + auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + *maxPwm = port->maxPwm; + *deadbandMaxPwm = port->deadbandMaxPwm; + *deadbandMinPwm = port->deadbandMinPwm; + *centerPwm = port->centerPwm; + *minPwm = port->minPwm; +} + +void setPWMEliminateDeadband(HalDigitalHandle pwm_port_handle, + uint8_t eliminateDeadband, int32_t* status) { + auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + port->eliminateDeadband = eliminateDeadband; +} + +uint8_t getPWMEliminateDeadband(HalDigitalHandle pwm_port_handle, + int32_t* status) { + auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + return port->eliminateDeadband; +} + /** * Set a PWM channel to the desired value. The values range from 0 to 255 and * the period is controlled @@ -82,8 +193,8 @@ bool checkPWMChannel(uint8_t pin) { return pin < kNumPWMPins; } * @param channel The PWM channel to set. * @param value The PWM value to set. */ -void setPWM(HalDigitalHandle pwm_port_handle, unsigned short value, - int32_t* status) { +void setPWMRaw(HalDigitalHandle pwm_port_handle, uint16_t value, + int32_t* status) { auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); if (port == nullptr) { *status = HAL_HANDLE_ERROR; @@ -97,13 +208,109 @@ void setPWM(HalDigitalHandle pwm_port_handle, unsigned short value, } } +/** + * Set a PWM channel to the desired scaled value. The values range from -1 to 1 + * and + * the period is controlled + * by the PWM Period and MinHigh registers. + * + * @param channel The PWM channel to set. + * @param value The scaled PWM value to set. + */ +void setPWMSpeed(HalDigitalHandle pwm_port_handle, float speed, + int32_t* status) { + auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + if (!port->configSet) { + *status = INCOMPATIBLE_STATE; + return; + } + + DigitalPort* dPort = port.get(); + + if (speed < -1.0) { + speed = -1.0; + } else if (speed > 1.0) { + speed = 1.0; + } + + // calculate the desired output pwm value by scaling the speed appropriately + int32_t rawValue; + if (speed == 0.0) { + rawValue = GetCenterPwm(dPort); + } else if (speed > 0.0) { + rawValue = (int32_t)(speed * ((float)GetPositiveScaleFactor(dPort)) + + ((float)GetMinPositivePwm(dPort)) + 0.5); + } else { + rawValue = (int32_t)(speed * ((float)GetNegativeScaleFactor(dPort)) + + ((float)GetMaxNegativePwm(dPort)) + 0.5); + } + + if (!((rawValue >= GetMinNegativePwm(dPort)) && + (rawValue <= GetMaxPositivePwm(dPort))) || + rawValue == kPwmDisabled) { + *status = HAL_PWM_SCALE_ERROR; + return; + } + + setPWMRaw(pwm_port_handle, rawValue, status); +} + +/** + * Set a PWM channel to the desired position value. The values range from 0 to 1 + * and + * the period is controlled + * by the PWM Period and MinHigh registers. + * + * @param channel The PWM channel to set. + * @param value The scaled PWM value to set. + */ +void setPWMPosition(HalDigitalHandle pwm_port_handle, float pos, + int32_t* status) { + auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + if (!port->configSet) { + *status = INCOMPATIBLE_STATE; + return; + } + DigitalPort* dPort = port.get(); + + if (pos < 0.0) { + pos = 0.0; + } else if (pos > 1.0) { + pos = 1.0; + } + + // note, need to perform the multiplication below as floating point before + // converting to int + uint16_t rawValue = (int32_t)((pos * (float)GetFullRangeScaleFactor(dPort)) + + GetMinNegativePwm(dPort)); + + if (rawValue == kPwmDisabled) { + *status = HAL_PWM_SCALE_ERROR; + return; + } + + setPWMRaw(pwm_port_handle, rawValue, status); +} + +void setPWMDisabled(HalDigitalHandle pwm_port_handle, int32_t* status) { + setPWMRaw(pwm_port_handle, kPwmDisabled, status); +} + /** * Get a value from a PWM channel. The values range from 0 to 255. * * @param channel The PWM channel to read from. * @return The raw PWM value. */ -unsigned short getPWM(HalDigitalHandle pwm_port_handle, int32_t* status) { +uint16_t getPWMRaw(HalDigitalHandle pwm_port_handle, int32_t* status) { auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); if (port == nullptr) { *status = HAL_HANDLE_ERROR; @@ -117,6 +324,75 @@ unsigned short getPWM(HalDigitalHandle pwm_port_handle, int32_t* status) { } } +/** + * Get a scaled value from a PWM channel. The values range from -1 to 1. + * + * @param channel The PWM channel to read from. + * @return The scaled PWM value. + */ +float getPWMSpeed(HalDigitalHandle pwm_port_handle, int32_t* status) { + auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + if (!port->configSet) { + *status = INCOMPATIBLE_STATE; + return 0; + } + + int32_t value = getPWMRaw(pwm_port_handle, status); + if (*status != 0) return 0; + DigitalPort* dPort = port.get(); + + if (value == kPwmDisabled) { + return 0.0; + } else if (value > GetMaxPositivePwm(dPort)) { + return 1.0; + } else if (value < GetMinNegativePwm(dPort)) { + return -1.0; + } else if (value > GetMinPositivePwm(dPort)) { + return (float)(value - GetMinPositivePwm(dPort)) / + (float)GetPositiveScaleFactor(dPort); + } else if (value < GetMaxNegativePwm(dPort)) { + return (float)(value - GetMaxNegativePwm(dPort)) / + (float)GetNegativeScaleFactor(dPort); + } else { + return 0.0; + } +} + +/** + * Get a position value from a PWM channel. The values range from 0 to 1. + * + * @param channel The PWM channel to read from. + * @return The scaled PWM value. + */ +float getPWMPosition(HalDigitalHandle pwm_port_handle, int32_t* status) { + auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + if (!port->configSet) { + *status = INCOMPATIBLE_STATE; + return 0; + } + + int32_t value = getPWMRaw(pwm_port_handle, status); + if (*status != 0) return 0; + DigitalPort* dPort = port.get(); + + if (value < GetMinNegativePwm(dPort)) { + return 0.0; + } else if (value > GetMaxPositivePwm(dPort)) { + return 1.0; + } else { + return (float)(value - GetMinNegativePwm(dPort)) / + (float)GetFullRangeScaleFactor(dPort); + } +} + void latchPWMZero(HalDigitalHandle pwm_port_handle, int32_t* status) { auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM); if (port == nullptr) { diff --git a/wpilibc/athena/include/PWM.h b/wpilibc/athena/include/PWM.h index 61a87f2560..9da3a05bc9 100644 --- a/wpilibc/athena/include/PWM.h +++ b/wpilibc/athena/include/PWM.h @@ -45,57 +45,23 @@ class PWM : public SensorBase, virtual ~PWM(); virtual void SetRaw(unsigned short value); virtual unsigned short GetRaw() const; - void SetPeriodMultiplier(PeriodMultiplier mult); - void SetZeroLatch(); - void EnableDeadbandElimination(bool eliminateDeadband); - void SetBounds(int32_t max, int32_t deadbandMax, int32_t center, - int32_t deadbandMin, int32_t min); - void SetBounds(double max, double deadbandMax, double center, - double deadbandMin, double min); - uint32_t GetChannel() const { return m_channel; } - - protected: - /** - * kDefaultPwmPeriod is in ms - * - * - 20ms periods (50 Hz) are the "safest" setting in that this works for all - * devices - * - 20ms periods seem to be desirable for Vex Motors - * - 20ms periods are the specified period for HS-322HD servos, but work - * reliably down to 10.0 ms; starting at about 8.5ms, the servo sometimes - * hums and get hot; by 5.0ms the hum is nearly continuous - * - 10ms periods work well for Victor 884 - * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed - * controllers. Due to the shipping firmware on the Jaguar, we can't run the - * update period less than 5.05 ms. - * - * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period - * scaling is implemented as an output squelch to get longer periods for old - * devices. - */ - static constexpr float kDefaultPwmPeriod = 5.05; - /** - * kDefaultPwmCenter is the PWM range center in ms - */ - static constexpr float kDefaultPwmCenter = 1.5; - /** - * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint - */ - static const int32_t kDefaultPwmStepsDown = 1000; - static const int32_t kPwmDisabled = 0; - virtual void SetPosition(float pos); virtual float GetPosition() const; virtual void SetSpeed(float speed); virtual float GetSpeed() const; + virtual void SetDisabled(); + void SetPeriodMultiplier(PeriodMultiplier mult); + void SetZeroLatch(); + void EnableDeadbandElimination(bool eliminateDeadband); + void SetBounds(double max, double deadbandMax, double center, + double deadbandMin, double min); + void SetRawBounds(int32_t max, int32_t deadbandMax, int32_t center, + int32_t deadbandMin, int32_t min); + void GetRawBounds(int32_t* max, int32_t* deadbandMax, int32_t* center, + int32_t* deadbandMin, int32_t* min); + uint32_t GetChannel() const { return m_channel; } - bool m_eliminateDeadband; - int32_t m_maxPwm; - int32_t m_deadbandMaxPwm; - int32_t m_centerPwm; - int32_t m_deadbandMinPwm; - int32_t m_minPwm; - + protected: void ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) override; void UpdateTable() override; @@ -110,22 +76,4 @@ class PWM : public SensorBase, private: uint32_t m_channel; HalDigitalHandle m_handle; - int32_t GetMaxPositivePwm() const { return m_maxPwm; } - int32_t GetMinPositivePwm() const { - return m_eliminateDeadband ? m_deadbandMaxPwm : m_centerPwm + 1; - } - int32_t GetCenterPwm() const { return m_centerPwm; } - int32_t GetMaxNegativePwm() const { - return m_eliminateDeadband ? m_deadbandMinPwm : m_centerPwm - 1; - } - int32_t GetMinNegativePwm() const { return m_minPwm; } - int32_t GetPositiveScaleFactor() const { - return GetMaxPositivePwm() - GetMinPositivePwm(); - } ///< The scale for positive speeds. - int32_t GetNegativeScaleFactor() const { - return GetMaxNegativePwm() - GetMinNegativePwm(); - } ///< The scale for negative speeds. - int32_t GetFullRangeScaleFactor() const { - return GetMaxPositivePwm() - GetMinNegativePwm(); - } ///< The scale for positions. }; diff --git a/wpilibc/athena/src/Jaguar.cpp b/wpilibc/athena/src/Jaguar.cpp index 3f614cf5c5..1452395836 100644 --- a/wpilibc/athena/src/Jaguar.cpp +++ b/wpilibc/athena/src/Jaguar.cpp @@ -27,7 +27,7 @@ Jaguar::Jaguar(uint32_t channel) : PWMSpeedController(channel) { */ SetBounds(2.31, 1.55, 1.507, 1.454, .697); SetPeriodMultiplier(kPeriodMultiplier_1X); - SetRaw(m_centerPwm); + SetSpeed(0.0); SetZeroLatch(); HALReport(HALUsageReporting::kResourceType_Jaguar, GetChannel()); diff --git a/wpilibc/athena/src/PWM.cpp b/wpilibc/athena/src/PWM.cpp index 4628401e71..bf782ae791 100644 --- a/wpilibc/athena/src/PWM.cpp +++ b/wpilibc/athena/src/PWM.cpp @@ -13,11 +13,6 @@ #include -constexpr float PWM::kDefaultPwmPeriod; -constexpr float PWM::kDefaultPwmCenter; -const int32_t PWM::kDefaultPwmStepsDown; -const int32_t PWM::kPwmDisabled; - /** * Allocate a PWM given a channel number. * @@ -43,14 +38,16 @@ PWM::PWM(uint32_t channel) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); m_channel = std::numeric_limits::max(); m_handle = HAL_INVALID_HANDLE; + return; } m_channel = channel; - setPWM(m_handle, kPwmDisabled, &status); + setPWMDisabled(m_handle, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + status = 0; + setPWMEliminateDeadband(m_handle, false, &status); wpi_setErrorWithContext(status, getHALErrorMessage(status)); - - m_eliminateDeadband = false; HALReport(HALUsageReporting::kResourceType_PWM, channel); } @@ -63,7 +60,7 @@ PWM::PWM(uint32_t channel) { PWM::~PWM() { int32_t status = 0; - setPWM(m_handle, kPwmDisabled, &status); + setPWMDisabled(m_handle, &status); wpi_setErrorWithContext(status, getHALErrorMessage(status)); freePWMPort(m_handle, &status); @@ -82,30 +79,9 @@ PWM::~PWM() { */ void PWM::EnableDeadbandElimination(bool eliminateDeadband) { if (StatusIsFatal()) return; - m_eliminateDeadband = eliminateDeadband; -} - -/** - * Set the bounds on the PWM values. - * - * This sets the bounds on the PWM values for a particular each type of - * controller. The values determine the upper and lower speeds as well as the - * deadband bracket. - * - * @param max The Minimum pwm value - * @param deadbandMax The high end of the deadband range - * @param center The center speed (off) - * @param deadbandMin The low end of the deadband range - * @param min The minimum pwm value - */ -void PWM::SetBounds(int32_t max, int32_t deadbandMax, int32_t center, - int32_t deadbandMin, int32_t min) { - if (StatusIsFatal()) return; - m_maxPwm = max; - m_deadbandMaxPwm = deadbandMax; - m_centerPwm = center; - m_deadbandMinPwm = deadbandMin; - m_minPwm = min; + int32_t status = 0; + setPWMEliminateDeadband(m_handle, eliminateDeadband, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); } /** @@ -123,24 +99,53 @@ void PWM::SetBounds(int32_t max, int32_t deadbandMax, int32_t center, */ void PWM::SetBounds(double max, double deadbandMax, double center, double deadbandMin, double min) { - // calculate the loop time in milliseconds - int32_t status = 0; - double loopTime = - getLoopTiming(&status) / (HAL_getSystemClockTicksPerMicrosecond() * 1e3); - wpi_setErrorWithContext(status, getHALErrorMessage(status)); - if (StatusIsFatal()) return; + int32_t status = 0; + setPWMConfig(m_handle, max, deadbandMax, center, deadbandMin, min, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); +} - m_maxPwm = (int32_t)((max - kDefaultPwmCenter) / loopTime + - kDefaultPwmStepsDown - 1); - m_deadbandMaxPwm = (int32_t)((deadbandMax - kDefaultPwmCenter) / loopTime + - kDefaultPwmStepsDown - 1); - m_centerPwm = (int32_t)((center - kDefaultPwmCenter) / loopTime + - kDefaultPwmStepsDown - 1); - m_deadbandMinPwm = (int32_t)((deadbandMin - kDefaultPwmCenter) / loopTime + - kDefaultPwmStepsDown - 1); - m_minPwm = (int32_t)((min - kDefaultPwmCenter) / loopTime + - kDefaultPwmStepsDown - 1); +/** + * Set the bounds on the PWM values. + * + * This sets the bounds on the PWM values for a particular each type of + * controller. The values determine the upper and lower speeds as well as the + * deadband bracket. + * + * @param max The Minimum pwm value + * @param deadbandMax The high end of the deadband range + * @param center The center speed (off) + * @param deadbandMin The low end of the deadband range + * @param min The minimum pwm value + */ +void PWM::SetRawBounds(int32_t max, int32_t deadbandMax, int32_t center, + int32_t deadbandMin, int32_t min) { + if (StatusIsFatal()) return; + int32_t status = 0; + setPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min, + &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); +} + +/** + * Get the bounds on the PWM values. + * + * This Gets the bounds on the PWM values for a particular each type of + * controller. The values determine the upper and lower speeds as well as the + * deadband bracket. + * + * @param max The Minimum pwm value + * @param deadbandMax The high end of the deadband range + * @param center The center speed (off) + * @param deadbandMin The low end of the deadband range + * @param min The minimum pwm value + */ +void PWM::GetRawBounds(int32_t* max, int32_t* deadbandMax, int32_t* center, + int32_t* deadbandMin, int32_t* min) { + int32_t status = 0; + getPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min, + &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); } /** @@ -155,21 +160,9 @@ void PWM::SetBounds(double max, double deadbandMax, double center, */ void PWM::SetPosition(float pos) { if (StatusIsFatal()) return; - if (pos < 0.0) { - pos = 0.0; - } else if (pos > 1.0) { - pos = 1.0; - } - - // note, need to perform the multiplication below as floating point before - // converting to int - unsigned short rawValue = - (int32_t)((pos * (float)GetFullRangeScaleFactor()) + GetMinNegativePwm()); - - wpi_assert(rawValue != kPwmDisabled); - - // send the computed pwm value to the FPGA - SetRaw((unsigned short)rawValue); + int32_t status = 0; + setPWMPosition(m_handle, pos, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); } /** @@ -184,15 +177,10 @@ void PWM::SetPosition(float pos) { */ float PWM::GetPosition() const { if (StatusIsFatal()) return 0.0; - int32_t value = GetRaw(); - if (value < GetMinNegativePwm()) { - return 0.0; - } else if (value > GetMaxPositivePwm()) { - return 1.0; - } else { - return (float)(value - GetMinNegativePwm()) / - (float)GetFullRangeScaleFactor(); - } + int32_t status = 0; + float position = getPWMPosition(m_handle, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + return position; } /** @@ -210,32 +198,9 @@ float PWM::GetPosition() const { */ void PWM::SetSpeed(float speed) { if (StatusIsFatal()) return; - // clamp speed to be in the range 1.0 >= speed >= -1.0 - if (speed < -1.0) { - speed = -1.0; - } else if (speed > 1.0) { - speed = 1.0; - } - - // calculate the desired output pwm value by scaling the speed appropriately - int32_t rawValue; - if (speed == 0.0) { - rawValue = GetCenterPwm(); - } else if (speed > 0.0) { - rawValue = (int32_t)(speed * ((float)GetPositiveScaleFactor()) + - ((float)GetMinPositivePwm()) + 0.5); - } else { - rawValue = (int32_t)(speed * ((float)GetNegativeScaleFactor()) + - ((float)GetMaxNegativePwm()) + 0.5); - } - - // the above should result in a pwm_value in the valid range - wpi_assert((rawValue >= GetMinNegativePwm()) && - (rawValue <= GetMaxPositivePwm())); - wpi_assert(rawValue != kPwmDisabled); - - // send the computed pwm value to the FPGA - SetRaw(rawValue); + int32_t status = 0; + setPWMSpeed(m_handle, speed, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); } /** @@ -252,22 +217,10 @@ void PWM::SetSpeed(float speed) { */ float PWM::GetSpeed() const { if (StatusIsFatal()) return 0.0; - int32_t value = GetRaw(); - if (value == PWM::kPwmDisabled) { - return 0.0; - } else if (value > GetMaxPositivePwm()) { - return 1.0; - } else if (value < GetMinNegativePwm()) { - return -1.0; - } else if (value > GetMinPositivePwm()) { - return (float)(value - GetMinPositivePwm()) / - (float)GetPositiveScaleFactor(); - } else if (value < GetMaxNegativePwm()) { - return (float)(value - GetMaxNegativePwm()) / - (float)GetNegativeScaleFactor(); - } else { - return 0.0; - } + int32_t status = 0; + float speed = getPWMSpeed(m_handle, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + return speed; } /** @@ -281,7 +234,7 @@ void PWM::SetRaw(unsigned short value) { if (StatusIsFatal()) return; int32_t status = 0; - setPWM(m_handle, value, &status); + setPWMRaw(m_handle, value, &status); wpi_setErrorWithContext(status, getHALErrorMessage(status)); } @@ -296,7 +249,7 @@ unsigned short PWM::GetRaw() const { if (StatusIsFatal()) return 0; int32_t status = 0; - unsigned short value = getPWM(m_handle, &status); + unsigned short value = getPWMRaw(m_handle, &status); wpi_setErrorWithContext(status, getHALErrorMessage(status)); return value; @@ -329,6 +282,19 @@ void PWM::SetPeriodMultiplier(PeriodMultiplier mult) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } +/** + * Temporarily disables the PWM output. The next set call will reenable + * the output. + */ +void PWM::SetDisabled() { + if (StatusIsFatal()) return; + + int32_t status = 0; + + setPWMDisabled(m_handle, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); +} + void PWM::SetZeroLatch() { if (StatusIsFatal()) return; diff --git a/wpilibc/athena/src/PWMSpeedController.cpp b/wpilibc/athena/src/PWMSpeedController.cpp index e8f5211dcf..87d1c91ccf 100644 --- a/wpilibc/athena/src/PWMSpeedController.cpp +++ b/wpilibc/athena/src/PWMSpeedController.cpp @@ -37,7 +37,7 @@ float PWMSpeedController::Get() const { return GetSpeed(); } /** * Common interface for disabling a motor. */ -void PWMSpeedController::Disable() { SetRaw(kPwmDisabled); } +void PWMSpeedController::Disable() { SetDisabled(); } /** * Common interface for inverting direction of a speed controller. diff --git a/wpilibc/athena/src/SD540.cpp b/wpilibc/athena/src/SD540.cpp index 6ee31b4736..8b8cb79bdd 100644 --- a/wpilibc/athena/src/SD540.cpp +++ b/wpilibc/athena/src/SD540.cpp @@ -34,7 +34,7 @@ SD540::SD540(uint32_t channel) : PWMSpeedController(channel) { SetBounds(2.05, 1.55, 1.50, 1.44, .94); SetPeriodMultiplier(kPeriodMultiplier_1X); - SetRaw(m_centerPwm); + SetSpeed(0.0); SetZeroLatch(); HALReport(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel()); diff --git a/wpilibc/athena/src/SafePWM.cpp b/wpilibc/athena/src/SafePWM.cpp index e54e3ae36d..0ba2c85718 100644 --- a/wpilibc/athena/src/SafePWM.cpp +++ b/wpilibc/athena/src/SafePWM.cpp @@ -48,7 +48,7 @@ bool SafePWM::IsAlive() const { return m_safetyHelper->IsAlive(); } * This is called by the MotorSafetyHelper object when it has a timeout for this * PWM and needs to stop it from running. */ -void SafePWM::StopMotor() { SetRaw(kPwmDisabled); } +void SafePWM::StopMotor() { SetDisabled(); } /** * Enable/disable motor safety for this device. diff --git a/wpilibc/athena/src/Spark.cpp b/wpilibc/athena/src/Spark.cpp index 68b1628118..76f7488bad 100644 --- a/wpilibc/athena/src/Spark.cpp +++ b/wpilibc/athena/src/Spark.cpp @@ -34,7 +34,7 @@ Spark::Spark(uint32_t channel) : PWMSpeedController(channel) { SetBounds(2.003, 1.55, 1.50, 1.46, .999); SetPeriodMultiplier(kPeriodMultiplier_1X); - SetRaw(m_centerPwm); + SetSpeed(0.0); SetZeroLatch(); HALReport(HALUsageReporting::kResourceType_RevSPARK, GetChannel()); diff --git a/wpilibc/athena/src/Talon.cpp b/wpilibc/athena/src/Talon.cpp index 3ab5e00768..b223bd3d0f 100644 --- a/wpilibc/athena/src/Talon.cpp +++ b/wpilibc/athena/src/Talon.cpp @@ -32,7 +32,7 @@ Talon::Talon(uint32_t channel) : PWMSpeedController(channel) { */ SetBounds(2.037, 1.539, 1.513, 1.487, .989); SetPeriodMultiplier(kPeriodMultiplier_1X); - SetRaw(m_centerPwm); + SetSpeed(0.0); SetZeroLatch(); HALReport(HALUsageReporting::kResourceType_Talon, GetChannel()); diff --git a/wpilibc/athena/src/TalonSRX.cpp b/wpilibc/athena/src/TalonSRX.cpp index b8432617bb..db268f9870 100644 --- a/wpilibc/athena/src/TalonSRX.cpp +++ b/wpilibc/athena/src/TalonSRX.cpp @@ -31,7 +31,7 @@ TalonSRX::TalonSRX(uint32_t channel) : PWMSpeedController(channel) { */ SetBounds(2.004, 1.52, 1.50, 1.48, .997); SetPeriodMultiplier(kPeriodMultiplier_1X); - SetRaw(m_centerPwm); + SetSpeed(0.0); SetZeroLatch(); HALReport(HALUsageReporting::kResourceType_TalonSRX, GetChannel()); diff --git a/wpilibc/athena/src/Victor.cpp b/wpilibc/athena/src/Victor.cpp index 775889041b..45f09bec40 100644 --- a/wpilibc/athena/src/Victor.cpp +++ b/wpilibc/athena/src/Victor.cpp @@ -33,7 +33,7 @@ Victor::Victor(uint32_t channel) : PWMSpeedController(channel) { */ SetBounds(2.027, 1.525, 1.507, 1.49, 1.026); SetPeriodMultiplier(kPeriodMultiplier_2X); - SetRaw(m_centerPwm); + SetSpeed(0.0); SetZeroLatch(); LiveWindow::GetInstance()->AddActuator("Victor", GetChannel(), this); diff --git a/wpilibc/athena/src/VictorSP.cpp b/wpilibc/athena/src/VictorSP.cpp index 8e7b6b7926..78c15310dc 100644 --- a/wpilibc/athena/src/VictorSP.cpp +++ b/wpilibc/athena/src/VictorSP.cpp @@ -33,7 +33,7 @@ VictorSP::VictorSP(uint32_t channel) : PWMSpeedController(channel) { */ SetBounds(2.004, 1.52, 1.50, 1.48, .997); SetPeriodMultiplier(kPeriodMultiplier_1X); - SetRaw(m_centerPwm); + SetSpeed(0.0); SetZeroLatch(); HALReport(HALUsageReporting::kResourceType_VictorSP, GetChannel()); diff --git a/wpilibj/src/athena/cpp/lib/HALUtil.cpp b/wpilibj/src/athena/cpp/lib/HALUtil.cpp index 1c8b89bfbd..9ec1ea0849 100644 --- a/wpilibj/src/athena/cpp/lib/HALUtil.cpp +++ b/wpilibj/src/athena/cpp/lib/HALUtil.cpp @@ -48,6 +48,7 @@ static jclass canMessageNotFoundExCls = nullptr; static jclass canMessageNotAllowedExCls = nullptr; static jclass canNotInitializedExCls = nullptr; static jclass uncleanStatusExCls = nullptr; +static jclass pwmConfigDataResultCls = nullptr; static void GetStackTrace(JNIEnv *env, std::string &res, std::string &func) { // create a throwable @@ -236,6 +237,17 @@ void ThrowBoundaryException(JNIEnv *env, double value, double lower, env->Throw(static_cast(ex)); } +jobject CreatePWMConfigDataResult(JNIEnv *env, int32_t maxPwm, + int32_t deadbandMaxPwm, int32_t centerPwm, + int32_t deadbandMinPwm, int32_t minPwm) { + static jmethodID constructor = + env->GetMethodID(pwmConfigDataResultCls, "", + "(IIIII)V"); + return env->NewObject(pwmConfigDataResultCls, constructor, maxPwm, + deadbandMaxPwm, centerPwm, deadbandMinPwm, + minPwm); +} + extern "C" { // @@ -328,6 +340,12 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) { uncleanStatusExCls = static_cast(env->NewGlobalRef(local)); if (!uncleanStatusExCls) return JNI_ERR; env->DeleteLocalRef(local); + + local = env->FindClass("edu/wpi/first/wpilibj/PWMConfigDataResult"); + if (!local) return JNI_ERR; + pwmConfigDataResultCls = static_cast(env->NewGlobalRef(local)); + if (!pwmConfigDataResultCls) return JNI_ERR; + env->DeleteLocalRef(local); return JNI_VERSION_1_6; } @@ -342,12 +360,15 @@ JNIEXPORT void JNICALL JNI_OnUnload(JavaVM *vm, void *reserved) { if (runtimeExCls) env->DeleteGlobalRef(runtimeExCls); if (illegalArgExCls) env->DeleteGlobalRef(illegalArgExCls); if (boundaryExCls) env->DeleteGlobalRef(boundaryExCls); + if (allocationExCls) env->DeleteGlobalRef(allocationExCls); + if (halHandleExCls) env->DeleteGlobalRef(halHandleExCls); if (canInvalidBufferExCls) env->DeleteGlobalRef(canInvalidBufferExCls); if (canMessageNotFoundExCls) env->DeleteGlobalRef(canMessageNotFoundExCls); if (canMessageNotAllowedExCls) env->DeleteGlobalRef(canMessageNotAllowedExCls); if (canNotInitializedExCls) env->DeleteGlobalRef(canNotInitializedExCls); if (uncleanStatusExCls) env->DeleteGlobalRef(uncleanStatusExCls); + if (pwmConfigDataResultCls) env->DeleteGlobalRef(pwmConfigDataResultCls); jvm = nullptr; } diff --git a/wpilibj/src/athena/cpp/lib/HALUtil.h b/wpilibj/src/athena/cpp/lib/HALUtil.h index 7bfaeb6172..dfe83ab90d 100644 --- a/wpilibj/src/athena/cpp/lib/HALUtil.h +++ b/wpilibj/src/athena/cpp/lib/HALUtil.h @@ -31,5 +31,9 @@ inline bool CheckCANStatus(JNIEnv *env, int32_t status, int message_id) { void ThrowIllegalArgumentException(JNIEnv *env, const char *msg); void ThrowBoundaryException(JNIEnv *env, double value, double lower, double upper); + +jobject CreatePWMConfigDataResult(JNIEnv *env, int32_t maxPwm, + int32_t deadbandMaxPwm, int32_t centerPwm, + int32_t deadbandMinPwm, int32_t minPwm); #endif // HALUTIL_H diff --git a/wpilibj/src/athena/cpp/lib/PWMJNI.cpp b/wpilibj/src/athena/cpp/lib/PWMJNI.cpp index 019ab27f16..b6f5c11cc6 100644 --- a/wpilibj/src/athena/cpp/lib/PWMJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/PWMJNI.cpp @@ -59,36 +59,198 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_freePWMPort( } /* - * Class: edu_wpi_first_wpilibj_hal_PWMJNI - * Method: setPWM - * Signature: (IS)V - */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWM( - JNIEnv* env, jclass, jint id, jshort value) { - PWMJNI_LOG(logDEBUG) << "PWM Handle = " << (HalDigitalHandle)id; - PWMJNI_LOG(logDEBUG) << "PWM Value = " << value; +* Class: edu_wpi_first_wpilibj_hal_DIOJNI +* Method: setPWMConfigRaw +* Signature: (IIIIII)V; +*/ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMConfigRaw( + JNIEnv *env, jclass, jint id, jint maxPwm, jint deadbandMaxPwm, + jint centerPwm, jint deadbandMinPwm, jint minPwm) { + PWMJNI_LOG(logDEBUG) << "Calling PWMJNI setPWMConfigRaw"; + PWMJNI_LOG(logDEBUG) << "Port Handle = " << (HalDigitalHandle)id; int32_t status = 0; - setPWM((HalDigitalHandle)id, value, &status); + setPWMConfigRaw((HalDigitalHandle)id, maxPwm, deadbandMaxPwm, centerPwm, + deadbandMinPwm, minPwm, &status); + CheckStatus(env, status); +} + +/* +* Class: edu_wpi_first_wpilibj_hal_DIOJNI +* Method: setPWMConfig +* Signature: (IDDDDD)V; +*/ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMConfig( + JNIEnv *env, jclass, jint id, jdouble maxPwm, jdouble deadbandMaxPwm, + jdouble centerPwm, jdouble deadbandMinPwm, jdouble minPwm) { + PWMJNI_LOG(logDEBUG) << "Calling PWMJNI setPWMConfig"; + PWMJNI_LOG(logDEBUG) << "Port Handle = " << (HalDigitalHandle)id; + int32_t status = 0; + setPWMConfig((HalDigitalHandle)id, maxPwm, deadbandMaxPwm, centerPwm, + deadbandMinPwm, minPwm, &status); + CheckStatus(env, status); +} + +/* +* Class: edu_wpi_first_wpilibj_hal_DIOJNI +* Method: getPWMConfigRaw +* Signature: (I)Ledu/wpi/first/wpilibj/PWMConfigDataResult; +*/ +JNIEXPORT jobject JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_getPWMConfigRaw( + JNIEnv *env, jclass, jint id) { + PWMJNI_LOG(logDEBUG) << "Calling PWMJNI getPWMConfigRaw"; + PWMJNI_LOG(logDEBUG) << "Port Handle = " << (HalDigitalHandle)id; + int32_t status = 0; + int32_t maxPwm = 0; + int32_t deadbandMaxPwm = 0; + int32_t centerPwm = 0; + int32_t deadbandMinPwm = 0; + int32_t minPwm = 0; + getPWMConfigRaw((HalDigitalHandle)id, &maxPwm, &deadbandMaxPwm, ¢erPwm, + &deadbandMinPwm, &minPwm, &status); + CheckStatus(env, status); + return CreatePWMConfigDataResult(env, maxPwm, deadbandMaxPwm, centerPwm, + deadbandMinPwm, minPwm); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_PWMJNI + * Method: setPWMEliminateDeadband + * Signature: (IZ)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMEliminateDeadband( + JNIEnv* env, jclass, jint id, jboolean value) { + PWMJNI_LOG(logDEBUG) << "PWM Handle = " << (HalDigitalHandle)id; + int32_t status = 0; + setPWMEliminateDeadband((HalDigitalHandle)id, value, &status); PWMJNI_LOG(logDEBUG) << "Status = " << status; CheckStatus(env, status); } /* * Class: edu_wpi_first_wpilibj_hal_PWMJNI - * Method: getPWM + * Method: getPWMEliminateDeadband + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWM( + JNIEnv* env, jclass, jint id) { + PWMJNI_LOG(logDEBUG) << "PWM Handle = " << (HalDigitalHandle)id; + int32_t status = 0; + auto val = getPWMEliminateDeadband((HalDigitalHandle)id, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); + return (jboolean)val; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_PWMJNI + * Method: setPWMRaw + * Signature: (IS)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMRaw( + JNIEnv* env, jclass, jint id, jshort value) { + PWMJNI_LOG(logDEBUG) << "PWM Handle = " << (HalDigitalHandle)id; + PWMJNI_LOG(logDEBUG) << "PWM Value = " << value; + int32_t status = 0; + setPWMRaw((HalDigitalHandle)id, value, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_PWMJNI + * Method: setPWMSpeed + * Signature: (IF)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMSpeed( + JNIEnv* env, jclass, jint id, jfloat value) { + PWMJNI_LOG(logDEBUG) << "PWM Handle = " << (HalDigitalHandle)id; + PWMJNI_LOG(logDEBUG) << "PWM Value = " << value; + int32_t status = 0; + setPWMSpeed((HalDigitalHandle)id, value, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_PWMJNI + * Method: setPWMPosition + * Signature: (IF)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMPosition( + JNIEnv* env, jclass, jint id, jfloat value) { + PWMJNI_LOG(logDEBUG) << "PWM Handle = " << (HalDigitalHandle)id; + PWMJNI_LOG(logDEBUG) << "PWM Value = " << value; + int32_t status = 0; + setPWMPosition((HalDigitalHandle)id, value, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_PWMJNI + * Method: getPWMRaw * Signature: (I)S */ JNIEXPORT jshort JNICALL -Java_edu_wpi_first_wpilibj_hal_PWMJNI_getPWM(JNIEnv* env, jclass, jint id) { +Java_edu_wpi_first_wpilibj_hal_PWMJNI_getPWMRaw( + JNIEnv* env, jclass, jint id) { PWMJNI_LOG(logDEBUG) << "PWM Handle = " << (HalDigitalHandle)id; int32_t status = 0; - jshort returnValue = getPWM((HalDigitalHandle)id, &status); + jshort returnValue = getPWMRaw((HalDigitalHandle)id, &status); PWMJNI_LOG(logDEBUG) << "Status = " << status; PWMJNI_LOG(logDEBUG) << "Value = " << returnValue; CheckStatus(env, status); return returnValue; } +/* + * Class: edu_wpi_first_wpilibj_hal_PWMJNI + * Method: getPWMSpeed + * Signature: (I)F + */ +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_PWMJNI_getPWMSpeed( + JNIEnv* env, jclass, jint id) { + PWMJNI_LOG(logDEBUG) << "PWM Handle = " << (HalDigitalHandle)id; + int32_t status = 0; + jfloat returnValue = getPWMSpeed((HalDigitalHandle)id, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + PWMJNI_LOG(logDEBUG) << "Value = " << returnValue; + CheckStatus(env, status); + return returnValue; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_PWMJNI + * Method: getPWMPosition + * Signature: (I)F + */ +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_PWMJNI_getPWMPosition( + JNIEnv* env, jclass, jint id) { + PWMJNI_LOG(logDEBUG) << "PWM Handle = " << (HalDigitalHandle)id; + int32_t status = 0; + jfloat returnValue = getPWMPosition((HalDigitalHandle)id, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + PWMJNI_LOG(logDEBUG) << "Value = " << returnValue; + CheckStatus(env, status); + return returnValue; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_PWMJNI + * Method: setPWMDisabled + * Signature: (I)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMDisabled( + JNIEnv* env, jclass, jint id) { + PWMJNI_LOG(logDEBUG) << "PWM Handle = " << (HalDigitalHandle)id; + int32_t status = 0; + setPWMDisabled((HalDigitalHandle)id, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + /* * Class: edu_wpi_first_wpilibj_hal_PWMJNI * Method: latchPWMZero diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Jaguar.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Jaguar.java index 3c8021b6d5..f0d28dc17b 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Jaguar.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Jaguar.java @@ -37,7 +37,7 @@ public class Jaguar extends PWMSpeedController { */ setBounds(2.31, 1.55, 1.507, 1.454, .697); setPeriodMultiplier(PeriodMultiplier.k1X); - setRaw(m_centerPwm); + setSpeed(0.0); setZeroLatch(); UsageReporting.report(tResourceType.kResourceType_Jaguar, getChannel()); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/PWM.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/PWM.java index bc0fe50594..12b4ba55d6 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/PWM.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/PWM.java @@ -63,40 +63,6 @@ public class PWM extends SensorBase implements LiveWindowSendable { private int m_channel; private int m_handle; - /** - * kDefaultPwmPeriod is in ms. - * - *

- 20ms periods (50 Hz) are the "safest" setting in that this works for all devices - 20ms - * periods seem to be desirable for Vex Motors - 20ms periods are the specified period for - * HS-322HD servos, but work reliably down to 10.0 ms; starting at about 8.5ms, the servo - * sometimes hums and get hot; by 5.0ms the hum is nearly continuous - 10ms periods work well for - * Victor 884 - 5ms periods allows higher update rates for Luminary Micro Jaguar speed - * controllers. Due to the shipping firmware on the Jaguar, we can't run the update period less - * than 5.05 ms. - * - *

kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period scaling is implemented - * as an output squelch to get longer periods for old devices. - */ - protected static final double kDefaultPwmPeriod = 5.05; - /** - * kDefaultPwmCenter is the PWM range center in ms. - */ - protected static final double kDefaultPwmCenter = 1.5; - /** - * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint. - */ - protected static final int kDefaultPwmStepsDown = 1000; - public static final int kPwmDisabled = 0; - private boolean m_eliminateDeadband; - private int m_maxPwm; - private int m_deadbandMaxPwm; - /* - * Intentionally package private - */ - int m_centerPwm; - private int m_deadbandMinPwm; - private int m_minPwm; - /** * Allocate a PWM given a channel. * @@ -108,9 +74,9 @@ public class PWM extends SensorBase implements LiveWindowSendable { m_handle = PWMJNI.initializePWMPort(DIOJNI.getPort((byte) channel)); - PWMJNI.setPWM(m_handle, (short) 0); + setDisabled(); - m_eliminateDeadband = false; + PWMJNI.setPWMEliminateDeadband(m_handle, false); UsageReporting.report(tResourceType.kResourceType_PWM, channel); } @@ -124,7 +90,7 @@ public class PWM extends SensorBase implements LiveWindowSendable { if (m_handle == 0) { return; } - PWMJNI.setPWM(m_handle, (short) 0); + setDisabled(); PWMJNI.freePWMPort(m_handle); m_handle = 0; } @@ -137,7 +103,7 @@ public class PWM extends SensorBase implements LiveWindowSendable { * modifying any values. */ public void enableDeadbandElimination(boolean eliminateDeadband) { - m_eliminateDeadband = eliminateDeadband; + PWMJNI.setPWMEliminateDeadband(m_handle, eliminateDeadband); } /** @@ -154,13 +120,9 @@ public class PWM extends SensorBase implements LiveWindowSendable { * double, double)} */ @Deprecated - public void setBounds(final int max, final int deadbandMax, final int center, + public void setRawBounds(final int max, final int deadbandMax, final int center, final int deadbandMin, final int min) { - m_maxPwm = max; - m_deadbandMaxPwm = deadbandMax; - m_centerPwm = center; - m_deadbandMinPwm = deadbandMin; - m_minPwm = min; + PWMJNI.setPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMax, min); } /** @@ -174,18 +136,18 @@ public class PWM extends SensorBase implements LiveWindowSendable { * @param deadbandMin The low end of the deadband pulse width in ms * @param min The minimum pulse width in ms */ - protected void setBounds(double max, double deadbandMax, double center, double deadbandMin, + public void setBounds(double max, double deadbandMax, double center, double deadbandMin, double min) { - double loopTime = - DIOJNI.getLoopTiming() / (kSystemClockTicksPerMicrosecond * 1e3); - - m_maxPwm = (int) ((max - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1); - m_deadbandMaxPwm = - (int) ((deadbandMax - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1); - m_centerPwm = (int) ((center - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1); - m_deadbandMinPwm = - (int) ((deadbandMin - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1); - m_minPwm = (int) ((min - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1); + PWMJNI.setPWMConfig(m_handle, max, deadbandMax, center, deadbandMax, min); + } + + /** + * Gets the bounds on the PWM pulse widths. This Gets the bounds on the PWM values for a + * particular type of controller. The values determine the upper and lower speeds as well + * as the deadband bracket. + */ + public PWMConfigDataResult getRawBounds() { + return PWMJNI.getPWMConfigRaw(m_handle); } /** @@ -207,19 +169,7 @@ public class PWM extends SensorBase implements LiveWindowSendable { * @pre SetMinNegativePwm() called. */ public void setPosition(double pos) { - if (pos < 0.0) { - pos = 0.0; - } else if (pos > 1.0) { - pos = 1.0; - } - - int rawValue; - // note, need to perform the multiplication below as floating point before - // converting to int - rawValue = (int) ((pos * (double) getFullRangeScaleFactor()) + getMinNegativePwm()); - - // send the computed pwm value to the FPGA - setRaw(rawValue); + PWMJNI.setPWMPosition(m_handle, (float)pos); } /** @@ -232,14 +182,7 @@ public class PWM extends SensorBase implements LiveWindowSendable { * @pre SetMinNegativePwm() called. */ public double getPosition() { - int value = getRaw(); - if (value < getMinNegativePwm()) { - return 0.0; - } else if (value > getMaxPositivePwm()) { - return 1.0; - } else { - return (double) (value - getMinNegativePwm()) / (double) getFullRangeScaleFactor(); - } + return PWMJNI.getPWMPosition(m_handle); } /** @@ -254,30 +197,8 @@ public class PWM extends SensorBase implements LiveWindowSendable { * @pre SetMaxNegativePwm() called. * @pre SetMinNegativePwm() called. */ - final void setSpeed(double speed) { - // clamp speed to be in the range 1.0 >= speed >= -1.0 - if (speed < -1.0) { - speed = -1.0; - } else if (speed > 1.0) { - speed = 1.0; - } - - // calculate the desired output pwm value by scaling the speed appropriately - int rawValue; - if (speed == 0.0) { - rawValue = getCenterPwm(); - } else if (speed > 0.0) { - rawValue = - (int) (speed * ((double) getPositiveScaleFactor()) - + ((double) getMinPositivePwm()) + 0.5); - } else { - rawValue = - (int) (speed * ((double) getNegativeScaleFactor()) - + ((double) getMaxNegativePwm()) + 0.5); - } - - // send the computed pwm value to the FPGA - setRaw(rawValue); + public void setSpeed(double speed) { + PWMJNI.setPWMSpeed(m_handle, (float)speed); } /** @@ -292,18 +213,7 @@ public class PWM extends SensorBase implements LiveWindowSendable { * @pre SetMinNegativePwm() called. */ public double getSpeed() { - int value = getRaw(); - if (value > getMaxPositivePwm()) { - return 1.0; - } else if (value < getMinNegativePwm()) { - return -1.0; - } else if (value > getMinPositivePwm()) { - return (double) (value - getMinPositivePwm()) / (double) getPositiveScaleFactor(); - } else if (value < getMaxNegativePwm()) { - return (double) (value - getMaxNegativePwm()) / (double) getNegativeScaleFactor(); - } else { - return 0.0; - } + return PWMJNI.getPWMSpeed(m_handle); } /** @@ -314,7 +224,7 @@ public class PWM extends SensorBase implements LiveWindowSendable { * @param value Raw PWM value. Range 0 - 255. */ public void setRaw(int value) { - PWMJNI.setPWM(m_handle, (short) value); + PWMJNI.setPWMRaw(m_handle, (short) value); } /** @@ -325,7 +235,15 @@ public class PWM extends SensorBase implements LiveWindowSendable { * @return Raw PWM control value. Range: 0 - 255. */ public int getRaw() { - return PWMJNI.getPWM(m_handle); + return PWMJNI.getPWMRaw(m_handle); + } + + /** + * Temporarily disables the PWM output. The next set call will reenable + * the output. + */ + public void setDisabled() { + PWMJNI.setPWMDisabled(m_handle); } /** @@ -356,38 +274,6 @@ public class PWM extends SensorBase implements LiveWindowSendable { PWMJNI.latchPWMZero(m_handle); } - private int getMaxPositivePwm() { - return m_maxPwm; - } - - private int getMinPositivePwm() { - return m_eliminateDeadband ? m_deadbandMaxPwm : m_centerPwm + 1; - } - - private int getCenterPwm() { - return m_centerPwm; - } - - private int getMaxNegativePwm() { - return m_eliminateDeadband ? m_deadbandMinPwm : m_centerPwm - 1; - } - - private int getMinNegativePwm() { - return m_minPwm; - } - - private int getPositiveScaleFactor() { - return getMaxPositivePwm() - getMinPositivePwm(); - } // /< The scale for positive speeds. - - private int getNegativeScaleFactor() { - return getMaxNegativePwm() - getMinNegativePwm(); - } // /< The scale for negative speeds. - - private int getFullRangeScaleFactor() { - return getMaxPositivePwm() - getMinNegativePwm(); - } // /< The scale for positions. - /* * Live Window code, only does anything if live window is activated. */ diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/PWMConfigDataResult.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/PWMConfigDataResult.java new file mode 100644 index 0000000000..e8e0630ad7 --- /dev/null +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/PWMConfigDataResult.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-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; + +/** + * Structure for holding the config data result for PWM. + */ +public class PWMConfigDataResult { + + PWMConfigDataResult(int max, int deadbandMax, int center, int deadbandMin, int min) { + this.max = max; + this.deadbandMax = deadbandMax; + this.center = center; + this.deadbandMin = deadbandMin; + this.min = min; + } + + /** + * The maximum PWM value. + */ + @SuppressWarnings("MemberName") + public int max; + + /** + * The deadband maximum PWM value. + */ + @SuppressWarnings("MemberName") + public int deadbandMax; + + /** + * The center PWM value. + */ + @SuppressWarnings("MemberName") + public int center; + + /** + * The deadband minimum PWM value. + */ + @SuppressWarnings("MemberName") + public int deadbandMin; + + /** + * The minimum PWM value. + */ + @SuppressWarnings("MemberName") + public int min; +} diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SD540.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SD540.java index 2f2f74aa2a..7fc7f5c0f5 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SD540.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SD540.java @@ -32,7 +32,7 @@ public class SD540 extends PWMSpeedController { protected void initSD540() { setBounds(2.05, 1.55, 1.50, 1.44, .94); setPeriodMultiplier(PeriodMultiplier.k1X); - setRaw(m_centerPwm); + setSpeed(0.0); setZeroLatch(); LiveWindow.addActuator("SD540", getChannel(), this); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SafePWM.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SafePWM.java index 9a542071b1..bed1b8162a 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SafePWM.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SafePWM.java @@ -90,6 +90,6 @@ public class SafePWM extends PWM implements MotorSafety { } public void disable() { - setRaw(kPwmDisabled); + setDisabled(); } } diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Spark.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Spark.java index e85cb082c3..5a277d52a8 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Spark.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Spark.java @@ -32,7 +32,7 @@ public class Spark extends PWMSpeedController { protected void initSpark() { setBounds(2.003, 1.55, 1.50, 1.46, .999); setPeriodMultiplier(PeriodMultiplier.k1X); - setRaw(m_centerPwm); + setSpeed(0.0); setZeroLatch(); LiveWindow.addActuator("Spark", getChannel(), this); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Talon.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Talon.java index 8b2b49cde7..80787604cf 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Talon.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Talon.java @@ -37,7 +37,7 @@ public class Talon extends PWMSpeedController { setBounds(2.037, 1.539, 1.513, 1.487, .989); setPeriodMultiplier(PeriodMultiplier.k1X); - setRaw(m_centerPwm); + setSpeed(0.0); setZeroLatch(); LiveWindow.addActuator("Talon", getChannel(), this); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/TalonSRX.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/TalonSRX.java index 2b66c7e6ea..19e0be075c 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/TalonSRX.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/TalonSRX.java @@ -40,7 +40,7 @@ public class TalonSRX extends PWMSpeedController { setBounds(2.004, 1.52, 1.50, 1.48, .997); setPeriodMultiplier(PeriodMultiplier.k1X); - setRaw(m_centerPwm); + setSpeed(0.0); setZeroLatch(); LiveWindow.addActuator("TalonSRX", getChannel(), this); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Victor.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Victor.java index 31cb12b699..9ae1c32198 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Victor.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Victor.java @@ -39,7 +39,7 @@ public class Victor extends PWMSpeedController { setBounds(2.027, 1.525, 1.507, 1.49, 1.026); setPeriodMultiplier(PeriodMultiplier.k2X); - setRaw(m_centerPwm); + setSpeed(0.0); setZeroLatch(); LiveWindow.addActuator("Victor", getChannel(), this); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/VictorSP.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/VictorSP.java index 1a457be3b3..192c5c2b54 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/VictorSP.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/VictorSP.java @@ -37,7 +37,7 @@ public class VictorSP extends PWMSpeedController { setBounds(2.004, 1.52, 1.50, 1.48, .997); setPeriodMultiplier(PeriodMultiplier.k1X); - setRaw(m_centerPwm); + setSpeed(0.0); setZeroLatch(); LiveWindow.addActuator("VictorSP", getChannel(), this); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/PWMJNI.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/PWMJNI.java index c5c73c9324..bc933a59eb 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/PWMJNI.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/PWMJNI.java @@ -7,15 +7,41 @@ package edu.wpi.first.wpilibj.hal; +import edu.wpi.first.wpilibj.PWMConfigDataResult; + @SuppressWarnings("AbbreviationAsWordInName") public class PWMJNI extends DIOJNI { public static native int initializePWMPort(int halPortHandle); public static native void freePWMPort(int pwmPortHandle); + + public static native void setPWMConfigRaw(int pwmPortHandle, int maxPwm, + int deadbandMaxPwm, int centerPwm, + int deadbandMinPwm, int minPwm); + + public static native void setPWMConfig(int pwmPortHandle, double maxPwm, + double deadbandMaxPwm, double centerPwm, + double deadbandMinPwm, double minPwm); - public static native void setPWM(int pwmPortHandle, short value); + public static native PWMConfigDataResult getPWMConfigRaw(int pwmPortHandle); - public static native short getPWM(int pwmPortHandle); + public static native void setPWMEliminateDeadband(int pwmPortHandle, boolean eliminateDeadband); + + public static native boolean getPWMEliminateDeadband(int pwmPortHandle); + + public static native void setPWMRaw(int pwmPortHandle, short value); + + public static native void setPWMSpeed(int pwmPortHandle, float speed); + + public static native void setPWMPosition(int pwmPortHandle, float position); + + public static native short getPWMRaw(int pwmPortHandle); + + public static native float getPWMSpeed(int pwmPortHandle); + + public static native float getPWMPosition(int pwmPortHandle); + + public static native void setPWMDisabled(int pwmPortHandle); public static native void latchPWMZero(int pwmPortHandle);