Solved
(Zephyr OS) L6472 Absolute position register reading
Hi,
I'm working with L6472 (custom shield) with an NUCLEO-H563ZI under zephyr OS.
I'm facing out some issues with reading the register ABS_POS. I send Goto command to the driver to move it from initial position 0 to position 3200. After the command is sent, the motor start moving to the good position but when I read ABS_POS register the driver send me the position 3072.
After multiple try, I notice that when the bit 7 (that represent 2^7 = 128) of the desired position is high the ABS_POS doesn't contains the correct value. Even if I write directly on the ABS_POS, when I read data this is not the good value.
I attach my code and a screenshot to make sure you understand what I'm doing.
/*Get innitial position position*/
int32_t AbsPos = l6472_GetAbsPos();
printf("\033[0;33mInitial position: %d (hex = 0x%X)\033[0;37m\r\n", AbsPos, AbsPos);
/*First move going to ABS_POS = 3200 steps from ABS_Pos = 0 steps */
posSend = 3200;
l6472_GoTo(posSend);
while (end != true);/* wait for move to be completed*/
/* Check absolute position after first move*/
AbsPos = l6472_GetAbsPos();
printf("\033[0;32mABS_POS reached = %d (hex = 0x%X)\033[0;37m\r\n", AbsPos, AbsPos);
/*Second move going to ABS_POS = 0 steps from ABS_Pos = 3200 steps */
end = false;
posSend = 0;
l6472_GoTo(posSend);
while (end != true); /* wait for move to be completed*/
/* Check absolute position after second move*/
AbsPos = l6472_GetAbsPos();
printf("\033[0;33mPosition reached after second move: %d (hex = 0x%X)\033[0;37m\r\n", AbsPos, AbsPos);
/*Last move going to ABS_POS = -3200 steps from ABS_Pos = 0 steps */
end = false;
posSend = -3200;
l6472_GoTo(posSend);
while (end != true); /* wait for move to be completed*/
/* Check absolute position after last move*/
AbsPos = l6472_GetAbsPos();
printf("\033[0;33mPosition reached after last move: %d (hex = 0x%X)\033[0;37m\r\n", AbsPos, AbsPos);
/* Check writing ABS_POS register*/
uint8_t data[3] = {0};
posSend = 3200;
data[0] = (posSend >> 16) & 0xFF;
data[1] = (posSend >> & 0xFF;
data[2] = posSend & 0xFF;
printf("Write directly 3200 steps into ABS_POS register\r\n");
l6472_SetParam(L6472_REG_ABS_POS,data,L6472_REG_SIZE_ABS_POS);
l6472_GetAbsPos();
void l6472_GoTo(int32_t abs_pos)
{
uint8_t cmd = L6472_CMD_GO_TO;
uint8_t abs_pos_bytes[L6472_REG_SIZE_ABS_POS];
uint32_t pos;
// Vérifier la plage de valeurs valides
if (abs_pos <= -2097152 || abs_pos >= 2097151)
{
printf("Position en dehors de la plage valide !\r\n");
return;
}
if (abs_pos < 0)
{
pos = (uint32_t)((abs_pos + (1 << 22)) & 0x3FFFFF);
printf("\033[0;31mPosition sent int32_t: %d (Hex : 0x%X)\r\nPosition sent uint32_t: %d (Hex : 0x%X)\033[0;37m\r\n", abs_pos, abs_pos, pos, pos);
}
else
{
pos = (uint32_t)(abs_pos & 0x3FFFFF);
printf("\033[0;31mPosition sent int32_t: %d (Hex : 0x%X)\r\nPosition sent uint32_t: %d (Hex : 0x%X)\033[0;37m\r\n", abs_pos, abs_pos, pos, pos);
}
// Store position in 3 Byte (MSB → LSB)
abs_pos_bytes[0] = (pos >> 16) & 0xFF;
abs_pos_bytes[1] = (pos >> & 0xFF;
abs_pos_bytes[2] = pos & 0xFF;
printf("\033[0;31mBuffer sent to L6472 : 0x%X 0x%X 0x%X \033[0;37m\r\n", abs_pos_bytes[0], abs_pos_bytes[1], abs_pos_bytes[2]);
struct spi_buf tx_buf[1] = {
{.buf = &cmd, .len = 1},
};
struct spi_buf_set tx_set = {.buffers = tx_buf, .count = 1};
// Send GoTo
spi_transceive(spi4, &config, &tx_set, NULL);
// Send positionn
for (uint8_t i = 0; i < L6472_REG_SIZE_ABS_POS; i++)
{
tx_buf[0].buf = &abs_pos_bytes[i];
spi_transceive(spi4, &config, &tx_set, NULL); /* 3 send because CS need to be toggled for each byte*/
}
}
int32_t l6472_GetAbsPos(void)
{
uint8_t absPos[L6472_REG_SIZE_ABS_POS] = {0};
uint32_t uPos = 0;
int32_t operation_result;
l6472_GetParam(L6472_REG_ABS_POS, absPos, L6472_REG_SIZE_ABS_POS); /* Read ABS_REG*/
uPos = (absPos[0] << 16) | (absPos[1] << | (absPos[2]);
if (uPos & L6474_ABS_POS_SIGN_BIT_MASK)
{
uPos = ~uPos + (1 << 22);
operation_result = (int32_t)(uPos & L6474_ABS_POS_VALUE_MASK);
operation_result = -operation_result;
}
else
{
operation_result = (int32_t)(uPos & L6474_ABS_POS_VALUE_MASK);
}
printf("\033[0;34mRead L6472 ABS_POS register = 0x%X 0x%X 0x%X\r\nAbsolute Pos converted = %d (hex = 0x%X)\033[0;37m \r\n", absPos[0], absPos[1], absPos[2], operation_result, operation_result);
return operation_result;
}