/*
 * msp_TTMotor_ZQ.c
 *
 *  Created on: 2026年4月20日
 *      Author: Lizongdi
 */
#ifdef BUILD_MOTOR
#include "msp_TTMotor_ZQ.h"
#include "BHBF_ROBOT.h"
#include "motor.h"

#define Move_Base_Speed_Count_m_Min 201.7*7.64//天太电机1m/min

#define Swing_Speed_Deg_Sencond_motor 201.7//HJ32-121
#define middle_position_motor -944334
#define TT_One_Deg_Count_motor 	11014///32768*121/360(减速比121)=11014

TT_MotorParameters *TT_Motor[7];

void MotorMoveParamInit(void)
{
    GV.LeftMotor.Target_Velcity=(int)(GV.Left_Speed_M_min*Move_Base_Speed_Count_m_Min);
    GV.RightMotor.Target_Velcity=(int)(GV.Right_Speed_M_min*Move_Base_Speed_Count_m_Min);
}

void MotorMoveCAN2ParamInit(void)
{
	GV.SwingMotor.Tar_Position_count=(int32_t)(middle_position_motor-GV.Tar_Position_angle*TT_One_Deg_Count_motor);
	GV.SwingMotor.Tar_Position_Velcity_RPM=GV.Tar_Position_Velcity_Degree_S*Swing_Speed_Deg_Sencond_motor;
}

void ActivateMotor(TMotorCtrl *ptMotorCtrl, int32_t WaitTime)
{
	ptMotorCtrl->fHandler->Tx_Buf[0] = 0x10;
	ptMotorCtrl->fHandler->Tx_Buf[1] = 0x10;
	ptMotorCtrl->fHandler->AddCANSendList(ptMotorCtrl->fHandler, ptMotorCtrl->MotorID, 2,
	        ptMotorCtrl->fHandler->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send
}

void Enable_NMT(TMotorCtrl *ptMotorCtrl, int32_t WaitTime)
{    
	ptMotorCtrl->fHandler->Tx_Buf[0] = 0x01;
	ptMotorCtrl->fHandler->Tx_Buf[1] = ptMotorCtrl->MotorID;
	ptMotorCtrl->fHandler->AddCANSendList(ptMotorCtrl->fHandler, 0, 2, ptMotorCtrl->fHandler->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send
}

void Configure_Asynchronous_Mode(TMotorCtrl *ptMotorCtrl, int32_t Node_Number,  int32_t WaitTime)
{
	ptMotorCtrl->fHandler->Tx_Buf[0] = 0x23;
	ptMotorCtrl->fHandler->Tx_Buf[1] = 0x16;
	ptMotorCtrl->fHandler->Tx_Buf[2] = 0x10;
	ptMotorCtrl->fHandler->Tx_Buf[3] = 0x01;
	ptMotorCtrl->fHandler->Tx_Buf[4] = 0xe8;
	ptMotorCtrl->fHandler->Tx_Buf[5] = 0x03;
	ptMotorCtrl->fHandler->Tx_Buf[6] = Node_Number;
	ptMotorCtrl->fHandler->Tx_Buf[7] = 0x00;
	ptMotorCtrl->fHandler->AddCANSendList(ptMotorCtrl->fHandler, 0x600 + ptMotorCtrl->MotorID, 8, ptMotorCtrl->fHandler->Tx_Buf, WaitTime, NULL);
}

void Consumer_Or_microcontroller_Heartbeat(TMotorCtrl *ptMotorCtrl, int32_t WaitTime)
{
	ptMotorCtrl->fHandler->Tx_Buf[0] = 0x05;
	ptMotorCtrl->fHandler->AddCANSendList(ptMotorCtrl->fHandler, ptMotorCtrl->HeartbeatID, 1, ptMotorCtrl->fHandler->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send
}

void CANSendMessageSDO_ADD_To_SendList(TMotorCtrl *ptMotorCtrl, uint8_t Function,
        uint16_t ControlWord, uint8_t subWord, int32_t ControlWordValue, int32_t WaitTime)
{
	//copy the corresponsiding data to the send Array
	ptMotorCtrl->fHandler->Tx_Buf[0] = Function;
	memcpy(&ptMotorCtrl->fHandler->Tx_Buf[1], &ControlWord, 2);
	ptMotorCtrl->fHandler->Tx_Buf[3] = subWord;
	memcpy(&ptMotorCtrl->fHandler->Tx_Buf[4], &ControlWordValue, 4);
	//send 8 bytes data

	ptMotorCtrl->fHandler->AddCANSendList(ptMotorCtrl->fHandler, 0x600 + ptMotorCtrl->MotorID, 8,
	        ptMotorCtrl->fHandler->Tx_Buf, WaitTime, NULL);
}

//位置速度模式
void Postion_Velcocity_Run_SetParameter(TMotorCtrl *ptMotorCtrl, int32_t TargetPosition,
        int32_t TargetSpeed, int32_t AccTime, int32_t DecTime, int32_t WaitTime)
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6060, 0x00, 1,
	        WaitTime); // 1：设置操作模式，向索引0x6060:00写入0x01
	if (TargetSpeed >= 3500) //the highest is 3500
	{
		TargetSpeed = 3500;
	}
	if (TargetSpeed < 0)
	{
		TargetSpeed = 0;
	}
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x6083, 0x00, AccTime,
	        WaitTime); //2：设置加速时间，向索引0x6083:00写入4字节数值
	// Thread.Sleep(2);
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x6084, 0x00, DecTime,
	        WaitTime);  //3：设置减速时间，向索引0x6084:00写入4字节数值
	//Thread.Sleep(2);
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x607A, 0x00,
	        TargetPosition, WaitTime); //4：设置目标位置，向索引	0x607A:00写入4字节数值
	// Thread.Sleep(2);

	//23：写4字节数据；
	//816000:索引号为0x6081,子索引号为0x00；
	//E80300 00:设置运行速度为1000（单位0.1rpm）
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x6081, 0x00,
	        TargetSpeed * 10, WaitTime); //5：设置运行速度，向索引	0x6081:00写入4字节数值

	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x80,
	        WaitTime);  ///6:清除异常，向控制字0x6040:00写入0x80

	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x06,
	        WaitTime);  //7：伺服准备，向控制字	0x6040:00写入0x06

	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x07,
	        WaitTime);  //8：伺服等待使能，向控制字0x6040:00写入0x07

	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x2f,
	        WaitTime);  //9：伺服使能，向控制字0x6040:00写入0x0F

	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x3f,
	        WaitTime); //10：伺服启动,向控制字	0x6040:00写入0x1F  ///：
}

