ErrCourse.c
4.37 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
133
134
135
136
137
138
139
140
141
142
#include "includes.h"
#include "hardware.h"
#include "all_value.h"
int Err_sign[2];
int ErrEnd_sign[2];
void ErrAddClear(void)//错误存放清除
{ u8 i,p;
for(i=1;i<65;i++)
{
p=(i-1)/32;//前32个错误码 P=0;
if( VaIndexDW_R(&Err_sign[p],(i-1)%32) )
{
if( (AGV.Err[0]==i)|(AGV.Err[1]==i)|(AGV.Err[2]==i) ) {;}
else if(AGV.Err[0]==0)AGV.Err[0]=i;
else if(AGV.Err[1]==0)AGV.Err[1]=i;
else if(AGV.Err[2]==0)AGV.Err[2]=i;
}
else //无措，则对应缓存清除
{
if(AGV.Err[0]==i)AGV.Err[0]=0;
else if(AGV.Err[1]==i)AGV.Err[1]=0;
else if(AGV.Err[2]==i)AGV.Err[2]=0;
}
}
if(AGV.Err[0]==AGV.Err[1])AGV.Err[1]=0;
if((AGV.Err[1]==AGV.Err[2])|(AGV.Err[0]==AGV.Err[2]))AGV.Err[2]=0;
memcpy(ErrEnd_sign,Err_sign,8);
}
void Set_ErrID(u8 ID,u8 TF)//将错误号对应到位上
{
u8 p;
if(ID!=0)ID--;
p=ID/32;//前32个错误码 P=0;
VaIndexDW_W(&Err_sign[p],(ID-p*32),TF);
}
u8 Read_ErrID(u8 ID)//读取错误号对应位的状态
{
u8 p;
if(ID!=0)ID--;
p=ID/32;//前32个错误码 P=0;
return VaIndexDW_R(&ErrEnd_sign[p],(ID-p*32));
}
void ErrConfirm(void)
{
if(TLaRadarDelay_Time4Nums>200)Set_ErrID(14,1); //ErrID=14;激光感应障碍物
if(VaIndexW_R(&Motor[0].MToPc.StatusWord,3)&&Motor[0].Y)Set_ErrID(17,1); //ErrID=17;升降/下层输送电机报错
if( (Workmode==AutoMode)&&(AGV.Charge_Step==100)&&Out_Charge&& (BatteryVa.Current<5 &&BatteryVa.Energy<93) )Set_ErrID(19,1); //ErrID=19;充电异常
if(IN_EmergStop)Set_ErrID(20,1);//ErrID=20;急停键被按下
if( VaIndexDW_R(&F_AgvContr,17)&&(VaIndexW_R(&NodeOnLine,12)==0))Set_ErrID(21,1); //ErrID=21;测距高度数值异常
if(IN_Safe && VaIndexDW_R(&F_AgvContr,2))Set_ErrID(28,1); //ErrID=28;触边被触发
if(BatteryVa.Temperature_Max>65)Set_ErrID(32,1); //ErrID=32;电池温度异常 >65℃
if(VaIndexW_R(&Motor[1].MToPc.StatusWord,3)&&Motor[1].Y)Set_ErrID(40,1); //ErrID=40;1#舵轮行走轴报错
if(VaIndexW_R(&Motor[2].MToPc.StatusWord,3)&&Motor[2].Y)Set_ErrID(41,1); //ErrID=41;1#舵轮转向轴报错
if(VaIndexW_R(&Motor[3].MToPc.StatusWord,3)&&Motor[3].Y)Set_ErrID(42,1); //ErrID=42;2#舵轮行走轴报错
if(VaIndexW_R(&Motor[4].MToPc.StatusWord,3)&&Motor[4].Y)Set_ErrID(43,1); //ErrID=43;2#舵轮转向轴报错
if(VaIndexW_R(&Motor[5].MToPc.StatusWord,3)&&Motor[5].Y)Set_ErrID(30,1); //ErrID=43;5#轴报错
if( (Workmode==AutoMode)&&(Status_SlamLaser!=2))Set_ErrID(45,1); //ErrID=44;激光定位位置丢失
if(VaIndexB_R(&AGV.OtheSta,4))//有动作任务
{
if(AGV.Load_Step>2&&AGV.Load_Step<100 )
{
if( (CCD_M[0].Tim4_Usart56Nums>CcdMissTime) && VaIndexB_R(&AGV.OtheSta1,0) )
{ if(VaIndexB_R(&AGV.OtheSta1,2)){ Set_ErrID(6,1);}//下视觉读码异常
else
{ Out_Fan1Power=1;Out_Fan2Power=1;VaIndexB_W(&AGV.OtheSta1,2,1);}
}
if( (CCD_M[1].Tim4_Usart56Nums>CcdMissTime) && VaIndexB_R(&AGV.OtheSta1,1) )Set_ErrID(56,1);//上视觉读码异常
}
if( (AGV.AxisEn_Step>0 && AGV.AxisEn_Step<100)&&(AGV.AllAct_Time5Nums>120))Set_ErrID(57,1); //ErrID=57;相关轴使能复位未完成
if( (AGV.AxisEn_Step==100)&&(AGV.AllAct_Time5Nums>180)&&(AGV.Err[0]==0) )//无其他故障时，需要
{
if(AGV.UpDown_Step>1&& AGV.UpDown_Step<100){Set_ErrID(47,1);}//ErrID=47;升降超时
else if(AGV.Load_Step>1&& AGV.Load_Step<60){Set_ErrID(49,1);}//ErrID=49;CCD定位超时
else if(AGV.Load_Step>60&& AGV.Load_Step<100){Set_ErrID(48,1);}//ErrID=48;激光定位超时
else if(AGV.RotationCCD_Step>1&& AGV.RotationCCD_Step<100){Set_ErrID(50,1);}//ErrID=50;CCD旋转超时
else if(AGV.RotationSlam_Step>1&& AGV.RotationSlam_Step<100){Set_ErrID(51,1);}//ErrID=51;激光旋转超时
else if(AGV.Charge_Step>1&& AGV.Charge_Step<100){Set_ErrID(52,1);}//ErrID=52;充电超时
else if(AGV.Conveyor_Step>1&& AGV.Conveyor_Step<100)
{
//ConyUpTy:1-负速度卸料;2-负速度接料;3-正速度卸料;4-正速度接料;
if((Unionu8_1.ConyUpTy%2)==1)Set_ErrID(35,1);//ErrID=35;上层卸料超时
else if(Unionu8_1.ConyUpTy)Set_ErrID(36,1);//ErrID=36;上层接料超时
if((Unionu8_2.ConyDownTy%2)==1)Set_ErrID(37,1);//ErrID=37;下层卸料超时
else if(Unionu8_2.ConyDownTy)Set_ErrID(38,1);//ErrID=38;下层接料超时
}//ErrID=53;输送超时
}
}
if( (VaIndexB_R(&AGV.AllAxisOK,4)|IN_Stop)==0)Set_ErrID(54,1); //ErrID=54;有轴离线
}
void Err_Pro(void)//
{ if(Electrify_Step==100)
{ ErrConfirm();
ErrAddClear();
}
}
void Err_Clear(u8 No)
{ u8 i;
for(i=35;i<53;i++)//35-38;47-52
{ if(i==39)i=47;
Set_ErrID(i,0);
}
if(No==1)
{
Set_ErrID(56,0);Set_ErrID(6,0);Set_ErrID(46,0);Set_ErrID(39,0);Set_ErrID(53,0);
}
}