Simplification of implementation for new position sensors and current senses: generic sensor classes

README.md

@@ -24,6 +24,7 @@ Therefore this is an attempt to:
       <li>Added the new motion control interface to the commander- possible to set the position, velocity and torque target at once</li>
       <li>NRF52 series mcus support by <a href="https://github.com/Polyphe">@Polyphe</a></li>
       <li>Voltage/current limit handling bugs #118</li>
+      <li>Generic position and current sense classes - to implement a new sensor only implement one function</li>
    </ul>
 </blockquote>
 

examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino

@@ -0,0 +1,59 @@
+/**
+ * An example code for the generic current sensing implementation
+*/
+#include <SimpleFOC.h>
+
+
+// user defined function for reading the phase currents
+// returning the value per phase in amps
+PhaseCurrent_s readCurrentSense(){
+  PhaseCurrent_s c;
+  // dummy example only reading analog pins
+  c.a = analogRead(A0);
+  c.b = analogRead(A1);
+  c.c = analogRead(A2); // if no 3rd current sense set it to 0
+  return(c);
+}
+
+// user defined function for intialising the current sense
+// it is optional and if provided it will be called in current_sense.init()
+void initCurrentSense(){
+  pinMode(A0,INPUT);
+  pinMode(A1,INPUT);
+  pinMode(A2,INPUT);
+}
+
+
+// GenericCurrentSense class constructor
+// it receives the user defined callback for reading the current sense
+// and optionally the user defined callback for current sense initialisation
+GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCurrentSense);
+
+
+void setup() {
+  // if callbacks are not provided in the constructor 
+  // they can be assigned directly: 
+  //current_sense.readCallback = readCurrentSense;
+  //current_sense.initCallback = initCurrentSense;
+
+  // initialise the current sensing
+  current_sense.init();
+
+
+  Serial.begin(115200);
+  Serial.println("Current sense ready.");
+}
+
+void loop() {
+
+    PhaseCurrent_s currents = current_sense.getPhaseCurrents();
+    float current_magnitude = current_sense.getDCCurrent();
+
+    Serial.print(currents.a); // milli Amps
+    Serial.print("\t");
+    Serial.print(currents.b); // milli Amps
+    Serial.print("\t");
+    Serial.print(currents.c); // milli Amps
+    Serial.print("\t");
+    Serial.println(current_magnitude); // milli Amps
+}

examples/utils/sensor_test/generic_sensor/generic_sensor.ino

@@ -0,0 +1,51 @@
+/**
+ *  Generic sensor example code 
+ * 
+ * This is a code intended to demonstrate how to implement the generic sensor class 
+ * 
+ */
+
+#include <SimpleFOC.h>
+
+// sensor reading function example
+//  for the magnetic sensor with analog communication
+// returning an angle in radians in between 0 and 2PI
+float readSensor(){
+  return analogRead(A0)*_2PI/1024.0;
+}
+
+// sensor intialising function
+void initSensor(){
+  pinMode(A0,INPUT);
+}
+
+// generic sensor class contructor
+// - read sensor callback 
+// - init sensor callback (optional)
+GenericSensor sensor = GenericSensor(readSensor, initSensor);
+
+void setup() {
+  // monitoring port
+  Serial.begin(115200);
+
+  // if callbacks are not provided in the constructor 
+  // they can be assigned directly: 
+  //sensor.readCallback = readSensor;
+  //sensor.initCallback = initSensor;
+
+  sensor.init();
+
+  Serial.println("Sensor ready");
+  _delay(1000);
+}
+
+void loop() {
+  // iterative function updating the sensor internal variables
+  // it is usually called in motor.loopFOC()
+  sensor.update();
+
+  // display the angle and the angular velocity to the terminal
+  Serial.print(sensor.getAngle());
+  Serial.print("\t");
+  Serial.println(sensor.getVelocity());
+}

keywords.txt

@@ -22,6 +22,8 @@ LowsideCurrentSense	KEYWORD1
 CurrentSense	KEYWORD1   
 Commander	KEYWORD1   
 StepDirListener	KEYWORD1   
+GenericCurrentSense	KEYWORD1   
+GenericSensor	KEYWORD1   
 
 
 initFOC	KEYWORD2
@@ -102,6 +104,8 @@ lpf	KEYWORD2
 motor	KEYWORD2
 handlePWM	KEYWORD2
 enableInterrupt	KEYWORD2
