Skip to content

Commit 329ab19

Browse files
SchnilzSchnilz
committed
add: ans_inc_combine sensor
1 parent 5dc0563 commit 329ab19

File tree

3 files changed

+377
-0
lines changed

3 files changed

+377
-0
lines changed

‎src/encoders/abs_inc_combine/AbsIncCombineSensor.cpp

Lines changed: 307 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,307 @@
1+
#include "CalibratedSensor.h"
2+
3+
// CalibratedSensor()
4+
// sensor - instance of original sensor object
5+
// n_lut - number of samples in the LUT
6+
CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut, float *lut)
7+
: _wrapped(wrapped), n_lut(n_lut), allocated(false), calibrationLut(lut) {
8+
};
9+
10+
CalibratedSensor::~CalibratedSensor() {
11+
// delete calibrationLut;
12+
if(allocated) {
13+
delete []calibrationLut;
14+
}
15+
};
16+
17+
// call update of calibrated sensor
18+
void CalibratedSensor::update()
19+
{
20+
_wrapped.update();
21+
this->Sensor::update();
22+
};
23+
24+
// Retrieve the calibrated sensor angle
25+
void CalibratedSensor::init()
26+
{
27+
// assume wrapped sensor has already been initialized
28+
this->Sensor::init(); // call superclass init
29+
}
30+
31+
// Retrieve the calibrated sensor angle
32+
float CalibratedSensor::getSensorAngle()
33+
{
34+
if(!calibrationLut) {
35+
return _wrapped.getMechanicalAngle();
36+
}
37+
// raw encoder position e.g. 0-2PI
38+
float raw_angle = fmodf(_wrapped.getMechanicalAngle(), _2PI);
39+
raw_angle += raw_angle < 0 ? _2PI:0;
40+
41+
// Calculate the resolution of the LUT in radians
42+
float lut_resolution = _2PI / n_lut;
43+
// Calculate LUT index
44+
int lut_index = raw_angle / lut_resolution;
45+
46+
// Get calibration values from the LUT
47+
float y0 = calibrationLut[lut_index];
48+
float y1 = calibrationLut[(lut_index + 1) % n_lut];
49+
50+
// Linearly interpolate between the y0 and y1 values
51+
// Calculate the relative distance from the y0 (raw_angle has to be between y0 and y1)
52+
// If distance = 0, interpolated offset = y0
53+
// If distance = 1, interpolated offset = y1
54+
float distance = (raw_angle - lut_index * lut_resolution) / lut_resolution;
55+
float offset = (1 - distance) * y0 + distance * y1;
56+
57+
// Calculate the calibrated angle
58+
return raw_angle - offset;
59+
}
60+
61+
// Perform filtering to linearize position sensor eccentricity
62+
// FIR n-sample average, where n = number of samples in the window
63+
// This filter has zero gain at electrical frequency and all integer multiples
64+
// So cogging effects should be completely filtered out
65+
void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks, int window){
66+
float window_buffer[window];
67+
memset(window_buffer, 0, window*sizeof(float));
68+
float window_sum = 0;
69+
int buffer_index = 0;
70+
// fill the inital window buffer
71+
for (int i = 0; i < window; i++) {
72+
int ind = n_ticks - window/2 -1 + i;
73+
window_buffer[i] = error[ind % n_ticks];
74+
window_sum += window_buffer[i];
75+
}
76+
// calculate the moving average
77+
error_mean = 0;
78+
for (int i = 0; i < n_ticks; i++)
79+
{
80+
// Update buffer
81+
window_sum -= window_buffer[buffer_index];
82+
window_buffer[buffer_index] = error[( i + window/2 ) %n_ticks];
83+
window_sum += window_buffer[buffer_index];
84+
// update the buffer index
85+
buffer_index = (buffer_index + 1) % window;
86+
87+
// Update filtered error
88+
error[i] = window_sum / (float)window;
89+
// update the mean value
90+
error_mean += error[i] / n_ticks;
91+
}
92+
93+
}
94+
95+
void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms)
96+
{
97+
// if the LUT is already defined, skip the calibration
98+
99+
if(calibrationLut == NULL) {
100+
allocated = true;
101+
calibrationLut = new float[n_lut];
102+
}
103+
motor.monitor_port->println("Starting Sensor Calibration.");
104+
105+
// Calibration variables
106+
107+
// Init inital angles
108+
float theta_actual = 0.0;
109+
float avg_elec_angle = 0.0;
110+
// set the inital electric angle to 0
111+
float elec_angle = 0.0;
112+
113+
// Calibration parameters
114+
// The motor will take a n_pos samples per electrical cycle
115+
// which amounts to n_ticks (n_pos * motor.pole_pairs) samples per mechanical rotation
116+
// Additionally, the motor will take n2_ticks steps to reach any of the n_ticks posiitons
117+
// incrementing the electrical angle by deltaElectricalAngle each time
118+
int n_pos = 5;
119+
int _NPP = motor.pole_pairs; // number of pole pairs which is user input
120+
const int n_ticks = n_pos * _NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later)
121+
const int n2_ticks = 5; // increments between saved samples (for smoothing motion)
122+
float deltaElectricalAngle = _2PI * _NPP / (n_ticks * n2_ticks); // Electrical Angle increments for calibration steps
123+
float error[n_ticks]; // pointer to error array (average of forward & backward)
124+
memset(error, 0, n_ticks*sizeof(float));
125+
// The fileter window size is set to n_pos - one electrical cycle
126+
// important for cogging filtering !!!
127+
const int window = n_pos; // window size for moving average filter of raw error
128+
129+
130+
// find the first guess of the motor.zero_electric_angle
131+
// and the sensor direction
132+
// updates motor.zero_electric_angle
133+
// updates motor.sensor_direction
134+
// temporarily unlink the sensor and current sense
135+
CurrentSense *current_sense = motor.current_sense;
136+
motor.current_sense = nullptr;
137+
motor.linkSensor(&this->_wrapped);
138+
if(!motor.initFOC()){
139+
motor.monitor_port->println("Failed to align the sensor.");
140+
return;
141+
}
142+
// link back the sensor and current sense
143+
motor.linkSensor(this);
144+
motor.linkCurrentSense(current_sense);
145+
146+
// Set voltage angle to zero, wait for rotor position to settle
147+
// keep the motor in position while getting the initial positions
148+
motor.setPhaseVoltage(1, 0, elec_angle);
149+
_delay(1000);
150+
_wrapped.update();
151+
float theta_init = _wrapped.getAngle();
152+
float theta_absolute_init = _wrapped.getMechanicalAngle();
153+
154+
/*
155+
Start Calibration
156+
Loop over electrical angles from 0 to NPP*2PI, once forward, once backward
157+
store actual position and error as compared to electrical angle
158+
*/
159+
160+
/*
161+
forwards rotation
162+
*/
163+
motor.monitor_port->print("Rotating: ");
164+
motor.monitor_port->println( motor.sensor_direction == Direction::CCW ? "CCW" : "CW" );
165+
float zero_angle_prev = 0.0;
166+
for (int i = 0; i < n_ticks; i++)
167+
{
168+
for (int j = 0; j < n2_ticks; j++) // move to the next location
169+
{
170+
_wrapped.update();
171+
elec_angle += deltaElectricalAngle;
172+
motor.setPhaseVoltage(voltage_calibration, 0, elec_angle);
173+
}
174+
175+
// delay to settle in position before taking a position sample
176+
_delay(settle_time_ms);
177+
_wrapped.update();
178+
// calculate the error
179+
theta_actual = (int)motor.sensor_direction * (_wrapped.getAngle() - theta_init);
180+
error[i] = 0.5 * (theta_actual - elec_angle / _NPP);
181+
182+
// calculate the current electrical zero angle
183+
float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2);
184+
zero_angle = _normalizeAngle(zero_angle);
185+
// remove the 2PI jumps
186+
if(zero_angle - zero_angle_prev > _PI){
187+
zero_angle = zero_angle - _2PI;
188+
}else if(zero_angle - zero_angle_prev < -_PI){
189+
zero_angle = zero_angle + _2PI;
190+
}
191+
zero_angle_prev = zero_angle;
192+
avg_elec_angle += zero_angle/n_ticks;
193+
194+
// motor.monitor_port->print(">zero:");
195+
// motor.monitor_port->println(zero_angle);
196+
// motor.monitor_port->print(">zero_average:");
197+
// motor.monitor_port->println(avg_elec_angle/2.0);
198+
}
199+
_delay(2000);
200+
201+
/*
202+
backwards rotation
203+
*/
204+
motor.monitor_port->print("Rotating: ");
205+
motor.monitor_port->println( motor.sensor_direction == Direction::CCW ? "CW" : "CCW" );
206+
for (int i = n_ticks - 1; i >= 0; i--)
207+
{
208+
for (int j = 0; j < n2_ticks; j++) // move to the next location
209+
{
210+
_wrapped.update();
211+
elec_angle -= deltaElectricalAngle;
212+
motor.setPhaseVoltage(voltage_calibration, 0, elec_angle);
213+
}
214+
215+
// delay to settle in position before taking a position sample
216+
_delay(settle_time_ms);
217+
_wrapped.update();
218+
// calculate the error
219+
theta_actual = (int)motor.sensor_direction * (_wrapped.getAngle() - theta_init);
220+
error[i] += 0.5 * (theta_actual - elec_angle / _NPP);
221+
// calculate the current electrical zero angle
222+
float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2);
223+
zero_angle = _normalizeAngle(zero_angle);
224+
// remove the 2PI jumps
225+
if(zero_angle - zero_angle_prev > _PI){
226+
zero_angle = zero_angle - _2PI;
227+
}else if(zero_angle - zero_angle_prev < -_PI){
228+
zero_angle = zero_angle + _2PI;
229+
}
230+
zero_angle_prev = zero_angle;
231+
avg_elec_angle += zero_angle/n_ticks;
232+
233+
// motor.monitor_port->print(">zero:");
234+
// motor.monitor_port->println(zero_angle);
235+
// motor.monitor_port->print(">zero_average:");
236+
// motor.monitor_port->println(avg_elec_angle/2.0);
237+
}
238+
239+
// get post calibration mechanical angle.
240+
_wrapped.update();
241+
float theta_absolute_post = _wrapped.getMechanicalAngle();
242+
243+
// done with the measurement
244+
motor.setPhaseVoltage(0, 0, 0);
245+
246+
// raw offset from initial position in absolute radians between 0-2PI
247+
float raw_offset = (theta_absolute_init + theta_absolute_post) / 2;
248+
249+
// calculating the average zero electrical angle from the forward calibration.
250+
motor.zero_electric_angle = _normalizeAngle(avg_elec_angle / (2.0));
251+
motor.monitor_port->print("Average Zero Electrical Angle: ");
252+
motor.monitor_port->println(motor.zero_electric_angle);
253+
_delay(1000);
254+
255+
// Perform filtering to linearize position sensor eccentricity
256+
// FIR n-sample average, where n = number of samples in one electrical cycle
257+
// This filter has zero gain at electrical frequency and all integer multiples
258+
// So cogging effects should be completely filtered out
259+
float error_mean = 0.0;
260+
this->filter_error(error, error_mean, n_ticks, window);
261+
262+
_delay(1000);
263+
// calculate offset index
264+
int index_offset = floor((float)n_lut * raw_offset / _2PI);
265+
float dn = n_ticks / (float)n_lut;
266+
267+
motor.monitor_port->println("Constructing LUT.");
268+
_delay(1000);
269+
// Build Look Up Table
270+
for (int i = 0; i < n_lut; i++)
271+
{
272+
int ind = index_offset + i*motor.sensor_direction;
273+
if (ind > (n_lut - 1)) ind -= n_lut;
274+
if (ind < 0) ind += n_lut;
275+
calibrationLut[ind] = (float)(error[(int)(i * dn)] - error_mean);
276+
// negate the error if the sensor is in the opposite direction
277+
calibrationLut[ind] = (int)motor.sensor_direction * calibrationLut[ind];
278+
}
279+
motor.monitor_port->println("");
280+
_delay(1000);
281+
282+
// Display the LUT
283+
motor.monitor_port->print("float calibrationLut[");
284+
motor.monitor_port->print(n_lut);
285+
motor.monitor_port->println("] = {");
286+
_delay(100);
287+
for (int i=0;i < n_lut; i++){
288+
motor.monitor_port->print(calibrationLut[i],6);
289+
if(i < n_lut - 1) motor.monitor_port->print(", ");
290+
_delay(1);
291+
}
292+
motor.monitor_port->println("};");
293+
_delay(1000);
294+
295+
// Display the zero electrical angle
296+
motor.monitor_port->print("float zero_electric_angle = ");
297+
motor.monitor_port->print(motor.zero_electric_angle,6);
298+
motor.monitor_port->println(";");
299+
300+
// Display the sensor direction
301+
motor.monitor_port->print("Direction sensor_direction = ");
302+
motor.monitor_port->println(motor.sensor_direction == Direction::CCW ? "Direction::CCW;" : "Direction::CW;");
303+
_delay(1000);
304+
305+
motor.monitor_port->println("Sensor Calibration Done.");
306+
}
307+

