(Zephyr OS) L6472 Absolute position register reading

[Xavier-Diasys]

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();

View more

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;
}

View more

[Xavier-Diasys]

After trying different SPI configurations, I notice that when the default occured I have the L6472 trying to respond on the command byte 0x21 (corresponding to the read of the register ABS_POS), but it doesn't make sense with the datasheet.

 

 

[Xavier-Diasys · Accepted solution]

I solved the problem, by just changing the GPIO mode of pins (MISO,MOSI,CS) to low speed.