/******************************************************************************

                  版权所有 (C), 2018-2099, Radkil

 ******************************************************************************
  文 件 名   : msp_TI5MOTOR.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年6月8日
  最近修改   :
  功能描述   : TI5电机
  
  修改历史   :
  1.日    期   : 2026年6月8日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "msp_TI5MOTOR.h"
#include "BHBF_robot.h"

/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
static MotorParameters Motor[4];
static int32_t Motor_ID_Errors[7] = { 0 };
static CSP tempCSP;

/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/

#define DF_MSP_Ti5Motor_StartID 0 

static int CANSendCommand(uint8_t id, uint8_t command, int32_t *content)
{
    int len = (content != NULL) ? 5 : 1;
    char buf[10] = {0};
    buf[0] = id;
    buf[1] = command;
    if (content != NULL)
    {
        RD_MEMCPY(&buf[2], content, 4);        
    }
    return rd_ComWrite(g_ptFDCAN1, (char *)buf, len);
}

static void Motor_ClearFault(uint8_t id)
{
	CANSendCommand(id, 0x0B, NULL);
}

static void Motor_GetFaultState(uint8_t id)
{
	CANSendCommand(id, 0x0A, NULL); // set motor stop mode

}

static void GetCSPByCommand(uint8_t id)
{
	CANSendCommand(id, 0x41, NULL);
}

static void Motor_SetVelocityModeAndTargetVelocity(uint8_t id, int32_t targetSpeed)
{
    CANSendCommand(id, 0x1d, &targetSpeed); //Motor_SetVelocityModeAndTargetVelocity
}

static int check_TI5MOTOR(char *buffer, uint32_t length)
{
    TCANUserData *ptCANUserData = (TCANUserData *)g_ptFDCAN1->m_pUserData;
    uint8_t ID_A_T = (uint8_t)ptCANUserData->m_canrx->Identifier;
    int32_t Function_code = 0;

	if (ID_A_T >= DF_MSP_Ti5Motor_StartID
			&& ID_A_T < DF_MSP_Ti5Motor_StartID + 7)
	{
		(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).RxIndex++;
	}

	switch (Function_code)
	{
		case 3:
			memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Run_Mode,
					&buffer[1], length - 1);
			break;
		case 4: //ma
			memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Current,
					&buffer[1], length - 1);

			break;
		case 5:
			memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Target_Current,
					&buffer[1], length - 1);

			break;
		case 6:
			memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Velcity,
					&buffer[1], length - 1);

			break;
		case 7:
			memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Target_Velcity,
					&buffer[1], length - 1);

			break;
		case 8:
			memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Position,
					&buffer[1], length - 1);

			break;
		case 9:
			memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Target_Position,
					&buffer[1], length - 1);
			break;
		case 10:

			memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).ERROR_Flag,
					&buffer[1], length - 1);

			memcpy(&(Motor_ID_Errors[ID_A_T - DF_MSP_Ti5Motor_StartID]),
					&buffer[1], length - 1);
			break;
		case 49:
			memcpy(
					&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Temperature_Motor,
					&buffer[1], length - 1);
			break;
		case 50:
			memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Temperature_PCB,
					&buffer[1], length - 1);
			break;

		case 65:
		case 66:
		case 67:
		case 68:
			//电流 MA
			// 	case 65-68 均返回CSP
			//tempCSP=&buffer[0];
			memcpy(&tempCSP, buffer, 8);
			(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Current =
					tempCSP.Current;//单位为mA
			(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Velcity =
					tempCSP.Velocity;//转化为度每秒公式：(返回值/100/减速比)*360
			(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Position =
					tempCSP.Position;//转化为减速机角度公式：(返回值/65536/减速比)*360
			return 8;
		default:
			return -1;
	}
	return 1;
}

static void decode_TI5MOTOR(const char *_pBuffer, uint32_t _iSize)
{
    return;
}

void MoveWheel_Init(void)
{
    TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 8, 0);
    g_ptFDCAN1 = rd_ComCreate(check_TI5MOTOR, decode_TI5MOTOR, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData);
    CAN_IT_init(g_ptFDCAN1);

    for (int i = 1; i < 5; i++)
	{
		GetCSPByCommand(i);
        Rd_Delay(4);
		Motor_ClearFault(i);
        Rd_Delay(4);
	}
}

void MoveWheel_Task(int32_t Target_Velcity[])
{
    for (int i = 1; i < 5; i++)
	{
		Motor_ClearFault(i);
        Rd_Delay(4);
		Motor_GetFaultState(i);
        Rd_Delay(4);
        GetCSPByCommand(i);
        Rd_Delay(4);
	}
	for (int i = 1; i < 5; i++)
	{
		Motor_SetVelocityModeAndTargetVelocity(i, Target_Velcity[i]);
        Rd_Delay(4);
	}
}