LiZongdi
/
BingooRobot_pro
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
仓库提交练习
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.
12 Commits
1 Branch
0 Tags
2.6 MiB
Tree: 1a12b72164
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '1a12b72164'
${ noResults }
BingooRobot_pro/Bingoo/motor/msp_TTMotor_ZQ.c

390 lines
16 KiB
Raw Normal View History

将FDCAN放到common模块，motor与TTmotor放到motor模块并编译通过
3 weeks ago
/*
* 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