void Postion_Velcocity_Set_Position(TMotorCtrl *ptMotorCtrl, int32_t TargetPosition, int32_t WaitTime)
{    
//	3、立即更新时，重复执行步骤4、步骤9(控制字(6040)写入2F)和步骤10(控制字(6040)
//	写入3F)，驱动器还没到达当前目标位置时，支持接收新的目标位置，最终到达的目标位
//	置是：新的目标位置。
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x607a, 0x00,
	        TargetPosition, WaitTime);   //设置目标位置
	//Thread.Sleep(2);
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x2f,
	        WaitTime);   ///：

	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x3f,
	        WaitTime);    ///：
}

void Driver_ReadError(TMotorCtrl *ptMotorCtrl, int32_t WaitTime)
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x300a, 0x00, 0,
	        WaitTime);  //40 0A 30 00 00 00 00 00
}
void SetMotorTargetPosition(TMotorCtrl *ptMotorCtrl, int32_t TargetPosition, int32_t WaitTime)
{
	Postion_Velcocity_Set_Position(ptMotorCtrl, TargetPosition, WaitTime);
}

void Postion_Velcocity_Stop(TMotorCtrl *ptMotorCtrl, int32_t WaitTime)
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6085, 0x00, 200,
	        WaitTime); // 6085 的减速时间100ms
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x605a, 0x00, 6,
	        WaitTime); // 0x06：按索引 6085 的减速时间进行减速，停下时电机 轴处于锁轴状态。
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x02,
	        WaitTime);  //快速停机
}

