Skip to content

CalibratedSensor: settle time as parameter #56

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: dev
Choose a base branch
from
Open

CalibratedSensor: settle time as parameter #56

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 8 additions & 7 deletions src/encoders/calibrated/CalibratedSensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,10 +28,11 @@ void CalibratedSensor::init()
// Retrieve the calibrated sensor angle
float CalibratedSensor::getSensorAngle()
{
// raw encoder position e.g. 0-2PI
float raw_angle = _wrapped.getMechanicalAngle();
// raw encoder position e.g. 0-2PI
float raw_angle = fmodf(_wrapped.getMechanicalAngle(), _2PI);
raw_angle += raw_angle < 0 ? _2PI:0;

// Calculate the resolution of the LUT in radians
// Calculate the resolution of the LUT in radians
float lut_resolution = _2PI / n_lut;
// Calculate LUT index
int lut_index = raw_angle / lut_resolution;
Expand All @@ -41,7 +42,7 @@ float CalibratedSensor::getSensorAngle()
float y1 = calibrationLut[(lut_index + 1) % n_lut];

// Linearly interpolate between the y0 and y1 values
// Calculate the relative distance from the y0 (raw_angle has to be between y0 and y1)
// Calculate the relative distance from the y0 (raw_angle has to be between y0 and y1)
// If distance = 0, interpolated offset = y0
// If distance = 1, interpolated offset = y1
float distance = (raw_angle - lut_index * lut_resolution) / lut_resolution;
Expand Down Expand Up @@ -85,7 +86,7 @@ void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks

}

void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electric_angle, Direction senor_direction)
void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electric_angle, Direction senor_direction, int settle_time_ms)
{
// if the LUT is already defined, skip the calibration
if(lut != nullptr){
Expand Down Expand Up @@ -170,7 +171,7 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri
}

// delay to settle in position before taking a position sample
_delay(30);
_delay(settle_time_ms);
_wrapped.update();
// calculate the error
theta_actual = motor.sensor_direction*(_wrapped.getAngle() - theta_init);
Expand Down Expand Up @@ -210,7 +211,7 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri
}

// delay to settle in position before taking a position sample
_delay(30);
_delay(settle_time_ms);
_wrapped.update();
// calculate the error
theta_actual = motor.sensor_direction*(_wrapped.getAngle() - theta_init);
Expand Down
4 changes: 2 additions & 2 deletions src/encoders/calibrated/CalibratedSensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ class CalibratedSensor: public Sensor{
/**
* Calibrate method computes the LUT for the correction
*/
virtual void calibrate(FOCMotor& motor, float* lut = nullptr, float zero_electric_angle = 0.0, Direction senor_direction= Direction::CW);
virtual void calibrate(FOCMotor& motor, float* lut = nullptr, float zero_electric_angle = 0.0, Direction senor_direction= Direction::CW, int settle_time_ms = 30);

// voltage to run the calibration: user input
float voltage_calibration = 1;
Expand Down Expand Up @@ -55,4 +55,4 @@ class CalibratedSensor: public Sensor{
float* calibrationLut;
};

#endif
#endif