+readCallback	KEYWORD2
+initCallback	KEYWORD2
 
 
 

src/SimpleFOC.h

@@ -104,12 +104,14 @@ void loop() {
 #include "sensors/MagneticSensorAnalog.h"
 #include "sensors/MagneticSensorPWM.h"
 #include "sensors/HallSensor.h"
+#include "sensors/GenericSensor.h"
 #include "drivers/BLDCDriver3PWM.h"
 #include "drivers/BLDCDriver6PWM.h"
 #include "drivers/StepperDriver4PWM.h"
 #include "drivers/StepperDriver2PWM.h"
 #include "current_sense/InlineCurrentSense.h"
 #include "current_sense/LowsideCurrentSense.h"
+#include "current_sense/GenericCurrentSense.h"
 #include "communication/Commander.h"
 #include "communication/StepDirListener.h"
 

src/current_sense/GenericCurrentSense.cpp

@@ -0,0 +1,162 @@
+#include "GenericCurrentSense.h"
+
+// GenericCurrentSense constructor
+GenericCurrentSense::GenericCurrentSense(PhaseCurrent_s (*readCallback)(), void (*initCallback)()){
+  // if function provided add it to the 
+  if(readCallback != nullptr) this->readCallback = readCallback;
+  if(initCallback != nullptr) this->initCallback = initCallback;
+}
+
+// Inline sensor init function
+void GenericCurrentSense::init(){
+    // configure ADC variables
+    if(initCallback != nullptr) initCallback();
+    // calibrate zero offsets
+    calibrateOffsets();
+}
+// Function finding zero offsets of the ADC
+void GenericCurrentSense::calibrateOffsets(){
+    const int calibration_rounds = 1000;
+
+    // find adc offset = zero current voltage
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
+    // read the adc voltage 1000 times ( arbitrary number )
+    for (int i = 0; i < calibration_rounds; i++) {
+        PhaseCurrent_s current = readCallback();
+        offset_ia += current.a;
+        offset_ib += current.b;
+        offset_ic += current.c;
+        _delay(1);
+    }
+    // calculate the mean offsets
+    offset_ia = offset_ia / calibration_rounds;
+    offset_ib = offset_ib / calibration_rounds;
+    offset_ic = offset_ic / calibration_rounds;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){
+    PhaseCurrent_s current = readCallback();
+    current.a = (current.a - offset_ia);// amps
+    current.b = (current.a - offset_ib);// amps
+    current.c = (current.a - offset_ic); // amps
+    return current;
+}
+
+// Function synchronizing current sense with motor driver.
+// for in-line sensig no such thing is necessary
+int GenericCurrentSense::driverSync(BLDCDriver *driver){
+    return 1;
+}
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int GenericCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
+    int exit_flag = 1;
+    if(skip_align) return exit_flag;
+
+    // // set phase A active and phases B and C down
+    // driver->setPwm(voltage, 0, 0);
+    // _delay(200);
+    // PhaseCurrent_s c = getPhaseCurrents();
+    // // read the current 100 times ( arbitrary number )
+    // for (int i = 0; i < 100; i++) {
+    //     PhaseCurrent_s c1 = getPhaseCurrents();
+    //     c.a = c.a*0.6f + 0.4f*c1.a;
+    //     c.b = c.b*0.6f + 0.4f*c1.b;
+    //     c.c = c.c*0.6f + 0.4f*c1.c;
+    //     _delay(3);
+    // }
+    // driver->setPwm(0, 0, 0);
+    // // align phase A
+    // float ab_ratio = fabs(c.a / c.b);
+    // float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+    // if( ab_ratio > 1.5f ){ // should be ~2
+    //     gain_a *= _sign(c.a);
+    // }else if( ab_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and B
+    //     int tmp_pinA = pinA;
+    //     pinA = pinB;
+    //     pinB = tmp_pinA;
+    //     gain_a *= _sign(c.b);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else if(_isset(pinC) &&  ac_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and C
+    //     int tmp_pinA = pinA;
+    //     pinA = pinC;
+    //     pinC= tmp_pinA;
+    //     gain_a *= _sign(c.c);
+    //     exit_flag = 2;// signal that pins have been switched
+    // }else{
+    //     // error in current sense - phase either not measured or bad connection
+    //     return 0;
+    // }
+
+    // // set phase B active and phases A and C down
+    // driver->setPwm(0, voltage, 0);
+    // _delay(200);
+    // c = getPhaseCurrents();
+    // // read the current 50 times
+    // for (int i = 0; i < 100; i++) {
+    //     PhaseCurrent_s c1 = getPhaseCurrents();
+    //     c.a = c.a*0.6f + 0.4f*c1.a;
+    //     c.b = c.b*0.6f + 0.4f*c1.b;
+    //     c.c = c.c*0.6f + 0.4f*c1.c;
+    //     _delay(3);
+    // }
+    // driver->setPwm(0, 0, 0);
+    // float ba_ratio = fabs(c.b/c.a);
+    // float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+    //  if( ba_ratio > 1.5f ){ // should be ~2
+    //     gain_b *= _sign(c.b);
+    // }else if( ba_ratio < 0.7f ){ // it should be ~0.5
+    //     // switch phase A and B
+    //     int tmp_pinB = pinB;
+    //     pinB = pinA;
+    //     pinA = tmp_pinB;
+    //     gain_b *= _sign(c.a);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and C
+    //     int tmp_pinB = pinB;
+    //     pinB = pinC;
+    //     pinC = tmp_pinB;
+    //     gain_b *= _sign(c.c);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else{
+    //     // error in current sense - phase either not measured or bad connection
+    //     return 0;
+    // }
+
+    // // if phase C measured
+    // if(_isset(pinC)){
+    //     // set phase B active and phases A and C down
+    //     driver->setPwm(0, 0, voltage);
+    //     _delay(200);
+    //     c = getPhaseCurrents();
+    //     // read the adc voltage 500 times ( arbitrary number )
+    //     for (int i = 0; i < 50; i++) {
+    //         PhaseCurrent_s c1 = getPhaseCurrents();
+    //         c.c = (c.c+c1.c)/50.0f;
+    //     }
+    //     driver->setPwm(0, 0, 0);
+    //     gain_c *= _sign(c.c);
+    // }
+
+    // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+    // exit flag is either
+    // 0 - fail
+    // 1 - success and nothing changed
+    // 2 - success but pins reconfigured
+    // 3 - success but gains inverted
+    // 4 - success but pins reconfigured and gains inverted
+    return exit_flag;
+}

