You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
110 lines
2.9 KiB
110 lines
2.9 KiB
11 months ago
|
/*
|
||
|
|
* msp_Angle_Encoder_YL159.c
|
||
|
|
*
|
||
|
|
* Created on: Jul 24, 2024
|
||
|
|
* Author: akeguo
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "msp_Angle_Encoder_YL159.h"
|
||
|
|
#include "BHBF_ROBOT.h"
|
||
|
|
#include "bsp_Log.h"
|
||
|
|
|
||
|
|
int32_t* Desulfurizer_Angle;
|
||
|
|
|
||
|
|
//int32_t* Desulfurizer_Thickness;
|
||
|
|
MB_REG_DATA mb_reg_value;
|
||
|
|
struct UARTHandler *encoder_UART_Handler;
|
||
|
|
DispacherController *encoder_dispacherController;
|
||
|
|
void angle_encoder_intialize(struct UARTHandler *Handler)
|
||
|
|
{
|
||
|
|
|
||
|
|
encoder_UART_Handler = Handler;
|
||
|
|
encoder_UART_Handler->Wait_time = 10; //等待10ms 最低不要低于4；
|
||
|
|
LOG("angle_encoder_intialize");
|
||
|
|
encoder_dispacherController=Handler->dispacherController;
|
||
|
|
encoder_dispacherController->DispacherCallTime=100;
|
||
|
|
//encoder_UART_Handler->DispacherCallPeriod=100;
|
||
|
|
//log_info("angle_encoder_intialize");
|
||
|
|
encoder_dispacherController->Add_Dispatcher_List(
|
||
|
|
encoder_dispacherController,read_angle);
|
||
|
|
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"angle_encoder",0,AngleEncoderErrorBitFlag);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
//set current position home/ aka 0
|
||
|
|
void reset_current_value_0()
|
||
|
|
{
|
||
|
|
|
||
|
|
|
||
|
|
LOG("Set Angle Encoder position 0");
|
||
|
|
|
||
|
|
encoder_UART_Handler->UART_Decode = NULL;
|
||
|
|
//RS_485_2_UART_Handler.UART_Decode=;
|
||
|
|
mb_reg_value.DATA_10H[0] = 0;
|
||
|
|
mb_reg_value.DATA_10H[1] = 0;
|
||
|
|
//set 16-17 为0
|
||
|
|
MB_WriteNumHoldingReg((encoder_UART_Handler->Tx_Buf),
|
||
|
|
&(encoder_UART_Handler->TxCount), 1, 16, 2, mb_reg_value.DATA_10H); //清零
|
||
|
|
encoder_UART_Handler->UART_Tx(encoder_UART_Handler);
|
||
|
|
}
|
||
|
|
void reset_position_value()
|
||
|
|
{
|
||
|
|
|
||
|
|
|
||
|
|
LOG("Set Angle Encoder position 0");
|
||
|
|
|
||
|
|
//set current decode function NULL, Indicate that there is no need to decode the received data;
|
||
|
|
//RS_485_2_UART_Handler.UART_Decode = NULL;
|
||
|
|
encoder_UART_Handler->UART_Decode = NULL;
|
||
|
|
MB_WriteHoldingReg(encoder_UART_Handler->Tx_Buf,
|
||
|
|
&encoder_UART_Handler->TxCount, 1, 67, 10); //read from the data;
|
||
|
|
encoder_UART_Handler->UART_Tx(encoder_UART_Handler);
|
||
|
|
}
|
||
|
|
|
||
|
|
void read_angle()
|
||
|
|
{
|
||
|
|
|
||
|
|
LOG("call data from Angle Encoder from 0x08 mqtt 444 update 00");
|
||
|
|
|
||
|
|
|
||
|
|
//RS_485_2_UART_Handler.UART_Decode = decode_Angle;
|
||
|
|
encoder_UART_Handler->UART_Decode = decode_Angle;
|
||
|
|
MB_ReadHoldingReg(&encoder_UART_Handler->Tx_Buf,
|
||
|
|
&encoder_UART_Handler->TxCount, 1, 16, 2); //read from the data;
|
||
|
|
if(encoder_UART_Handler->uart!=NULL)
|
||
|
|
{
|
||
|
|
encoder_UART_Handler->UART_Tx(encoder_UART_Handler);
|
||
|
|
}else
|
||
|
|
{
|
||
|
|
LOGFF(DL_ERROR,"encoder_UART_Handler did not intialized");
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
static int32_t angle;
|
||
|
|
void decode_Angle(uint8_t *buffer, uint16_t length)
|
||
|
|
{
|
||
|
|
/* CRC 校验 */
|
||
|
|
uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]);
|
||
|
|
/* CRC 校验正确 */
|
||
|
|
if (crc_check == MB_CRC16(buffer, length - 2))
|
||
|
|
{
|
||
|
|
angle = buffer[5] << 24 | buffer[6] << 16 | buffer[3] << 8 | buffer[4];
|
||
|
|
*Desulfurizer_Angle = angle;
|
||
|
|
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"angle_encoder",1);
|
||
|
|
|
||
|
|
//Decoded successfully
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
//LOGFF(DL_ERROR,"angle encoder decoding failed");
|
||
|
|
LOGFF(DL_ERROR,"angle encoder decoding failed");
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|