/*
 * msp_TI5MOTOR.c
 *
 *  Created on: Oct 26, 2023
 *      Author: shiya
 */
#include "msp_TI5MOTOR.h"
#include "bsp_FDCAN.h"
#include "msp_Motor.pb.h"
#include "BHBF_ROBOT.h"
#pragma pack (2) /*指定按2字节对齐*/
typedef struct _CSP
{
	int16_t Current;
	int16_t Velocity;
	int32_t Position;
} CSP;

typedef struct _TSP
{
	int32_t Torque;
	int32_t Velocity;
	int32_t Position;
} TSP;

#pragma pack () /*取消指定对齐，恢复缺省对齐*/
CSP tempCSP;

int32_t Function_code = 0;
uint8_t ID_A_T;

void DecodeTi5MotorCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
{
	ID_A_T = canID;

	if (ID_A_T >= DF_MSP_Ti5Motor_StartID
			&& ID_A_T < DF_MSP_Ti5Motor_StartID + 3)
	{
		(*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: //
			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

			break;
		default:

			break;
	}
}

void OneByteCommand(int8_t MotorID, uint8_t command,
		FDCANHandler *Ti5_Motor_Controller, int WaitTime)
{

	Ti5_Motor_Controller->Tx_Buf[0] = command;

	Ti5_Motor_Controller->AddCANSendList(Ti5_Motor_Controller, MotorID, 1,Ti5_Motor_Controller->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send
}
void FiveByteCommand(int8_t MotorID, uint8_t command, int32_t content,
		FDCANHandler *Ti5_Motor_Controller, int WaitTime)//五字节指令
{

	Ti5_Motor_Controller->Tx_Buf[0] = command;
	memcpy(&Ti5_Motor_Controller->Tx_Buf[1], &content, 4);
//	Ti5_Motor_Controller->CAN_Send(Ti5_Motor_Controller, MotorID, 5,
//			Ti5_Motor_Controller->Tx_Buf);
	Ti5_Motor_Controller->AddCANSendList(Ti5_Motor_Controller, MotorID, 5,
			Ti5_Motor_Controller->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send
}

void Motor_Stop(uint8_t id, FDCANHandler *Ti5_Motor_Controller, int WaitTime)//停车 0x02
{
	OneByteCommand(id, 2, Ti5_Motor_Controller, WaitTime); // set motor stop mode
}
void Motor_ClearFault(uint8_t id, FDCANHandler *Ti5_Motor_Controller,
		int WaitTime)
{
	OneByteCommand(id, 0x0B, Ti5_Motor_Controller, WaitTime);
}
//设置电机运行模式为速度模式，并设置目标速度
void Motor_SetVelocityModeAndTargetVelocity(uint8_t id, int32_t targetSpeed,
		FDCANHandler *Ti5_Motor_Controller, int WaitTime)
{
	FiveByteCommand(id, 0x1d, targetSpeed, Ti5_Motor_Controller, WaitTime); //Motor_SetVelocityModeAndTargetVelocity
}
void Motor_GetFaultState(uint8_t id, FDCANHandler *Ti5_Motor_Controller,
		int WaitTime)
{
	OneByteCommand(id, 0x0A, Ti5_Motor_Controller, WaitTime); // set motor stop mode
}

void SetMaximumAllowablePositiveVelocity(uint8_t id, int32_t maxSpeed,
		FDCANHandler *Ti5_Motor_Controller, int WaitTime)
{
	FiveByteCommand(id, 0x24, maxSpeed, Ti5_Motor_Controller, WaitTime);
}
void SetMinimumAllowableNegativeVelocity(uint8_t id, int32_t maxSpeed,
		FDCANHandler *Ti5_Motor_Controller, int WaitTime)
{
	FiveByteCommand(id, 0x25, maxSpeed, Ti5_Motor_Controller, WaitTime);
}

void SetPositionModeAndTargetPosition(uint8_t id, int32_t targetPosition,
		FDCANHandler *Ti5_Motor_Controller, int WaitTime)
{
	FiveByteCommand(id, 0x1e, targetPosition, Ti5_Motor_Controller, WaitTime);
}
void GetCSPByCommand(uint8_t id, FDCANHandler *Ti5_Motor_Controller,
		int WaitTime)
{
	OneByteCommand(id, 0x41, Ti5_Motor_Controller, WaitTime);
}

void SetMaximumPositivePosition(uint8_t id, int32_t position,
		FDCANHandler *Ti5_Motor_Controller, int WaitTime)
{
	FiveByteCommand(id, 0x26, position, Ti5_Motor_Controller, WaitTime);
}
/// <summary>
/// 获取最小负向位置 减速机角度为：(返回值/65536/减速比)*360<
/// </summary>
void SetMinimumNegativePosition(uint8_t id, int32_t position,
		FDCANHandler *Ti5_Motor_Controller, int WaitTime)
{
	FiveByteCommand(id, 0x27, position, Ti5_Motor_Controller, WaitTime);
}
void SetCurrentModeAndTargetCurent(uint8_t id, int32_t targetCurrent,
		FDCANHandler *Ti5_Motor_Controller, int WaitTime)
{
	FiveByteCommand(id, 0x1C, targetCurrent, Ti5_Motor_Controller, WaitTime);

}

/// <summary>
/// 获取电机位置
/// </summary>
void ReadPosition(uint8_t id, FDCANHandler *Ti5_Motor_Controller,
		int WaitTime)
{
	OneByteCommand(id, 0x08, Ti5_Motor_Controller, WaitTime);
}