diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 6b7afbb2..eee0297b 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -102,6 +102,7 @@ void loop() { #include "sensors/MagneticSensorSPI.h" #include "sensors/MagneticSensorI2C.h" #include "sensors/MagneticSensorAnalog.h" +#include "sensors/MagneticSensorPWM.h" #include "sensors/HallSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp new file mode 100644 index 00000000..7ae989be --- /dev/null +++ b/src/sensors/MagneticSensorPWM.cpp @@ -0,0 +1,66 @@ +#include "MagneticSensorPWM.h" + +/** MagneticSensorPWM(uint8_t _pinPWM, int _min, int _max) + * @param _pinPWM the pin that is reading the pwm from magnetic sensor + * @param _min_raw_count the smallest expected reading + * @param _max_raw_count the largest expected reading + */ +MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _max_raw_count){ + + pinPWM = _pinPWM; + + cpr = _max_raw_count - _min_raw_count; + min_raw_count = _min_raw_count; + max_raw_count = _max_raw_count; + + pinMode(pinPWM, INPUT); +} + + +void MagneticSensorPWM::init(){ + + // velocity calculation init + angle_prev = 0; + velocity_calc_timestamp = _micros(); + + // full rotations tracking number + full_rotation_offset = 0; + raw_count_prev = getRawCount(); +} + +// get current angle (rad) +float MagneticSensorPWM::getAngle(){ + + // raw data from sensor + raw_count = getRawCount(); + + int delta = raw_count - raw_count_prev; + // if overflow happened track it as full rotation + if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; + + float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; + + // calculate velocity here + long now = _micros(); + float Ts = (now - velocity_calc_timestamp)*1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + velocity = (angle - angle_prev)/Ts; + + // save variables for future pass + raw_count_prev = raw_count; + angle_prev = angle; + velocity_calc_timestamp = now; + + return angle; +} + +// get velocity (rad/s) +float MagneticSensorPWM::getVelocity(){ + return velocity; +} + +// read the raw counter of the magnetic sensor +int MagneticSensorPWM::getRawCount(){ + return pulseIn(pinPWM,HIGH); +} diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h new file mode 100644 index 00000000..2957bcff --- /dev/null +++ b/src/sensors/MagneticSensorPWM.h @@ -0,0 +1,57 @@ +#ifndef MAGNETICSENSORPWM_LIB_H +#define MAGNETICSENSORPWM_LIB_H + +#include "Arduino.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +// This sensor has been tested with AS5048a running in PWM mode. + +class MagneticSensorPWM: public Sensor{ + public: + /** + * MagneticSensorPWM class constructor + * @param _pinPWM the pin to read the PWM sensor input signal + */ + MagneticSensorPWM(uint8_t _pinPWM,int _min = 0, int _max = 0); + + + // initialize the sensor hardware + void init(); + + int pinPWM; + + // get current angle (rad) + float getAngle() override; + // get current angular velocity (rad/s) + float getVelocity() override; + + + private: + // raw count (typically in range of 0-1023) + int raw_count; + int min_raw_count; + int max_raw_count; + int cpr; + int read(); + + /** + * Function getting current angle register value + * it uses angle_register variable + */ + int getRawCount(); + + // total angle tracking variables + float full_rotation_offset; //!