Charge.c
2.29 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
#include "hardware.h"
#include "all_value.h"
/*********************************************************************************
函数名：
功能：
参数：
返回：无返回。
实现方法：
编程：pcy
日期：2021-12-16
*********************************************************************************/
u8 F_CharDir;//充电口方向
void Charge_Pro(void)
{
signed char Dir;
switch(AGV.Charge_Step)
{
case 1:
AGV.AxisEn_Step=1;
AGV.Axis_No=0x3E;
AGV.Charge_Step++;
break;
case 2://舵轮组都已使能
if (AGV.AxisEn_Step==100)AGV.Charge_Step=3;
break;
case 3://若是取消充电 操作接触器
if (AGV.Type3_Ac==6) AGV.Charge_Step=20;//6->充电；7->取消充电；
else
{ Out_Charge=0;
AGV.Charge_Step=4;
}
AGV.AllAct_Time5Nums=0;
break;
case 4://操作接触器
if(AGV.AllAct_Time5Nums>2) AGV.Charge_Step=20;
break;
case 20://根据运行方向，调整舵轮角度
if( (F_CharDir==2)|(F_CharDir==4) )
{ Motor[2].PcToM.Angle_Target=Motor[4].PcToM.Angle_Target=90;}
else
{ Motor[2].PcToM.Angle_Target=Motor[4].PcToM.Angle_Target=0;}
if (AGV.Type3_Ac==6)
{
if( (F_CharDir==1)|(F_CharDir==2) ) Dir=1;
else Dir=-1;
}
else
{ if ((F_CharDir==1)|(F_CharDir==2) ) Dir=-1;
else Dir=1;
}
Motor[1].PcToM.Angle_Target=Motor[3].PcToM.Angle_Target=F_Un.Dis_Char_T*(float)Dir;
AGV.Charge_Step++;
break;
case 21://启动
AGV.GoDis_Step=1;
AGV.Charge_Step=31;
break;
case 31://判断是否完成
if( AGV.GoDis_Step==100 ){ AGV.Charge_Step++;AGV.AllAct_Time5Nums=0;}
break;
case 32://操作接触器
if (AGV.Type3_Ac==6)
{ if(AGV.AllAct_Time5Nums>1)
{
Out_Charge=1;
AGV.Charge_Step++;
}
}
else AGV.Charge_Step=90;
break;
case 33://充电接触吸合延时
AGV.AllAct_Time5Nums=0;
AGV.Charge_Step++;
break;
case 34://充电接触吸合延时
if(AGV.AllAct_Time5Nums>7)
{
AGV.Charge_Step=90;
}
break;
case 90://
AGV.Charge_Step=100;
break;
case 100://完成
break;
}
if(AGV.Charge_Step==3)
{ if(AGV.Type3_Ac==6)
{ AGV.DirGo=F_CharDir;}
else
{ AGV.DirGo=F_CharDir+2;
if(AGV.DirGo>4)AGV.DirGo-=4;
}
}
}