void SpeedModeSetup(TMotorCtrl *ptMotorCtrl, int32_t WaitTime, int32_t Acc, int32_t Dec, int32_t TargetVelocity) //设定速度模式，并更改相关速度
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x02,
	        100);
	//设置操作模式，向索引0x6060：00写入0x03
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6060, 0x00, 0x3,
	        WaitTime);
	//2:清除异常， 向控制字	0x6040:00 写入 0x80
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x80,
	        WaitTime);
	//3： 伺服准备, 向控制字	0x6040:00 写入 0x06
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x06,
	        WaitTime);
	//4： 伺服等待使能, 向控制字 0x6040:00 写入 0x07
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x07,
	        WaitTime);
	//5： 伺服使能,向控制字0x6040:00 写入 0x0F
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x0f,
	        WaitTime);
	//6:设置加速时间， 向索引	0x201C:00写入4字节数值 //不关心加速时间的模式下， 加减速时间建议设置为 1500（ms） 以上
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x201c, 0x00, Acc,
	        WaitTime);
	//7： 设置减速时间， 向索引0x201D:00写入4字节数据
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x201d, 0x00, Dec,
	        WaitTime);
	//8： 设定目标速度值， 向索引 0x60FF:00 写入 4 字节数值（有符号）
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x60ff, 0x00,
	        TargetVelocity, WaitTime);  ///6:清除异常，向控制字

	//修改步骤8 可以实现速度的改变
	// 23:写 4 字节数据；FF 60 00:索引号为 0x60FF，
	// 子索引号为 0x00;E8 03 00 00：
	// 设置目标速度 1000（单位 0.1rpm）
}
void TT_SpeedMode_Set_TargetSpeed(TMotorCtrl *ptMotorCtrl, int32_t WaitTime, int32_t TargetSpeed)
{    
    CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x60ff, 0x00, TargetSpeed,
	        WaitTime);
}

void Set_Current_Positon_Zero(TMotorCtrl *ptMotorCtrl, int32_t WaitTime)  // Home  设置当前位置为零点
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6098, 0x00, 0x23,
	        WaitTime); //1. 设置定位的方式,向索引0x6098:00写入0X23
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6060, 0x00, 0x06,
	        WaitTime); //2：设置操作模式，向索引0x6060:00写入0x06
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x80,
	        WaitTime); //3:清除异常，向控制字0x6040:00写入0x80
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x06,
	        WaitTime); //4.伺服准备，向控制字0x6040:00写入0x06
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x07,
	        WaitTime); //5. 伺服等待使能，向控制字0x6040:00写入0x07
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x0f,
	        WaitTime); //6. 伺服使能 向控制字	0x6040:00写入0x0F
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6040, 0x00, 0x1f,
	        WaitTime); //7. ：启动原点定位,向控制字0x6040:00写入0x1F
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x6041, 0x00, 0x0,
	        100); //8. ：启动原点定位,向控制字0x6040:00写入0x1F

}

void TT_Request_Position(TMotorCtrl *ptMotorCtrl, int32_t WaitTime)     // Home  设置当前位置为零点
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x6064, 0x00, 0x00,
	        WaitTime); //用户实际速度反馈(0.1rpm)
}


void Position_Immediately_Setting(TMotorCtrl *ptMotorCtrl, int32_t Deri_Position, int32_t Deri_Speed, int32_t WaitTime)  // Home  设置当前位置为零点
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x607A, 0x00, Deri_Position, WaitTime); //设置位置
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x6081, 0x00, Deri_Speed,    WaitTime); //设置速度
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2B, 0x6040, 0x00, 0x2f,          WaitTime);
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2B, 0x6040, 0x00, 0x3f,          WaitTime);
}

void Position_Lag_Setting(TMotorCtrl *ptMotorCtrl, int32_t Deri_Position, int32_t Deri_Speed, int32_t WaitTime)  // Home  设置当前位置为零点
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x607A, 0x00, Deri_Position, WaitTime); //设置位置
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x6081, 0x00, Deri_Speed,    WaitTime); //设置速度
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2B, 0x6040, 0x00, 0x0f,          WaitTime);
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2B, 0x6040, 0x00, 0x1f,          WaitTime);
}

void Position_Lay_Setting(TMotorCtrl *ptMotorCtrl, int32_t Deri_Position,int32_t Deri_Speed, int32_t WaitTime)  // Home  设置当前位置为零点
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6098, 0x00, 0x23,
	        WaitTime); //1. 设置定位的方式,向索引0x6098:00写入0X23
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6060, 0x00, 0x06,
	        WaitTime); //2：设置操作模式，向索引0x6060:00写入0x06
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x80,
	        WaitTime); //3:清除异常，向控制字0x6040:00写入0x80
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x06,
	        WaitTime); //4.伺服准备，向控制字0x6040:00写入0x06
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x07,
	        WaitTime); //5. 伺服等待使能，向控制字0x6040:00写入0x07
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x0f,
	        WaitTime); //6. 伺服使能 向控制字	0x6040:00写入0x0F
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6040, 0x00, 0x1f,
	        WaitTime); //7. ：启动原点定位,向控制字0x6040:00写入0x1F
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x6041, 0x00, 0x0,
	        100); //8. ：启动原点定位,向控制字0x6040:00写入0x1F
}

