Skip to content
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

AMCBLDC: Fix computation of angle when using quadrature encoder #332

Merged
merged 3 commits into from
Jan 13, 2023
Merged

AMCBLDC: Fix computation of angle when using quadrature encoder #332

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8dd05bb
fix computation of hall sector in amcbldc HAL
mfussi66 Jan 11, 2023
fc00d95
AMCBLDC hal: add encoder reset to avoid desynch
mfussi66 Jan 11, 2023
b64656a
amcbldc hal: add meaning to values used by calibration of angle
mfussi66 Jan 12, 2023
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
5 changes: 4 additions & 1 deletion emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,9 @@ uint32_t encoderGetCounter(void)
return (uint32_t)__HAL_TIM_GET_COUNTER(&htim2);
}


/*******************************************************************************************************************//**
* @brief Reset encoder value
*/
void encoderReset()
{
__HAL_TIM_SET_COUNTER(&htim2, 0);
Expand All @@ -213,6 +215,7 @@ void encoderReset()
*/
uint16_t encoderGetElectricalAngle(void)
{

if (MainConf.encoder.resolution == 0)
{
return encoderForcedValue;
Expand Down
47 changes: 31 additions & 16 deletions emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/pwm.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -223,7 +223,8 @@ static uint8_t updateHallStatus(void)
uint16_t angle = MainConf.pwm.hall_offset + hallAngleTable[hallStatus];

//int16_t sector = (MainConf.pwm.sector_offset + hallSectorTable[hallStatus]) % 6;
int16_t sector = ((1+(int16_t)(angle)) / 60) % 6;
// Check which sector [0 ... 5] (30 + angle) / 60
int16_t sector = ((5461 + angle) / 10922) % 6;
static int16_t sector_old = sector;

hallAngle = angle;
Expand All @@ -242,6 +243,7 @@ static uint8_t updateHallStatus(void)
}
else
{

bool forward = ((sector-sector_old+6)%6)==1;

if (forward) // forward
Expand All @@ -258,43 +260,56 @@ static uint8_t updateHallStatus(void)
if (calibration_step == 0)
{
encoderForce(angle);
calibration_step = 1;

if ((sector == 0 && sector_old == 5) || (sector == 5 && sector_old == 0))
{
encoderReset();
calibration_step = 1;
}
}
else if (calibration_step == 1)
{
encoderForce(angle);

// use the current sector if forward rotation or previous if reverse
uint8_t s = forward ? sector : (sector+1)%6;

// keep track of the encoder value between sectors
border[s] = encoderGetUncalibrated();


// found the s-th border, put a 1 in the mask
border_flag |= 1<<s;

if (border_flag == 63)
// After finding all borders compute offset through minimization of MSE
if (border_flag == 63) // 111111
{
calibration_step = 2;

int32_t offset = int16_t(border[0]);
offset += int16_t(border[1]-10923);
offset += int16_t(border[2]-21845);
offset += int16_t(border[3]-32768);
offset += int16_t(border[4]-43691);
offset += int16_t(border[5]-54613);
int32_t offset = int16_t(MainConf.pwm.hall_offset-5461-border[0]);
offset += int16_t(MainConf.pwm.hall_offset-5461+10923 -border[1]);
offset += int16_t(MainConf.pwm.hall_offset-5461+21845 -border[2]);
offset += int16_t(MainConf.pwm.hall_offset-5461+32768 -border[3]);
offset += int16_t(MainConf.pwm.hall_offset-5461+43691 -border[4]);
offset += int16_t(MainConf.pwm.hall_offset-5461+54613 -border[5]);

offset /= 6;

embot::core::print("CALIBRATED\n");

encoderCalibrate(-int16_t(offset));
}
}
//else if (calibration_step == 2)
//{
// encoderCalibrate(angle);
//}
else if (calibration_step == 2)
{
encoderForce(angle);
// reset the angle after full rotation
if ((sector == 0 && sector_old == 5) || (sector == 5 && sector_old == 0))
{
encoderReset();
}
}
}
}

// update the old sector and hall status
sector_old = sector;
hallStatus_old = hallStatus;
Expand Down