Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-25 02:47:27 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Add Encoder CPR range checking

Browse Source
This commit is contained in:
Paul Guenette
2018-02-19 19:42:47 -05:00
parent ed12f357c4
commit 06ab4d7bae
2 changed files with 16 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/low_level.c
+14 -3
View File
@@ -129,6 +129,7 @@ Motor_t motors[] = {
.encoder_offset = 0,
.encoder_state = 0,
.motor_dir = 1, // 1 or -1
.encoder_calib_range = 0.02,
.phase = 0.0f, // [rad]
.pll_pos = 0.0f, // [rad]
.pll_vel = 0.0f, // [rad/s]
@@ -231,6 +232,7 @@ Motor_t motors[] = {
.encoder_offset = 0,
.encoder_state = 0,
.motor_dir = 1, // 1 or -1
.encoder_calib_range = 0.02,
.phase = 0.0f, // [rad]
.pll_pos = 0.0f, // [rad]
.pll_vel = 0.0f, // [rad/s]
@@ -765,9 +767,6 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) {
static const float scan_range = 4.0f * M_PI;
const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?)
int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT;
int32_t encvaluesum = 0;
// go to motor zero phase for start_lock_duration to get ready to scan
for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) {
if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) {
@@ -776,6 +775,10 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) {
}
queue_voltage_timings(motor, voltage_magnitude, 0.0f);
}
int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT;
int32_t encvaluesum = 0;
// scan forwards
for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) {
for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) {
@@ -789,6 +792,14 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) {
}
encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT;
}
float estEnc = scan_range / elec_rad_per_enc;
float adjustedCount = fabsf((int16_t)motor->encoder.encoder_timer->Instance->CNT)-init_enc_val;
if(fabsf(adjustedCount-estEnc)/estEnc > motor->encoder.encoder_calib_range)
{
motor->error = ERROR_ENCODER_CPR_OUT_OF_RANGE;
return false;
}
// check direction
if ((int16_t)motor->encoder.encoder_timer->Instance->CNT > init_enc_val + 8) {
// motor same dir as encoder
Firmware/MotorControl/low_level.h
+2
View File
@@ -50,6 +50,7 @@ typedef enum {
ERROR_SPIN_UP_TIMEOUT,
ERROR_DRV_FAULT,
ERROR_NOT_IMPLEMENTED_MOTOR_TYPE,
ERROR_ENCODER_CPR_OUT_OF_RANGE
} Error_t;
// Note: these should be sorted from lowest level of control to
@@ -119,6 +120,7 @@ typedef struct {
int32_t encoder_offset;
int32_t encoder_state;
int32_t motor_dir; // 1/-1 for fwd/rev alignment to encoder.
float encoder_calib_range;
float phase;
float pll_pos;
float pll_vel;