From a481e1f2c00e80f08e7b6226dd051ac6b3266d48 Mon Sep 17 00:00:00 2001 From: conroy-cheers Date: Sat, 11 Sep 2021 19:23:39 +1000 Subject: [PATCH 1/2] Add HardwareEncoder implementation for STM32 --- src/SimpleFOC.h | 1 + src/sensors/HardwareEncoder.cpp | 150 ++++++++++++++++++++++++++++++++ src/sensors/HardwareEncoder.h | 62 +++++++++++++ 3 files changed, 213 insertions(+) create mode 100644 src/sensors/HardwareEncoder.cpp create mode 100644 src/sensors/HardwareEncoder.h diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4f9f1108..5f2042e3 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -99,6 +99,7 @@ void loop() { #include "BLDCMotor.h" #include "StepperMotor.h" #include "sensors/Encoder.h" +#include "sensors/HardwareEncoder.h" #include "sensors/MagneticSensorSPI.h" #include "sensors/MagneticSensorI2C.h" #include "sensors/MagneticSensorAnalog.h" diff --git a/src/sensors/HardwareEncoder.cpp b/src/sensors/HardwareEncoder.cpp new file mode 100644 index 00000000..901ef188 --- /dev/null +++ b/src/sensors/HardwareEncoder.cpp @@ -0,0 +1,150 @@ +#include "HardwareEncoder.h" + +/* + HardwareEncoder(int cpr) +*/ +HardwareEncoder::HardwareEncoder(unsigned int _ppr) { + rotations_per_overflow = 0; + ticks_per_overflow = 0; + + overflow_count = 0; + count = 0; + prev_count = 0; + pulse_timestamp = 0; + + cpr = _ppr; + + // velocity calculation variables + prev_timestamp = getCurrentMicros(); + pulse_timestamp = getCurrentMicros(); +} + +void HardwareEncoder::update() { + // handle overflow of the 16-bit counter here + // must be called at least twice per traversal of overflow range + // TODO(conroy-cheers): investigate performance impact + prev_count = count; + count = encoder_handle.Instance->CNT; + + prev_timestamp = pulse_timestamp; + pulse_timestamp = getCurrentMicros(); + + if (prev_count > (ticks_per_overflow - overflow_margin) && + prev_count <= ticks_per_overflow && count < overflow_margin) + ++overflow_count; + if (prev_count >= 0 && prev_count < overflow_margin && + count >= (ticks_per_overflow - overflow_margin)) + --overflow_count; +} + +/* + Shaft angle calculation +*/ +float HardwareEncoder::getSensorAngle() { return getAngle(); } + +float HardwareEncoder::getMechanicalAngle() { + return _2PI * (count % static_cast(cpr)) / static_cast(cpr); +} +float HardwareEncoder::getAngle() { + return _2PI * (count / static_cast(cpr) + + overflow_count * rotations_per_overflow); +} +double HardwareEncoder::getPreciseAngle() { + return _2PI * (count / static_cast(cpr) + + overflow_count * rotations_per_overflow); +} +int32_t HardwareEncoder::getFullRotations() { + return count / static_cast(cpr) + + overflow_count * rotations_per_overflow; +} + +/* + Shaft velocity calculation +*/ +float HardwareEncoder::getVelocity() { + // sampling time calculation + float dt = (pulse_timestamp - prev_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow) + if (dt <= 0 || dt > 0.5f) + dt = 1e-3f; + + // time from last impulse + int32_t dN = count - prev_count; + + float pulse_per_second = dN / dt; + + // velocity calculation + return pulse_per_second / (static_cast(cpr)) * _2PI; +} + +// getter for index pin +// return -1 if no index +int HardwareEncoder::needsSearch() { return -1; } + +// private function used to determine if encoder has index +int HardwareEncoder::hasIndex() { return 0; } + +// encoder initialisation of the hardware pins +// and calculation variables +void HardwareEncoder::init() { + // TODO(conroy-cheers): make quadrature optional + cpr *= 4; + + // overflow handling + rotations_per_overflow = 0xFFFF / cpr; + ticks_per_overflow = cpr * rotations_per_overflow; + + // set up GPIO + GPIO_InitTypeDef gpio; + + gpio.Pin = GPIO_PIN_6; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_PULLDOWN; + gpio.Speed = GPIO_SPEED_FREQ_MEDIUM; + gpio.Alternate = GPIO_AF2_TIM4; + HAL_GPIO_Init(GPIOB, &gpio); + + gpio.Pin = GPIO_PIN_7; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_PULLDOWN; + gpio.Speed = GPIO_SPEED_FREQ_MEDIUM; + gpio.Alternate = GPIO_AF2_TIM4; + HAL_GPIO_Init(GPIOB, &gpio); + + // set up timer for encoder + encoder_handle.Init.Period = ticks_per_overflow; + encoder_handle.Init.Prescaler = 0; + encoder_handle.Init.ClockDivision = 0; + encoder_handle.Init.CounterMode = TIM_COUNTERMODE_UP; + encoder_handle.Init.RepetitionCounter = 0; + encoder_handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + + TIM_Encoder_InitTypeDef encoder_config; + + encoder_config.EncoderMode = TIM_ENCODERMODE_TI12; + + encoder_config.IC1Polarity = TIM_ICPOLARITY_RISING; + encoder_config.IC1Selection = TIM_ICSELECTION_DIRECTTI; + encoder_config.IC1Prescaler = TIM_ICPSC_DIV1; + encoder_config.IC1Filter = 0; + + encoder_config.IC2Polarity = TIM_ICPOLARITY_RISING; + encoder_config.IC2Selection = TIM_ICSELECTION_DIRECTTI; + encoder_config.IC2Prescaler = TIM_ICPSC_DIV1; + encoder_config.IC2Filter = 0; + + encoder_handle.Instance = TIM4; + enableTimerClock(&encoder_handle); + if (HAL_TIM_Encoder_Init(&encoder_handle, &encoder_config) != HAL_OK) { + _Error_Handler(__FILE__, __LINE__); + } + HAL_TIM_Encoder_Start(&encoder_handle, TIM_CHANNEL_1); + + // counter setup + overflow_count = 0; + count = 0; + prev_count = 0; + + prev_timestamp = getCurrentMicros(); + pulse_timestamp = getCurrentMicros(); +} diff --git a/src/sensors/HardwareEncoder.h b/src/sensors/HardwareEncoder.h new file mode 100644 index 00000000..ba8726ee --- /dev/null +++ b/src/sensors/HardwareEncoder.h @@ -0,0 +1,62 @@ +#ifndef HARDWARE_ENCODER_LIB_H +#define HARDWARE_ENCODER_LIB_H + +#include +#include + +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +class HardwareEncoder : public Sensor { + public: + /** + Encoder class constructor + @param ppr impulses per rotation (cpr=ppr*4) + */ + explicit HardwareEncoder(unsigned int ppr); + + /** encoder initialise pins */ + void init() override; + + // Encoder configuration + unsigned int cpr; //!< encoder cpr number + + // Abstract functions of the Sensor class implementation + /** get current angle (rad) */ + float getSensorAngle() override; + float getMechanicalAngle() override; + /** get current angular velocity (rad/s) */ + float getVelocity() override; + float getAngle() override; + double getPreciseAngle() override; + int32_t getFullRotations() override; + void update() override; + + /** + * returns 0 if it does need search for absolute zero + * 0 - encoder without index + * 1 - ecoder with index + */ + int needsSearch() override; + + private: + int hasIndex(); // !< function returning 1 if encoder has index pin and 0 if not. + + void handleOverflow(); + + TIM_HandleTypeDef encoder_handle; + + static constexpr u_int16_t overflow_margin = 20000; + u_int16_t rotations_per_overflow; + u_int16_t ticks_per_overflow; + + volatile int32_t overflow_count; + volatile u_int16_t count; //!< current pulse counter + volatile u_int16_t prev_count; + + // velocity calculation variables + volatile int32_t pulse_timestamp, prev_timestamp; +}; + +#endif From 5b7f3a2ea6d2796706343332c35aa278603fe536 Mon Sep 17 00:00:00 2001 From: conroy-cheers Date: Sat, 2 Oct 2021 16:13:25 +1000 Subject: [PATCH 2/2] Fix velocity calculation at overflow --- src/sensors/HardwareEncoder.cpp | 9 ++++++--- src/sensors/HardwareEncoder.h | 1 + 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/sensors/HardwareEncoder.cpp b/src/sensors/HardwareEncoder.cpp index 901ef188..3d6a756b 100644 --- a/src/sensors/HardwareEncoder.cpp +++ b/src/sensors/HardwareEncoder.cpp @@ -10,6 +10,7 @@ HardwareEncoder::HardwareEncoder(unsigned int _ppr) { overflow_count = 0; count = 0; prev_count = 0; + prev_overflow_count = 0; pulse_timestamp = 0; cpr = _ppr; @@ -29,6 +30,7 @@ void HardwareEncoder::update() { prev_timestamp = pulse_timestamp; pulse_timestamp = getCurrentMicros(); + prev_overflow_count = overflow_count; if (prev_count > (ticks_per_overflow - overflow_margin) && prev_count <= ticks_per_overflow && count < overflow_margin) ++overflow_count; @@ -69,7 +71,8 @@ float HardwareEncoder::getVelocity() { dt = 1e-3f; // time from last impulse - int32_t dN = count - prev_count; + int32_t overflow_diff = overflow_count - prev_overflow_count; + int32_t dN = (count - prev_count) + (ticks_per_overflow * overflow_diff); float pulse_per_second = dN / dt; @@ -78,8 +81,7 @@ float HardwareEncoder::getVelocity() { } // getter for index pin -// return -1 if no index -int HardwareEncoder::needsSearch() { return -1; } +int HardwareEncoder::needsSearch() { return false; } // private function used to determine if encoder has index int HardwareEncoder::hasIndex() { return 0; } @@ -144,6 +146,7 @@ void HardwareEncoder::init() { overflow_count = 0; count = 0; prev_count = 0; + prev_overflow_count = 0; prev_timestamp = getCurrentMicros(); pulse_timestamp = getCurrentMicros(); diff --git a/src/sensors/HardwareEncoder.h b/src/sensors/HardwareEncoder.h index ba8726ee..a1587295 100644 --- a/src/sensors/HardwareEncoder.h +++ b/src/sensors/HardwareEncoder.h @@ -54,6 +54,7 @@ class HardwareEncoder : public Sensor { volatile int32_t overflow_count; volatile u_int16_t count; //!< current pulse counter volatile u_int16_t prev_count; + volatile int32_t prev_overflow_count; // velocity calculation variables volatile int32_t pulse_timestamp, prev_timestamp;