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.
426 lines
18 KiB
426 lines
18 KiB
/*
|
|
* msp_TTMotor_ZQ.c
|
|
*
|
|
* Created on: Oct 10, 2024
|
|
* Author: akeguo
|
|
*/
|
|
#include "msp_TTMotor_ZQ.h"
|
|
#include "BHBF_ROBOT.h"
|
|
|
|
|
|
TT_MotorParameters *TT_Motor[7];
|
|
|
|
|
|
void ActivateMotor(int32_t 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 Enable_NMT(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t Node_Number, int32_t WaitTime)
|
|
{
|
|
ZQ_Motor_Controller->Tx_Buf[0] = 0x01;
|
|
ZQ_Motor_Controller->Tx_Buf[1] = Node_Number;
|
|
ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, MotorID, 2, ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send
|
|
}
|
|
|
|
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;
|
|
ZQ_Motor_Controller->Tx_Buf[1] = 0x16;
|
|
ZQ_Motor_Controller->Tx_Buf[2] = 0x10;
|
|
ZQ_Motor_Controller->Tx_Buf[3] = 0x01;
|
|
ZQ_Motor_Controller->Tx_Buf[4] = 0xe8;
|
|
ZQ_Motor_Controller->Tx_Buf[5] = 0x03;
|
|
ZQ_Motor_Controller->Tx_Buf[6] = Node_Number;
|
|
ZQ_Motor_Controller->Tx_Buf[7] = 0x00;
|
|
ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, 0x600 + MotorID, 8, ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL);
|
|
}
|
|
|
|
void Consumer_Or_microcontroller_Heartbeat(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime)
|
|
{
|
|
ZQ_Motor_Controller->Tx_Buf[0] = 0x05;
|
|
ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, MotorID, 1, ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send
|
|
}
|
|
|
|
|
|
void CANSendMessageSDO_ADD_To_SendList(int32_t 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(int32_t MotorID, int32_t TargetPosition,
|
|
int32_t TargetSpeed, int32_t AccTime, int32_t 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 >= 3500) //the highest is 3500
|
|
{
|
|
TargetSpeed = 3500;
|
|
}
|
|
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);
|
|
|
|
//23：写4字节数据；
|
|
//816000:索引号为0x6081,子索引号为0x00；
|
|
//E80300 00:设置运行速度为1000（单位0.1rpm）
|
|
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(int32_t MotorID, int32_t 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(int32_t 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(int32_t 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(int32_t 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(int32_t 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(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller,
|
|
int32_t WaitTime, int32_t Acc, int32_t Dec, int32_t TargetVelocity) //设定速度模式，并更改相关速度
|
|
{
|
|
CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x02,
|
|
ZQ_Motor_Controller, 100);
|
|
//设置操作模式，向索引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, 0x2b, 0x6040, 0x00, 0x2f,ZQ_Motor_Controller,WaitTime); ///：
|
|
// CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x3f,ZQ_Motor_Controller,WaitTime); ///：
|
|
|
|
//CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x2008, 0x00, 0x01,ZQ_Motor_Controller,1000);
|
|
//CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x02,ZQ_Motor_Controller,100); //1. 停止执行
|
|
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
|
|
CANSendMessageSDO_ADD_To_SendList(MotorID, 0x40, 0x6041, 0x00, 0x0,
|
|
ZQ_Motor_Controller, 100); //8. ：启动原点定位,向控制字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 Position_Immediately_Setting(uint8_t MotorID, FDCANHandler *ZQ_Motor_Controller,int32_t Deri_Position,int32_t Deri_Speed, int32_t WaitTime) // Home 设置当前位置为零点
|
|
{
|
|
CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x607A, 0x00, Deri_Position, ZQ_Motor_Controller, WaitTime); //设置位置
|
|
CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x6081, 0x00, Deri_Speed, ZQ_Motor_Controller, WaitTime); //设置速度
|
|
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 Position_Lag_Setting(uint8_t MotorID, FDCANHandler *ZQ_Motor_Controller,int32_t Deri_Position,int32_t Deri_Speed, int32_t WaitTime) // Home 设置当前位置为零点
|
|
{
|
|
CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x607A, 0x00, Deri_Position, ZQ_Motor_Controller, WaitTime); //设置位置
|
|
CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x6081, 0x00, Deri_Speed, ZQ_Motor_Controller, WaitTime); //设置速度
|
|
CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2B, 0x6040, 0x00, 0x0f, ZQ_Motor_Controller, WaitTime);
|
|
CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2B, 0x6040, 0x00, 0x1f, ZQ_Motor_Controller, WaitTime);
|
|
}
|
|
|
|
|
|
|
|
|
|
void Position_Lay_Setting(uint8_t MotorID, FDCANHandler *ZQ_Motor_Controller,int32_t Deri_Position,int32_t Deri_Speed, 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
|
|
CANSendMessageSDO_ADD_To_SendList(MotorID, 0x40, 0x6041, 0x00, 0x0,
|
|
ZQ_Motor_Controller, 100); //8. ：启动原点定位,向控制字0x6040:00写入0x1F
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
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));
|
|
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;
|
|
}
|
|
}
|
|
|
|
|