ZhenYulin/BASE/Src/MSP/msp_TTMotor_ZQ.c

/*
 * msp_TTMotor_ZQ.c
 *
 *  Created on: Oct 10, 2024
 *      Author: akeguo
 */
#include "msp_TTMotor_ZQ.h"
#include "BHBF_ROBOT.h"
#include "bsp_FDCAN.h"

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

	//CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x6083, 0x00, AccTime,ZQ_Motor_Controller,WaitTime);

}




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

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

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

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

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

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

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

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

}

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

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

void Driver_ReadError(int MotorID,FDCANHandler *ZQ_Motor_Controller,int32_t WaitTime)
{
	//CANSendMessageSDO_ADD_To_SendList(CAN_ID, new byte[8] { 0x40, 0x0A, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00 });//40 0A 30 00 00 00 00 00

	CANSendMessageSDO_ADD_To_SendList(MotorID, 0x40, 0x300a, 0x00, 0,ZQ_Motor_Controller,WaitTime);  //40 0A 30 00 00 00 00 00
}
void SetMotorTargetPosition(int MotorID, int32_t TargetPosition,FDCANHandler *ZQ_Motor_Controller,int32_t WaitTime)
{
	Postion_Velcocity_Set_Position(MotorID, TargetPosition,ZQ_Motor_Controller,WaitTime);
}

void Postion_Velcocity_Stop(int MotorID,FDCANHandler *ZQ_Motor_Controller,int32_t WaitTime)
{

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

void SetCurrentPositionZero(int MotorID,FDCANHandler *ZQ_Motor_Controller,int32_t WaitTime)
{
	CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6098, 0x00, 0x23,ZQ_Motor_Controller,WaitTime); //1：设置定位的方式,向索	引0x6098:00写入0X23
	CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6060, 0x00, 6,ZQ_Motor_Controller,WaitTime);  //2：设置操作模式，向索引0x6060:00写入0x06
	CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x80,ZQ_Motor_Controller,WaitTime);  //3:清除异常，向控制字	0x6040:00写入0x80
	CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x06,ZQ_Motor_Controller,WaitTime);  //4：伺服准备，向控制字	0x6040:00写入0x06
	CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x07,ZQ_Motor_Controller,WaitTime);  //5：伺服等待使能，向控制字0x6040:00写入0x07
	CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x0F,ZQ_Motor_Controller,WaitTime);  //6：伺服使能，向控制字0x6040:00写入0x0F
	CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6040, 0x00, 0x1F,ZQ_Motor_Controller,WaitTime);  //7：启动原点定位,向控制字0x6040:00写入0x1F
}

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

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

}



void Swing_Motor_Set_Target_Position()
{
	CANSendMessageSDO(SwingMotorID, 0x40, 0x6041, 0x00, 0x00);     ///：是否到达目的位置
}

void Swing_Motor_Read_ReachedEnd()
{
	CANSendMessageSDO(SwingMotorID, 0x40, 0x6041, 0x00, 0x00);     ///：是否到达目的位置

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

}


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

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

}
//
//
void TT_Request_Fault(uint32_t Motor_ID_1,FDCANHandler *ZQ_Motor_Controller,int32_t WaitTime)
{
	CANSendMessageSDO_ADD_To_SendList(Motor_ID_1, 0x40, 0x300A, 0x00, 0x00,ZQ_Motor_Controller,WaitTime);

}

void TT_Request_Current(uint32_t Motor_ID_1,FDCANHandler *ZQ_Motor_Controller,int32_t WaitTime)
{
	CANSendMessageSDO_ADD_To_SendList(Motor_ID_1, 0x40, 0x6078, 0x00, 0x00,ZQ_Motor_Controller,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));
			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:
			TT_Motor[ID_A_T_A]->TT_Motor_Fault = (int) (buffer[4]
					| (buffer[5]) << 8 | (buffer[6]) << 16
					| (buffer[7] << 24)); //0.1A

			*Motor_ID_Errors[ID_A_T_A]=TT_Motor[ID_A_T_A]->TT_Motor_Fault;
			break;

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