/* * 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; } }