Skip to content

Commit 82178dc

Browse files
askuricaskuric
authored
Merge pull request #145 from simplefoc/FEAT_generic_sensors
Simplification of implementation for new position sensors and current senses: generic sensor classes
2 parents 94c102b + 902c5a6 commit 82178dc

File tree

9 files changed

+377
-0
lines changed

9 files changed

+377
-0
lines changed

‎README.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ Therefore this is an attempt to:
2424
<li>Added the new motion control interface to the commander- possible to set the position, velocity and torque target at once</li>
2525
<li>NRF52 series mcus support by <a href="https://github.com/Polyphe">@Polyphe</a></li>
2626
<li>Voltage/current limit handling bugs #118</li>
27+
<li>Generic position and current sense classes - to implement a new sensor only implement one function</li>
2728
</ul>
2829
</blockquote>
2930

‎examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino‎

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
/**
2+
* An example code for the generic current sensing implementation
3+
*/
4+
#include <SimpleFOC.h>
5+
6+
7+
// user defined function for reading the phase currents
8+
// returning the value per phase in amps
9+
PhaseCurrent_s readCurrentSense(){
10+
PhaseCurrent_s c;
11+
// dummy example only reading analog pins
12+
c.a = analogRead(A0);
13+
c.b = analogRead(A1);
14+
c.c = analogRead(A2); // if no 3rd current sense set it to 0
15+
return(c);
16+
}
17+
18+
// user defined function for intialising the current sense
19+
// it is optional and if provided it will be called in current_sense.init()
20+
void initCurrentSense(){
21+
pinMode(A0,INPUT);
22+
pinMode(A1,INPUT);
23+
pinMode(A2,INPUT);
24+
}
25+
26+
27+
// GenericCurrentSense class constructor
28+
// it receives the user defined callback for reading the current sense
29+
// and optionally the user defined callback for current sense initialisation
30+
GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCurrentSense);
31+
32+
33+
void setup() {
34+
// if callbacks are not provided in the constructor
35+
// they can be assigned directly:
36+
//current_sense.readCallback = readCurrentSense;
37+
//current_sense.initCallback = initCurrentSense;
38+
39+
// initialise the current sensing
40+
current_sense.init();
41+
42+
43+
Serial.begin(115200);
44+
Serial.println("Current sense ready.");
45+
}
46+
47+
void loop() {
48+
49+
PhaseCurrent_s currents = current_sense.getPhaseCurrents();
50+
float current_magnitude = current_sense.getDCCurrent();
51+
52+
Serial.print(currents.a); // milli Amps
53+
Serial.print("\t");
54+
Serial.print(currents.b); // milli Amps
55+
Serial.print("\t");
56+
Serial.print(currents.c); // milli Amps
57+
Serial.print("\t");
58+
Serial.println(current_magnitude); // milli Amps
59+
}

‎examples/utils/sensor_test/generic_sensor/generic_sensor.ino‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
/**
2+
* Generic sensor example code
3+
*
4+
* This is a code intended to demonstrate how to implement the generic sensor class
5+
*
6+
*/
7+
8+
#include <SimpleFOC.h>
9+
10+
// sensor reading function example
11+
// for the magnetic sensor with analog communication
12+
// returning an angle in radians in between 0 and 2PI
13+
float readSensor(){
14+
return analogRead(A0)*_2PI/1024.0;
15+
}
16+
17+
// sensor intialising function
18+
void initSensor(){
19+
pinMode(A0,INPUT);
20+
}
21+
22+
// generic sensor class contructor
23+
// - read sensor callback
24+
// - init sensor callback (optional)
25+
GenericSensor sensor = GenericSensor(readSensor, initSensor);
26+
27+
void setup() {
28+
// monitoring port
29+
Serial.begin(115200);
30+
31+
// if callbacks are not provided in the constructor
32+
// they can be assigned directly:
33+
//sensor.readCallback = readSensor;
34+
//sensor.initCallback = initSensor;
35+
36+
sensor.init();
37+
38+
Serial.println("Sensor ready");
39+
_delay(1000);
40+
}
41+
42+
void loop() {
43+
// iterative function updating the sensor internal variables
44+
// it is usually called in motor.loopFOC()
45+
sensor.update();
46+
47+
// display the angle and the angular velocity to the terminal
48+
Serial.print(sensor.getAngle());
49+
Serial.print("\t");
50+
Serial.println(sensor.getVelocity());
51+
}

‎keywords.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,8 @@ LowsideCurrentSense KEYWORD1
2222
CurrentSense KEYWORD1
2323
Commander KEYWORD1
2424
StepDirListener KEYWORD1
25+
GenericCurrentSense KEYWORD1
26+
GenericSensor KEYWORD1
2527

2628

2729
initFOC KEYWORD2
@@ -102,6 +104,8 @@ lpf KEYWORD2
102104
motor KEYWORD2
103105
handlePWM KEYWORD2
104106
enableInterrupt KEYWORD2
107+
readCallback KEYWORD2
108+
initCallback KEYWORD2
105109

106110

107111