src/current_sense/GenericCurrentSense.h

@@ -0,0 +1,41 @@
+#ifndef GENERIC_CS_LIB_H
+#define GENERIC_CS_LIB_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "../common/base_classes/CurrentSense.h"
+#include "../common/lowpass_filter.h"
+#include "hardware_api.h"
+
+
+class GenericCurrentSense: public CurrentSense{
+  public:
+    /**
+      GenericCurrentSense class constructor
+    */
+    GenericCurrentSense(PhaseCurrent_s (*readCallback)() = nullptr, void (*initCallback)() = nullptr);
+
+    // CurrentSense interface implementing functions 
+    void init() override;
+    PhaseCurrent_s getPhaseCurrents() override;
+    int driverSync(BLDCDriver *driver) override;
+    int driverAlign(BLDCDriver *driver, float voltage) override;
+
+
+    PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading
+    void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation
+
+  private:
+    /**
+     *  Function finding zero offsets of the ADC
+     */
+    void calibrateOffsets();
+    float offset_ia; //!< zero current A voltage value (center of the adc reading)
+    float offset_ib; //!< zero current B voltage value (center of the adc reading)
+    float offset_ic; //!< zero current C voltage value (center of the adc reading)
+
+};
+
+#endif

src/sensors/GenericSensor.cpp

@@ -0,0 +1,26 @@
+#include "GenericSensor.h"
+
+
+/*
+  GenericSensor( float (*readCallback)() )
+  - readCallback - pointer to the function which reads the sensor angle.
+*/
+
+GenericSensor::GenericSensor(float (*readCallback)(), void (*initCallback)()){
+  // if function provided add it to the 
+  if(readCallback != nullptr) this->readCallback = readCallback;
+  if(initCallback != nullptr) this->initCallback = initCallback;
+}
+
+void GenericSensor::init(){
+  // if init callback specified run it
+  if(initCallback != nullptr) this->initCallback();
+  this->Sensor::init(); // call base class init
+}
+
+/*
+	Shaft angle calculation
+*/
+float GenericSensor::getSensorAngle(){
+  return this->readCallback();
+}