MagneticSensorAnalog.cpp

Go to the documentation of this file.

#include "MagneticSensorAnalog.h"
 
MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_count, int _max_raw_count){
  
  pinAnalog = _pinAnalog;
 
  cpr = _max_raw_count - _min_raw_count;
  min_raw_count = _min_raw_count;
  max_raw_count = _max_raw_count;
 
  if(pullup == Pullup::INTERN){
    pinMode(pinAnalog, INPUT_PULLUP);
  }else{
    pinMode(pinAnalog, INPUT);
  }
 
}
 
 
void MagneticSensorAnalog::init(){
 
 // velocity calculation init
 angle_prev = 0;
 velocity_calc_timestamp = _micros(); 
 
 // full rotations tracking number
 full_rotation_offset = 0;
 raw_count_prev = getRawCount();  
}
 
//  Shaft angle calculation
//  angle is in radians [rad]
float MagneticSensorAnalog::getAngle(){
  // raw data from the 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;
}
 
// Shaft velocity calculation
float MagneticSensorAnalog::getVelocity(){
  // TODO: Refactor?: to avoid angle being called twice, velocity is pre-calculted during getAngle
  return velocity;
}
 
// function reading the raw counter of the magnetic sensor
int MagneticSensorAnalog::getRawCount(){
 return analogRead(pinAnalog);
}