‎src/SimpleFOC.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -104,12 +104,14 @@ void loop() {
104104
#include "sensors/MagneticSensorAnalog.h"
105105
#include "sensors/MagneticSensorPWM.h"
106106
#include "sensors/HallSensor.h"
107+
#include "sensors/GenericSensor.h"
107108
#include "drivers/BLDCDriver3PWM.h"
108109
#include "drivers/BLDCDriver6PWM.h"
109110
#include "drivers/StepperDriver4PWM.h"
110111
#include "drivers/StepperDriver2PWM.h"
111112
#include "current_sense/InlineCurrentSense.h"
112113
#include "current_sense/LowsideCurrentSense.h"
114+
#include "current_sense/GenericCurrentSense.h"
113115
#include "communication/Commander.h"
114116
#include "communication/StepDirListener.h"
115117

‎src/current_sense/GenericCurrentSense.cpp‎

Lines changed: 162 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,162 @@
1+
#include "GenericCurrentSense.h"
2+
3+
// GenericCurrentSense constructor
4+
GenericCurrentSense::GenericCurrentSense(PhaseCurrent_s (*readCallback)(), void (*initCallback)()){
5+
// if function provided add it to the
6+
if(readCallback != nullptr) this->readCallback = readCallback;
7+
if(initCallback != nullptr) this->initCallback = initCallback;
8+
}
9+
10+
// Inline sensor init function
11+
void GenericCurrentSense::init(){
12+
// configure ADC variables
13+
if(initCallback != nullptr) initCallback();
14+
// calibrate zero offsets
15+
calibrateOffsets();
16+
}
17+
// Function finding zero offsets of the ADC
18+
void GenericCurrentSense::calibrateOffsets(){
19+
const int calibration_rounds = 1000;
20+
21+
// find adc offset = zero current voltage
22+
offset_ia = 0;
23+
offset_ib = 0;
24+
offset_ic = 0;
25+
// read the adc voltage 1000 times ( arbitrary number )
26+
for (int i = 0; i < calibration_rounds; i++) {
27+
PhaseCurrent_s current = readCallback();
28+
offset_ia += current.a;
29+
offset_ib += current.b;
30+
offset_ic += current.c;
31+
_delay(1);
32+
}
33+
// calculate the mean offsets
34+
offset_ia = offset_ia / calibration_rounds;
35+
offset_ib = offset_ib / calibration_rounds;
36+
offset_ic = offset_ic / calibration_rounds;
37+
}
38+
39+
// read all three phase currents (if possible 2 or 3)
40+
PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){
41+
PhaseCurrent_s current = readCallback();
42+
current.a = (current.a - offset_ia);// amps
43+
current.b = (current.a - offset_ib);// amps
44+
current.c = (current.a - offset_ic); // amps
45+
return current;
46+
}
47+
48+
// Function synchronizing current sense with motor driver.
49+
// for in-line sensig no such thing is necessary
50+
int GenericCurrentSense::driverSync(BLDCDriver *driver){
51+
return 1;
52+
}
53+
54+
// Function aligning the current sense with motor driver
55+
// if all pins are connected well none of this is really necessary! - can be avoided
56+
// returns flag
57+
// 0 - fail
58+
// 1 - success and nothing changed
59+
// 2 - success but pins reconfigured
60+
// 3 - success but gains inverted
61+
// 4 - success but pins reconfigured and gains inverted
62+
int GenericCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
63+
int exit_flag = 1;
64+
if(skip_align) return exit_flag;
65+
66+
// // set phase A active and phases B and C down
67+
// driver->setPwm(voltage, 0, 0);
68+
// _delay(200);
69+
// PhaseCurrent_s c = getPhaseCurrents();
70+
// // read the current 100 times ( arbitrary number )
71+
// for (int i = 0; i < 100; i++) {
72+
// PhaseCurrent_s c1 = getPhaseCurrents();
73+
// c.a = c.a*0.6f + 0.4f*c1.a;
74+
// c.b = c.b*0.6f + 0.4f*c1.b;
75+
// c.c = c.c*0.6f + 0.4f*c1.c;
76+
// _delay(3);
77+
// }
78+
// driver->setPwm(0, 0, 0);
79+
// // align phase A
80+
// float ab_ratio = fabs(c.a / c.b);
81+
// float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
82+
// if( ab_ratio > 1.5f ){ // should be ~2
83+
// gain_a *= _sign(c.a);
84+
// }else if( ab_ratio < 0.7f ){ // should be ~0.5
85+
// // switch phase A and B
86+
// int tmp_pinA = pinA;
87+
// pinA = pinB;
88+
// pinB = tmp_pinA;
89+
// gain_a *= _sign(c.b);
90+
// exit_flag = 2; // signal that pins have been switched
91+
// }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
92+
// // switch phase A and C
93+
// int tmp_pinA = pinA;
94+
// pinA = pinC;
95+
// pinC= tmp_pinA;
96+
// gain_a *= _sign(c.c);
97+
// exit_flag = 2;// signal that pins have been switched
98+
// }else{
99+
// // error in current sense - phase either not measured or bad connection
100+
// return 0;
101+
// }
102+
103+
// // set phase B active and phases A and C down
104+
// driver->setPwm(0, voltage, 0);
105+
// _delay(200);
106+
// c = getPhaseCurrents();
107+
// // read the current 50 times
108+
// for (int i = 0; i < 100; i++) {
109+
// PhaseCurrent_s c1 = getPhaseCurrents();
110+
// c.a = c.a*0.6f + 0.4f*c1.a;
111+
// c.b = c.b*0.6f + 0.4f*c1.b;
112+
// c.c = c.c*0.6f + 0.4f*c1.c;
113+
// _delay(3);
114+
// }
115+
// driver->setPwm(0, 0, 0);
116+
// float ba_ratio = fabs(c.b/c.a);
117+
// float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
118+
// if( ba_ratio > 1.5f ){ // should be ~2
119+
// gain_b *= _sign(c.b);
120+
// }else if( ba_ratio < 0.7f ){ // it should be ~0.5
121+
// // switch phase A and B
122+
// int tmp_pinB = pinB;
123+
// pinB = pinA;
124+
// pinA = tmp_pinB;
125+
// gain_b *= _sign(c.a);
126+
// exit_flag = 2; // signal that pins have been switched
127+
// }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
128+
// // switch phase A and C
129+
// int tmp_pinB = pinB;
130+
// pinB = pinC;
131+
// pinC = tmp_pinB;
132+
// gain_b *= _sign(c.c);
133+
// exit_flag = 2; // signal that pins have been switched
134+
// }else{
135+
// // error in current sense - phase either not measured or bad connection
136+
// return 0;
137+
// }
138+
139+
// // if phase C measured
140+
// if(_isset(pinC)){
141+
// // set phase B active and phases A and C down
142+
// driver->setPwm(0, 0, voltage);
143+
// _delay(200);
144+
// c = getPhaseCurrents();
145+
// // read the adc voltage 500 times ( arbitrary number )
146+
// for (int i = 0; i < 50; i++) {
147+
// PhaseCurrent_s c1 = getPhaseCurrents();
148+
// c.c = (c.c+c1.c)/50.0f;
149+
// }
150+
// driver->setPwm(0, 0, 0);
151+
// gain_c *= _sign(c.c);
152+
// }
153+
154+
// if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
155+
// exit flag is either
156+
// 0 - fail
157+
// 1 - success and nothing changed
158+
// 2 - success but pins reconfigured
159+
// 3 - success but gains inverted
160+
// 4 - success but pins reconfigured and gains inverted
161+
return exit_flag;
162+
}

