You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
336 lines
9.8 KiB
336 lines
9.8 KiB
///*
|
|
// * 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));
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|