void TT_Request_Velocity(TMotorCtrl *ptMotorCtrl, int32_t WaitTime)
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x606c, 0x00, 0x00,
	        WaitTime); //用户实际速度反馈(0.1rpm)

}
//
//
void TT_Request_Fault(TMotorCtrl *ptMotorCtrl, int32_t WaitTime)
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x300A, 0x00, 0x00,
	        WaitTime);

}

void TT_Request_Current(TMotorCtrl *ptMotorCtrl, int32_t WaitTime)
{
	CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x6078, 0x00, 0x00,
	        WaitTime);
}

void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer)
{

	int Function_code = (int) ((int16_t) (buffer[1] | buffer[2] << 8));
	switch (Function_code)
	{

		case 24672:
			TT_Motor[ID_A_T_A]->Cont_Posi_Suc = 1;
			break;
		case 24707:
			TT_Motor[ID_A_T_A]->Acc_Suc = 1;
			break;
		case 24708:
			TT_Motor[ID_A_T_A]->Dec_Suc = 1;
			break;
		case 24698:
			TT_Motor[ID_A_T_A]->Target_Posi_Suc = 1;
			break;
		case 24705:
			TT_Motor[ID_A_T_A]->Run_Speed_Suc = 1;
			break;
		case 24640:
			TT_Motor[ID_A_T_A]->Clear_Suc = 1;
			break;
		case 8220:
			TT_Motor[ID_A_T_A]->Acc_Suc_Speed = 1;
			break;
		case 24831:
			TT_Motor[ID_A_T_A]->Suc_Speed_S = 1;
			break;
		case 8221:
			TT_Motor[ID_A_T_A]->Dec_Suc_Speed = 1;
			break;
		case 24676:
		{
			TT_Motor[ID_A_T_A]->Real_Position = (int) (buffer[4]
			        | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24));
			if (TT_Motor[ID_A_T_A]->Start_Measuring == 1)
			{
				TT_Motor[ID_A_T_A]->Start_Measuring =0;
				TT_Motor[ID_A_T_A]->Number_Of_Rounds=0;

				TT_Motor[ID_A_T_A]->Start_Measuring_Position=TT_Motor[ID_A_T_A]->Real_Position;
				TT_Motor[ID_A_T_A]->Last_Real_Position=TT_Motor[ID_A_T_A]->Real_Position;
			}
			else
			{
				if (abs(
				        TT_Motor[ID_A_T_A]->Real_Position
				                - TT_Motor[ID_A_T_A]->Last_Real_Position)
				        >= 30000)
				{
					if (TT_Motor[ID_A_T_A]->Real_Position > 0)
					{
						TT_Motor[ID_A_T_A]->Number_Of_Rounds -= 1;
					}
					else
					{
						TT_Motor[ID_A_T_A]->Number_Of_Rounds += 1;
					}
				}
			}

			TT_Motor[ID_A_T_A]->Last_Real_Position =
			        TT_Motor[ID_A_T_A]->Real_Position;

			double CircleLength = 3.14 * 280/1000 ; //pi*d //m

			double _tempDeltCounts = TT_Motor[ID_A_T_A]->Real_Position
			        - TT_Motor[ID_A_T_A]->Start_Measuring_Position;
			TT_Motor[ID_A_T_A]->Real_Disatnce = (_tempDeltCounts/ 32768/101
			        + (double) TT_Motor[ID_A_T_A]->Number_Of_Rounds
			                )
			        * CircleLength;

		}

			break;
		case 24684:
			TT_Motor[ID_A_T_A]->Real_Velcity = (int) (buffer[4]
			        | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24));
			break;

		case 24696:
			TT_Motor[ID_A_T_A]->Real_Current = (int) (buffer[4]
			        | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24)); //0.1A
			break;
		case 12298://0x3001
			TT_Motor[ID_A_T_A]->TT_Motor_Fault = (int) (buffer[4]
			        | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24));

			break;

		default:
			if ((buffer[0] == (0XAA)) | (buffer[1] == (0XBB)))
			{
				TT_Motor[ID_A_T_A]->Act_Suc = 1;
			}
			break;
	}
}

#endif