‎src/current_sense/GenericCurrentSense.h‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
#ifndef GENERIC_CS_LIB_H
2+
#define GENERIC_CS_LIB_H
3+
4+
#include "Arduino.h"
5+
#include "../common/foc_utils.h"
6+
#include "../common/time_utils.h"
7+
#include "../common/defaults.h"
8+
#include "../common/base_classes/CurrentSense.h"
9+
#include "../common/lowpass_filter.h"
10+
#include "hardware_api.h"
11+
12+
13+
class GenericCurrentSense: public CurrentSense{
14+
public:
15+
/**
16+
GenericCurrentSense class constructor
17+
*/
18+
GenericCurrentSense(PhaseCurrent_s (*readCallback)() = nullptr, void (*initCallback)() = nullptr);
19+
20+
// CurrentSense interface implementing functions
21+
void init() override;
22+
PhaseCurrent_s getPhaseCurrents() override;
23+
int driverSync(BLDCDriver *driver) override;
24+
int driverAlign(BLDCDriver *driver, float voltage) override;
25+
26+
27+
PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading
28+
void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation
29+
30+
private:
31+
/**
32+
* Function finding zero offsets of the ADC
33+
*/
34+
void calibrateOffsets();
35+
float offset_ia; //!< zero current A voltage value (center of the adc reading)
36+
float offset_ib; //!< zero current B voltage value (center of the adc reading)
37+
float offset_ic; //!< zero current C voltage value (center of the adc reading)
38+
39+
};
40+
41+
#endif

‎src/sensors/GenericSensor.cpp‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
#include "GenericSensor.h"
2+
3+
4+
/*
5+
GenericSensor( float (*readCallback)() )
6+
- readCallback - pointer to the function which reads the sensor angle.
7+
*/
8+
9+
GenericSensor::GenericSensor(float (*readCallback)(), void (*initCallback)()){
10+
// if function provided add it to the
11+
if(readCallback != nullptr) this->readCallback = readCallback;
12+
if(initCallback != nullptr) this->initCallback = initCallback;
13+
}
14+
15+
void GenericSensor::init(){
16+
// if init callback specified run it
17+
if(initCallback != nullptr) this->initCallback();
18+
this->Sensor::init(); // call base class init
19+
}
20+
21+
/*
22+
Shaft angle calculation
23+
*/
24+
float GenericSensor::getSensorAngle(){
25+
return this->readCallback();
26+
}

0 commit comments

Comments
 (0)