adding MagneticSensorAnalog implementation

Author: owennewo

examples/motion_control/torque_voltage_control/analog/voltage_control.ino

@@ -0,0 +1,107 @@
+/**
+ * 
+ * Torque control example using voltage control loop.
+ * 
+ * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers 
+ * you a way to control motor torque by setting the voltage to the motor instead of the current. 
+ * 
+ * This makes the BLDC motor effectively a DC motor, and you can use it in a same way.
+ */
+#include <SimpleFOC.h>
+
+// motor instance
+BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 7);
+
+/**
+ * magnetic sensor reading analog voltage on pin A1.  This voltage is proportional to rotation position.
+ * Tested on AS5600 magnetic sensor running in 'analog mode'.  Note AS5600 works better in 'i2C mode' (less noise) but only supports one sensor per i2c bus. 
+ */
+MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
+
+
+
+void setup() { 
+  
+  // initialize encoder sensor hardware
+  sensor.init();
+  // link the motor to the sensor
+  motor.linkSensor(&sensor);
+
+  // power supply voltage
+  // default 12V
+  motor.voltage_power_supply = 12;
+  // aligning voltage
+  motor.voltage_sensor_align = 3;
+  
+  // choose FOC modulation (optional)
+  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
+
+  // set motion control loop to be used
+  motor.controller = ControlType::voltage;
+
+  // use monitoring with serial for motor init
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+
+  // use monitoring with serial 
+  Serial.begin(115200);
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+
+  // initialize motor
+  motor.init();
+  // align sensor and start FOC
+  motor.initFOC();
+
+  Serial.println("Motor ready.");
+  Serial.println("Set the target voltage using serial terminal:");
+  _delay(1000);
+}
+
+// target voltage to be set to the motor
+float target_voltage = 2;
+
+void loop() {
+
+  // main FOC algorithm function
+  // the faster you run this function the better
+  // Arduino UNO loop  ~1kHz
+  // Bluepill loop ~10kHz 
+  motor.loopFOC();
+
+  // Motion control function
+  // velocity, position or voltage (defined in motor.controller)
+  // this function can be run at much lower frequency than loopFOC() function
+  // You can also use motor.move() and set the motor.target in the code
+  motor.move(target_voltage);
+  
+  // communicate with the user
+  serialReceiveUserCommand();
+}
+
+
+// utility function enabling serial communication with the user to set the target values
+// this function can be implemented in serialEvent function as well
+void serialReceiveUserCommand() {
+  
+  // a string to hold incoming data
+  static String received_chars;
+  
+  while (Serial.available()) {
+    // get the new byte:
+    char inChar = (char)Serial.read();
+    // add it to the string buffer:
+    received_chars += inChar;
+    // end of user input
+    if (inChar == '\n') {
+      
+      // change the motor target
+      target_voltage = received_chars.toFloat();
+      Serial.print("Target voltage: ");
+      Serial.println(target_voltage);
+      
+      // reset the command buffer 
+      received_chars = "";
+    }
+  }
+}

src/MagneticSensorAnalog.cpp

@@ -0,0 +1,108 @@
+#include "MagneticSensorAnalog.h"
+
+/** MagneticSensorAnalog(uint8_t _pinAnalog, int _min, int _max)
+ * @param _pinAnalog  the pin that is reading the pwm from magnetic sensor
+ * @param _min_raw_count  the smallest expected reading.  Whilst you might expect it to be 0 it is often ~15.  Getting this wrong results in a small click once per revolution
+ * @param _max_raw_count  the largest value read.  whilst you might expect it to be 2^10 = 1023 it is often ~ 1020
+ */
+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();  
+	zero_offset = 0;
+}
+
+//  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 = natural_direction * (full_rotation_offset + ( (float) (raw_count - min_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;
+}
+
+// set current angle as zero angle 
+// return the angle [rad] difference
+float MagneticSensorAnalog::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 MagneticSensorAnalog::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 MagneticSensorAnalog::hasAbsoluteZero(){
+  return 1;
+}
+// returns 0 if it does need search for absolute zero
+// 0 - magnetic sensor 
+// 1 - ecoder with index
+int MagneticSensorAnalog::needsAbsoluteZeroSearch(){
+  return 0;
+}
+
+// function reading the raw counter of the magnetic sensor
+int MagneticSensorAnalog::getRawCount(){
+	return analogRead(pinAnalog);
+}

src/MagneticSensorAnalog.h

@@ -0,0 +1,82 @@
+#ifndef MAGNETICSENSORANALOG_LIB_H
+#define MAGNETICSENSORANALOG_LIB_H
+
+#include "Arduino.h"
+#include <Wire.h>
+#include "FOCutils.h"
+#include "Sensor.h"
+
+/**
+ * This sensor has been tested with AS5600 running in 'analog mode'.  This is where output pin of AS6000 is connected to an analog pin on your microcontroller.
+ * This approach is very simple but you may more accurate results with MagneticSensorI2C if that is also supported as its skips the DAC -> ADC conversion (ADC supports MagneticSensorI2C)
+ */
+class MagneticSensorAnalog: public Sensor{
+ public:
+    /**
+     * MagneticSensorAnalog class constructor
+     * @param _pinAnalog  the pin to read the PWM signal
+     * @param _pinAnalog  the pin to read the PWM signal
+     */
+    MagneticSensorAnalog(uint8_t _pinAnalog, int _min = 0, int _max = 0);
+    
+
+    /** sensor initialise pins */
+    void init();
+
+    int pinAnalog; //!< encoder hardware pin A
+    
+    // Encoder configuration
+    Pullup pullup;
+
+    // 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();
+    /** raw count (typically in range of 0-1023), useful for debugging resolution issues */
+    int raw_count;
+    int min_raw_count;
+    int max_raw_count;
+    int cpr;
+
+  private:
+    // float cpr; //!< Maximum range of the magnetic sensor
+    
+
+    int read();
+
+    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
+    int raw_count_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
+    float velocity;
+    
+
+};
+
+
+#endif

src/SimpleFOC.h

@@ -91,6 +91,7 @@ void loop() {
 #include "Encoder.h"
 #include "MagneticSensorSPI.h"
 #include "MagneticSensorI2C.h"
+#include "MagneticSensorAnalog.h"
 #include "HallSensor.h"
 #include "BLDCMotor.h"