FEAT updated with library version 1.4.0

Author: askuric

arduino_foc_minimal_encoder/FOCutils.cpp

@@ -1,7 +1,7 @@
 #include "FOCutils.h"
 
 
-void setPwmFrequency(int pin) {
+void _setPwmFrequency(int pin) {
 #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other atmega328p chips
 //  High PWM frequency
 //  https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328

arduino_foc_minimal_encoder/FOCutils.h

@@ -26,7 +26,7 @@
  * 
  * @param pin pin number to configure
  */
-void setPwmFrequency(int pin);
+void _setPwmFrequency(int pin);
 
 /** 
  * Function implementing delay() function in milliseconds 

arduino_foc_minimal_encoder/SimpleFOC.h

@@ -39,19 +39,20 @@
  *
  */
 
-#include "SimpleFOC.h"
+#include "BLDCMotor.h"
+#include "Encoder.h"
 
 //  BLDCMotor( int phA, int phB, int phC, int pp, int en)
 //  - phA, phB, phC - motor A,B,C phase pwm pins
 //  - pp            - pole pair number
 //  - enable pin    - (optional input)
-BLDCMotor motor = BLDCMotor(3, 10, 11, 11,7);
+BLDCMotor motor = BLDCMotor(9, 10, 11, 11,8);
 
 //Encoder(int encA, int encB , int cpr, int index)
 //    - encA, encB    - encoder A and B pins
 //    - ppr           - impulses per rotation  (cpr=ppr*4)
 //    - index pin     - (optional input)
-Encoder encoder = Encoder(A1, A2, 8196);
+Encoder encoder = Encoder(2, 3, 8196);
 // interrupt routine intialisation
 void doA(){encoder.handleA();}
 void doB(){encoder.handleB();}

arduino_foc_minimal_encoder/arduino_foc_minimal_encoder.ino

@@ -70,9 +70,9 @@ void BLDCMotor::init() {
   if(monitor_port) monitor_port->println("MONITOR: Set high frequency PWM.");
   // Increase PWM frequency to 32 kHz
   // make silent
-  setPwmFrequency(pwmA);
-  setPwmFrequency(pwmB);
-  setPwmFrequency(pwmC);
+  _setPwmFrequency(pwmA);
+  _setPwmFrequency(pwmB);
+  _setPwmFrequency(pwmC);
 
   // sanity check for the voltage limit configuration
   if(PI_velocity.voltage_limit > voltage_power_supply) PI_velocity.voltage_limit =  voltage_power_supply;

arduino_foc_minimal_magnetic/BLDCMotor.cpp

@@ -1,7 +1,7 @@
 #include "FOCutils.h"
 
 
-void setPwmFrequency(int pin) {
+void _setPwmFrequency(int pin) {
 #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other atmega328p chips
 //  High PWM frequency
 //  https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328

arduino_foc_minimal_magnetic/FOCutils.cpp

@@ -26,7 +26,7 @@
  * 
  * @param pin pin number to configure
  */
-void setPwmFrequency(int pin);
+void _setPwmFrequency(int pin);
 
 /** 
  * Function implementing delay() function in milliseconds 

arduino_foc_minimal_magnetic/FOCutils.h

@@ -0,0 +1,153 @@
+#include "MagneticSensorI2C.h"
+
+// MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb)
+//  @param _chip_address  I2C chip address
+//  @param _bit_resolution  bit resolution of the sensor  
+//  @param _angle_register_msb  angle read register
+//  @param _bits_used_msb number of used bits in msb
+MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){
+  // chip I2C address
+  chip_address = _chip_address; 
+  // angle read register of the magnetic sensor
+  angle_register_msb = _angle_register_msb;
+  // register maximum value (counts per revolution)
+  cpr = pow(2,_bit_resolution);
+  
+  // depending on the sensor architecture there are different combinations of
+  // LSB and MSB register used bits
+  // AS5600 uses 0..7 LSB and 8..11 MSB
+  // AS5048 uses 0..5 LSB and 6..13 MSB 
+  // used bits in LSB
+  lsb_used = _bit_resolution - _bits_used_msb;
+  // extraction masks
+  lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
+  msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 );
+}
+
+
+void MagneticSensorI2C::init(){
+  
+	//I2C communication begin
+	Wire.begin();
+
+	// velocity calculation init
+	angle_prev = 0;
+	velocity_calc_timestamp = _micros(); 
+
+	// full rotations tracking number
+	full_rotation_offset = 0;
+	angle_data_prev = getRawCount();  
+	zero_offset = 0;
+}
+
+//  Shaft angle calculation
+//  angle is in radians [rad]
+float MagneticSensorI2C::getAngle(){
+  // raw data from the sensor
+  float angle_data = getRawCount(); 
+
+  // tracking the number of rotations 
+  // in order to expand angle range form [0,2PI] 
+  // to basically infinity
+  float d_angle = angle_data - angle_data_prev;
+  // if overflow happened track it as full rotation
+  if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; 
+  // save the current angle value for the next steps
+  // in order to know if overflow happened
+  angle_data_prev = angle_data;
+
+  // zero offset adding
+  angle_data -= (int)zero_offset;
+  // return the full angle 
+  // (number of full rotations)*2PI + current sensor angle
+  return full_rotation_offset + ( angle_data / (float)cpr) * _2PI;
+}
+
+// Shaft velocity calculation
+float MagneticSensorI2C::getVelocity(){
+  // calculate sample time
+  float Ts = (_micros() - velocity_calc_timestamp)*1e-6;
+  // quick fix for strange cases (micros overflow)
+  if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; 
+
+  // current angle
+  float angle_c = getAngle();
+  // velocity calculation
+  float vel = (angle_c - angle_prev)/Ts;
+  
+  // save variables for future pass
+  angle_prev = angle_c;
+  velocity_calc_timestamp = _micros();
+  return vel;
+}
+
+// set current angle as zero angle 
+// return the angle [rad] difference
+float MagneticSensorI2C::initRelativeZero(){
+  float angle_offset = -getAngle();
+  zero_offset = getRawCount();
+
+  // angle tracking variables
+  full_rotation_offset = 0;
+  return angle_offset;
+}
+// set absolute zero angle as zero angle
+// return the angle [rad] difference
+float MagneticSensorI2C::initAbsoluteZero(){
+  float rotation = -(int)zero_offset;
+  // init absolute zero
+  zero_offset = 0;
+
+  // angle tracking variables
+  full_rotation_offset = 0;
+  // return offset in radians
+  return rotation / (float)cpr * _2PI;
+}
+// returns 0 if it has no absolute 0 measurement
+// 0 - incremental encoder without index
+// 1 - encoder with index & magnetic sensors
+int MagneticSensorI2C::hasAbsoluteZero(){
+  return 1;
+}
+// returns 0 if it does need search for absolute zero
+// 0 - magnetic sensor 
+// 1 - ecoder with index
+int MagneticSensorI2C::needsAbsoluteZeroSearch(){
+  return 0;
+}
+
+
+// function reading the raw counter of the magnetic sensor
+int MagneticSensorI2C::getRawCount(){
+	return (int)MagneticSensorI2C::read(angle_register_msb);
+}
+
+// I2C functions 
+/*
+* Read a register from the sensor
+* Takes the address of the register as a uint8_t
+* Returns the value of the register
+*/
+int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
+  // read the angle register first MSB then LSB
+	byte readArray[2];
+	uint16_t readValue = 0;
+  // notify the device that is aboout to be read
+	Wire.beginTransmission(chip_address);
+	Wire.write(angle_reg_msb);
+  Wire.endTransmission(false);
+  
+  // read the data msb and lsb
+	Wire.requestFrom(chip_address, (uint8_t)2);
+	for (byte i=0; i < 2; i++) {
+		readArray[i] = Wire.read();
+	}
+
+  // depending on the sensor architecture there are different combinations of 
+  // LSB and MSB register used bits
+  // AS5600 uses 0..7 LSB and 8..11 MSB
+  // AS5048 uses 0..5 LSB and 6..13 MSB
+  readValue = ( readArray[1] &  lsb_mask );
+	readValue += ( ( readArray[0] & msb_mask ) << lsb_used );
+	return readValue;
+}

arduino_foc_minimal_magnetic/MagneticSensorI2C.cpp

@@ -0,0 +1,78 @@
+#ifndef MAGNETICSENSORI2C_LIB_H
+#define MAGNETICSENSORI2C_LIB_H
+
+#include "Arduino.h"
+#include <Wire.h>
+#include "FOCutils.h"
+#include "Sensor.h"
+
+
+class MagneticSensorI2C: public Sensor{
+ public:
+    /**
+     * MagneticSensorI2C class constructor
+     * @param chip_address  I2C chip address
+     * @param bits number of bits of the sensor resolution 
+     * @param angle_register_msb  angle read register msb
+     * @param _bits_used_msb number of used bits in msb
+     */
+    MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _msb_bits_used);
+    
+
+    /** sensor initialise pins */
+    void init();
+
+    // implementation of abstract functions of the Sensor class
+    /** get current angle (rad) */
+    float getAngle();
+    /** get current angular velocity (rad/s) **/
+    float getVelocity();
+    /**
+     *  set current angle as zero angle 
+     * return the angle [rad] difference
+     */
+    float initRelativeZero();
+    /**
+     *  set absolute zero angle as zero angle
+     * return the angle [rad] difference
+     */
+    float initAbsoluteZero();
+    /** returns 1 because it is the absolute sensor */
+    int hasAbsoluteZero();
+    /** returns 0  maning it doesn't need search for absolute zero */
+    int needsAbsoluteZeroSearch();
+    
+
+  private:
+    float cpr; //!< Maximum range of the magnetic sensor
+    uint16_t lsb_used; //!< Number of bits used in LSB register
+    uint8_t lsb_mask;
+    uint8_t msb_mask;
+    
+    // I2C variables
+    uint8_t angle_register_msb; //!< I2C angle register to read
+    uint8_t chip_address; //!< I2C chip select pins
+
+    // I2C functions
+    /** Read one I2C register value */
+    int read(uint8_t angle_register_msb);
+
+    word zero_offset; //!< user defined zero offset
+    /**
+     * Function getting current angle register value
+     * it uses angle_register variable
+     */
+    int getRawCount();
+
+    // total angle tracking variables
+    float full_rotation_offset; //!<number of full rotations made
+    float angle_data_prev; //!< angle in previous position calculation step
+
+    // velocity calculation variables
+    float angle_prev; //!< angle in previous velocity calculation step
+    long velocity_calc_timestamp; //!< last velocity calculation timestamp
+
+};
+
+
+#endif