Axis.c
4.71 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
#include "includes.h"
#include "hardware.h"
#include "all_value.h"
#include "Axis.h"
Str_MotorControl Motor[MaxMotNums];
u8 Node_AxisNoChange(u8 Node) //轴号节点转换
{
if (Node==14) return 0;
else if (Node==15) return 5;
else return Node-9;
}
void AxisData_Analysis(CanRxMsg*Message)//轴信息解析函数
{
u8 BoxNo;//转换前的包号
u8 Node;//转换前的轴节点号
u8 AxisNum=0;//转换后的节点号
u8 NodeTpdoNo;//转换后的包号
u32 M_Index;//解析索引
signed int S32;
// RxMessage2=*Message;
/**************************轴PDO报文时根据节点号解析数据******************************/
//节点号10-左前舵轮行走轴；11-左前舵轮转向轴；12-右后舵轮行走轴；13-右后舵轮转向轴；14-升降轴；15-输送轴；
AxisNum=0; //0x18A-18F,0x28A-28F,0x38A-38F,0x48A-48F
NodeTpdoNo=0;
BoxNo=(Message->StdId>>8)&0x0F;
Node=Message->StdId&0x0F;
if(Node>9 &&Node<16)
{ if( ((BoxNo>0&&BoxNo<=5)&&((Message->StdId&0x0F0)==0x080))|((BoxNo==7)&&((Message->StdId&0x0F0)==0x000)) )
{ AxisNum=Node_AxisNoChange(Node);
NodeTpdoNo=BoxNo;
}
}
if ( (AxisNum<MaxMotNums)&& (NodeTpdoNo>=1&& NodeTpdoNo<=4)&& (Message->ExtId==0) )
{
switch(NodeTpdoNo)
{
case 1://包1 状态字2B+实际位置4B+实际电流2B；
// Motor[AxisNum].MToPc.StatusWord=((Message->Data[1]<<8)+Message->Data[0]);
memcpy(&(Motor[AxisNum].MToPc.StatusWord),&(Message->Data[0]),2);//位3：->故障 ；位10：->到大目标位置 ；位11：->轴限位触发 ；位15：->原点到位；位5：->快速停止 ；
if ((Motor[AxisNum].MToPc.StatusWord & 0x07)==0x07){ Motor[AxisNum].MToPc.Powered=1;/*Motor[AxisNum].MToPc.Ax_Status&=0x0FFF;*/}
else{Motor[AxisNum].MToPc.Powered=0;/*M_Index=Motor[AxisNum].MToPc.Ax_Status>>12;M_Index++;if(M_Index>2)Motor[AxisNum].MToPc.Powered=0;Motor[AxisNum].MToPc.Ax_Status|=(M_Index<<12);*/}
memcpy(&(Motor[AxisNum].MToPc.Pos_Actual),&(Message->Data[2]),4);
if(Motor[AxisNum].PulseS_Unit)
{ if( (AxisNum==2)|(AxisNum==4))Motor[AxisNum].MToPc.Angle_Actual=(float)F_Ang_HelmZerToAgvX*0.01f-(float)((float)Motor[AxisNum].MToPc.Pos_Actual/(float)Motor[AxisNum].PulseS_Unit);//转向轴位置，逆时针角度增大，与车本体坐标系一致
else Motor[AxisNum].MToPc.Angle_Actual=(float)Motor[AxisNum].MToPc.Pos_Actual/(float)Motor[AxisNum].PulseS_Unit;
}
memcpy(&(Motor[AxisNum].MToPc.Current),&(Message->Data[6]),2);
VaIndexW_W(&Motor[AxisNum].MToPc.Ax_Status,1,1);//标记收到1号报文
Motor[AxisNum].MToPc.Tim4_NodeProNums=0;
break;
case 2://包2 错误码2B +实际速度4B+工作模式1B+DI口1B
memcpy(&(Motor[AxisNum].MToPc.ErrCode),&(Message->Data[0]),2);
memcpy(&(S32),&(Message->Data[2]),4);//单位1rpm
Motor[AxisNum].MToPc.speed=(int)((float)S32*0.1f);
Motor[AxisNum].MToPc.WorkModeNow=Message->Data[6];
VaIndexW_W(&Motor[AxisNum].MToPc.Ax_Status,5,(Message->Data[7]>>1)&0x01);//Ax_Status位0：->节点是否在线；位1：->收到1号报文；位2：->收到2号报文；位3：->收到3号报文；位4：->左限位触发；位5：->右限位触发；
VaIndexW_W(&Motor[AxisNum].MToPc.Ax_Status,4,(Message->Data[7]>>3)&0x01);//
VaIndexW_W(&Motor[AxisNum].MToPc.Ax_Status,2,1);//标记收到2号报文
Motor[AxisNum].MToPc.Tim4_NodeProNums=0;
break;
case 3://包3 驱动电压4B+控制字2B
memcpy(&(Motor[AxisNum].MToPc.ContWord),&(Message->Data[4]),2);
VaIndexW_W(&Motor[AxisNum].MToPc.Ax_Status,3,1);//标记收到3号报文
Motor[AxisNum].MToPc.Tim4_NodeProNums=0;
break;
case 4:
break;
default:
break;
}
}
/***********************轴心跳报文反馈******************************/
if( (AxisNum<MaxMotNums) && (NodeTpdoNo==7) )
{
if ( (Motor[AxisNum].MToPc.NodeStatus>>7)!=(Message->Data[0]>>7) && ( (Motor[AxisNum].PcToM.NodeProtTime_Step==100)|(NodeProEn==0)) )//开启节点保护之后再看是否有节点反馈心跳
{
Motor[AxisNum].MToPc.Tim4_NodeProNums=0;
}
Motor[AxisNum].MToPc.NodeStatus=Message->Data[0];
// Motor[AxisNum].MToPc.NodeRealStatus=(Motor[AxisNum].MToPc.NodeStatus&0x7F);
}
/***********************索引解析******************************/
if( (AxisNum<MaxMotNums) && (NodeTpdoNo==5)) //SDO报文信息处理
{
M_Index=(Message->Data[2]<<16)+(Message->Data[1]<<8)+Message->Data[3];//SDO通讯下解析索引
switch(M_Index)
{
case MC_TimeGuard_Index: //节点保护时间
if(Motor[AxisNum].PcToM.NodeProtTime_Step<20)
Motor[AxisNum].PcToM.NodeProtTime_Step=20;
break;
case MC_TimeFactor_Index: //节点保护系数
if(Motor[AxisNum].PcToM.NodeProtTime_Step<40)
Motor[AxisNum].PcToM.NodeProtTime_Step=40;
break;
case MC_SetZero_Index:
if(Motor[AxisNum].PcToM.SetZero_Step<20)
Motor[AxisNum].PcToM.SetZero_Step=20;
break;
}
}
}