///* // * motors.c // * // * Created on: 2025年12月29日 // * Author: xsq // */ // //#include "motors.h" //#include "msp_TTMotor_ZQ.h" // //char TT_Motor_Need_To_Activate = 0; // //FDCANHandler *Roughening_Motor_Controller; //DispacherController *Roughening_DispacherController; //TT_MotorParameters *TT_Motor[7]; //#define LeftMotorID 1 //#define RightMotorID 2 //int32_t speed_E01m_per_min(int32_t speed_E01m_per_min); // //void MotorCommandsLoop(); //void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length); // //void Roughening_Motor_Controller_intialize(FDCANHandler *Handler) //{ // //初始化 // // Roughening_Motor_Controller = Handler; // Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN; // // HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, // "ZQ_CAN_ID2_LeftMotor", 0, ComError_ZQ_LeftMotor); // HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, // "ZQ_CAN_ID3_RightMotor", 0, ComError_ZQ_RightMotor); // // Roughening_DispacherController = Handler->dispacherController; // Roughening_DispacherController->DispacherCallTime = 2; // Roughening_DispacherController->Add_Dispatcher_List( // Roughening_DispacherController, MotorCommandsLoop); // // LOGFF(DL_WARN,"TT_Motors_intialize"); // //} //void MotorCommandsLoop() //{ // // if (TT_Motor_Need_To_Activate == 1) // { // // ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); // ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); // ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); // ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); // // SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 1000, 1000, 0); // SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 1000, 1000, 0); //// SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 3500, 450, 0); //// SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 3500, 450, 0); // TT_Motor_Need_To_Activate = 2; // } // else if (TT_Motor_Need_To_Activate == 2) // { // for (int i = 1; i < 3; i++) // { // TT_Request_Position(i, Roughening_Motor_Controller, 6); // TT_Request_Velocity(i, Roughening_Motor_Controller, 6); // TT_Request_Current(i, Roughening_Motor_Controller, 6); // TT_Request_Fault(i, Roughening_Motor_Controller, 6); // TT_Request_Tempature(i, Roughening_Motor_Controller, 6); // } // TT_SpeedMode_Set_TargetSpeed(LeftMotorID, Roughening_Motor_Controller, 6, // speed_E01m_per_min(GV.LeftMotor.Target_Velcity) ); // TT_SpeedMode_Set_TargetSpeed(RightMotorID, Roughening_Motor_Controller, 6, // speed_E01m_per_min(-GV.RightMotor.Target_Velcity) ); // // } // //} // //char a[10]; //void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length) //{ // // memcpy(a,buffer,8); // switch (canID - 0x580) // { // // case LeftMotorID: // { // HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, // "ZQ_CAN_ID2_LeftMotor", 1); // TT_Analytic_Fun(LeftMotorID, buffer); // } // break; // case RightMotorID: // { // HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, // "ZQ_CAN_ID3_RightMotor", 1); // TT_Analytic_Fun(RightMotorID, buffer); // } // break; // } // //} ///* // * SpeedMPMin:0.1 m/min // * return:1 pulse/s // * */ //int32_t speed_E01m_per_min(int32_t speed_E01m_per_min) //{ // return (int32_t) (speed_E01m_per_min*0.1 *10 * CV.wheel_Reduction_Ratio // / (3.14 * CV.wheel_Diameter_m)); //} /** * motors.c版本02：仿照摆臂的心跳模式，有问题，左电机死掉了 * * / /* * motors.c * * Created on: 2025年12月29日 * Author: xsq */ #include "motors.h" #include "msp_TTMotor_ZQ.h" char TT_Motor_Need_To_Activate = 0; FDCANHandler *Roughening_Motor_Controller; DispacherController *Roughening_DispacherController; TT_MotorParameters *TT_Motor[7]; #define LeftMotorID 1 #define RightMotorID 2 // --- 定义心跳包 CAN ID (0x700 + 节点ID) --- #define HEARTBEAT_ID_LEFT 0x701 #define HEARTBEAT_ID_RIGHT 0x702 int32_t speed_E01m_per_min(int32_t speed_E01m_per_min); void MotorCommandsLoop(); void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length); // --- 1. 心跳包发送函数 --- void Send_Motor_Heartbeat(int32_t can_id, FDCANHandler *handler) { handler->Tx_Buf[0] = 0x05; // 0x05 = Operational handler->AddCANSendList(handler, can_id, 1, handler->Tx_Buf, 0, NULL); } // --- 2. 配置心跳监控 (Consumer Heartbeat) --- void Configure_Asynchronous_Mode(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t Node_Number, int32_t WaitTime) { ZQ_Motor_Controller->Tx_Buf[0] = 0x23; // SDO 写入 ZQ_Motor_Controller->Tx_Buf[1] = 0x16; // 0x1016 ZQ_Motor_Controller->Tx_Buf[2] = 0x10; ZQ_Motor_Controller->Tx_Buf[3] = 0x01; // ZQ_Motor_Controller->Tx_Buf[4] = 0xe8; // 1000ms // ZQ_Motor_Controller->Tx_Buf[5] = 0x03; ZQ_Motor_Controller->Tx_Buf[4] = 0x64; // 100ms ZQ_Motor_Controller->Tx_Buf[5] = 0x00; ZQ_Motor_Controller->Tx_Buf[6] = Node_Number; // 监听 ID ZQ_Motor_Controller->Tx_Buf[7] = 0x00; ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, 0x600 + MotorID, 8, ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); } // --- 3. 新增：NMT 启动函数 --- void Enable_NMT(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t Node_Number, int32_t WaitTime) { ZQ_Motor_Controller->Tx_Buf[0] = 0x01; // 0x01 = Start Node ZQ_Motor_Controller->Tx_Buf[1] = Node_Number; // 发送到 0x000 (NMT 广播地址) ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, 0x000, 2, ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); } void Roughening_Motor_Controller_intialize(FDCANHandler *Handler) { Roughening_Motor_Controller = Handler; Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN; HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID2_LeftMotor", 0, ComError_ZQ_LeftMotor); HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_RightMotor", 0, ComError_ZQ_RightMotor); Roughening_DispacherController = Handler->dispacherController; Roughening_DispacherController->DispacherCallTime = 2; Roughening_DispacherController->Add_Dispatcher_List( Roughening_DispacherController, MotorCommandsLoop); LOGFF(DL_WARN,"TT_Motors_intialize"); } void MotorCommandsLoop() { static int heartbeat_counter = 0; if (TT_Motor_Need_To_Activate == 1) { // 1. 激活电机 (内部可能包含状态机切换) ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); // ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); // ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); // 3. 【新增】发送 NMT 启动指令 // 这一步是必须的，确保电机进入运行状态 Enable_NMT(000, Roughening_Motor_Controller, 1, 1000); Enable_NMT(000, Roughening_Motor_Controller, 2, 1000); // 2. 配置心跳监控 // 告诉电机：监听 ID 1 和 2，超时 1000ms Configure_Asynchronous_Mode(LeftMotorID, Roughening_Motor_Controller, 1, 500); Configure_Asynchronous_Mode(RightMotorID, Roughening_Motor_Controller, 2, 500); // 4. 配置速度模式 SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 500, 100, 0); SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 500, 100, 0); // 5. 立即发送一次心跳 Send_Motor_Heartbeat(HEARTBEAT_ID_LEFT, Roughening_Motor_Controller); Send_Motor_Heartbeat(HEARTBEAT_ID_RIGHT, Roughening_Motor_Controller); TT_Motor_Need_To_Activate = 2; } else if (TT_Motor_Need_To_Activate == 2) { // 读取状态 for (int i = 1; i < 3; i++) { TT_Request_Position(i, Roughening_Motor_Controller, 6); TT_Request_Velocity(i, Roughening_Motor_Controller, 6); TT_Request_Current(i, Roughening_Motor_Controller, 6); TT_Request_Fault(i, Roughening_Motor_Controller, 6); TT_Request_Tempature(i, Roughening_Motor_Controller, 6); } // 速度控制 TT_SpeedMode_Set_TargetSpeed(LeftMotorID, Roughening_Motor_Controller, 4, speed_E01m_per_min(GV.LeftMotor.Target_Velcity) ); TT_SpeedMode_Set_TargetSpeed(RightMotorID, Roughening_Motor_Controller, 4, speed_E01m_per_min(-GV.RightMotor.Target_Velcity) ); // 周期性心跳 heartbeat_counter++; if (heartbeat_counter >= 5) { Send_Motor_Heartbeat(HEARTBEAT_ID_LEFT, Roughening_Motor_Controller); Send_Motor_Heartbeat(HEARTBEAT_ID_RIGHT, Roughening_Motor_Controller); heartbeat_counter = 0; } } } char a[10]; void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length) { memcpy(a,buffer,8); switch (canID - 0x580) { case LeftMotorID: { HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID2_LeftMotor", 1); TT_Analytic_Fun(LeftMotorID, buffer); } break; case RightMotorID: { HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_RightMotor", 1); TT_Analytic_Fun(RightMotorID, buffer); } break; } } int32_t speed_E01m_per_min(int32_t speed_E01m_per_min) { return (int32_t) (speed_E01m_per_min*0.1 *10 * CV.wheel_Reduction_Ratio / (3.14 * CV.wheel_Diameter_m)); }