‎src/encoders/abs_inc_combine/AbsIncCombineSensor.h

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
#ifndef __ABSINCCOMBINESENSOR_H__
2+
#define __ABSINCCOMBINESENSOR_H__
3+
4+
#include "common/base_classes/Sensor.h"
5+
#include "BLDCMotor.h"
6+
#include "common/base_classes/FOCMotor.h"
7+
#include "common/foc_utils.h"
8+
9+
10+
class AbsIncCombineSensor: public Sensor{
11+
12+
public:
13+
/**
14+
* @brief Constructor of class with pointer to base class sensor and driver
15+
* @param wrapped the wrapped sensor which needs calibration
16+
* @param n_lut the number of entries in the lut
17+
* @param lut the look up table (if null, the lut will be allocated on the heap)
18+
*/
19+
AbsIncCombineSensor(Sensor& wrapped, int n_lut = 200, float* lut = nullptr);
20+
21+
~AbsIncCombineSensor();
22+
23+
/*
24+
Override the update function
25+
*/
26+
virtual void update() override;
27+
28+
/**
29+
* Calibrate method computes the LUT for the correction
30+
*/
31+
virtual void calibrate(FOCMotor& motor, int settle_time_ms = 30);
32+
33+
// voltage to run the calibration: user input
34+
float voltage_calibration = 1;
35+
protected:
36+
37+
/**
38+
* getSenorAngle() method of AbsIncCombineSensor class.
39+
* This should call getAngle() on the wrapped instance, and then apply the correction to
40+
* the value returned.
41+
*/
42+
virtual float getSensorAngle() override;
43+
/**
44+
* init method of CaibratedSensor - call after calibration
45+
*/
46+
virtual void init() override;
47+
/**
48+
* delegate instance of Sensor class
49+
*/
50+
Sensor& _wrapped;
51+
52+
void alignSensor(FOCMotor &motor);
53+
void filter_error(float* error, float &error_mean, int n_ticks, int window);
54+
55+
// lut size - settable by the user
56+
int n_lut { 200 } ;
57+
// pointer for lut memory
58+
// depending on the size of the lut
59+
// will be allocated in the calibrate function if not given.
60+
bool allocated;
61+
float* calibrationLut;
62+
};
63+
64+
#endif /* __ABSINCCOMBINESENSOR_H__ */

‎src/encoders/abs_inc_combine/README.md

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
# abs_inc_combine sensor
2+
3+
This sensor wrapper is able to combine an incremental sensor (with Quadrature / AB interface) and an absolut Sensor (e.g. a MagneticSensor).
4+
This is useful if you have an incremental encoder for its good FOC performance and an absolute encoder after a gear box to skip initialization / calibration.
5+
6+
The absolute encoder is only read on init (and optional validity checks) after which the incremental sensor is used.

0 commit comments

Comments
 (0)