/*
* 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 ) ;
}
#if 0
void Swing_Motor_Set_Target_Position ( )
{
CANSendMessageSDO ( SwingMotorID , 0x40 , 0x6041 , 0x00 , 0x00 ) ; ///：是否到达目的位置
}
void Swing_Motor_Read_ReachedEnd ( )
{
CANSendMessageSDO ( SwingMotorID , 0x40 , 0x6041 , 0x00 , 0x00 ) ; ///：是否到达目的位置
}
# endif
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 ] = = ( 0 XAA ) ) | ( buffer [ 1 ] = = ( 0 XBB ) ) )
{
TT_Motor [ ID_A_T_A ] - > Act_Suc = 1 ;
}
break ;
}
}
