UpDown.c
4.58 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
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
#include "hardware.h"
#include "all_value.h"
/*********************************************************************************
函数名：
功能：
参数：
返回：无返回。
实现方法：
编程：pcy
日期：2021-12-16
*********************************************************************************/
# define Dis1_Hight 40.0f
# define Dis2_Hight 500.0f
# define Override1_UD 12.0f //Dis1速度
# define Override2_UD 80.0f //上升速度
u8 F_OverrideUP;//上升速度 百分比
u8 F_OverrideDown;//下降速度 百分比
u8 ToSetHeight_Fc(float *Height)//升降到达目标高度的控制函数
{
float AutoOverride_UD;
float HighDelta;
HighDelta=fabs(High_Dis-(*Height));
if ( VaIndexW_R(&NodeOnLine,12) | ( ((*Height)<Highmin)|((*Height)>HighMax) ) )
{ Fc_Matrix_Agv(0,0,0,0,0,0);}
if (HighDelta<1.0f) //到达设定高度
{ Fc_Matrix_Agv(0,0,0,0,0,0);
Out_DownValve=0;
if (Motor[0].MToPc.Tim4_AxisDelayNum>15)return 1;//0.2s
else return 0;
}
else//根据升降距离差确定速度
{ Motor[0].MToPc.Tim4_AxisDelayNum=0;
if(VaIndexDW_R(&F_AgvContr,1))//液压
{
if (HighDelta<Dis1_Hight)AutoOverride_UD=F_OverrideUP/3;
else AutoOverride_UD=F_OverrideUP;
}
else
{ if (HighDelta<4)AutoOverride_UD=4;
else if (HighDelta<Dis1_Hight)AutoOverride_UD=Override1_UD;
else if (HighDelta<Dis2_Hight)AutoOverride_UD=(HighDelta-Dis1_Hight)*((float)F_OverrideUP-Override1_UD)/(Dis2_Hight-Dis1_Hight)+Override1_UD;
else AutoOverride_UD=(float)F_OverrideUP;
}
if (High_Dis<(*Height)) //低于目标高度-上升
{ Out_DownValve=0;
if (IN_UpLimit){AutoOverride_UD=0;}
}
else //高于目标高度-下降
{
if(VaIndexDW_R(&F_AgvContr,1))//液压
{ if (IN_DownLimit){Out_DownValve=0;}
else Out_DownValve=1;
AutoOverride_UD=0;
}
else
{ if (IN_DownLimit){AutoOverride_UD=0;}
else AutoOverride_UD=AutoOverride_UD*(-1);
}
}
Fc_Matrix_Agv(0,0,0,0,AutoOverride_UD*0.01f*Motor[0].Max_Speed,0);
return 0;
}
}
void UpDown_Pro(void)//获取工作模式
{
// Up_Pro();
// Down_Pro();
switch(AGV.UpDown_Step)
{
case 1: //先判断轴无故障
AGV.AxisEn_Step=1;
AGV.Axis_No=0x01;
AGV.UpDown_Step++;
break;
case 2://给出使能指令
if (AGV.AxisEn_Step==100)AGV.UpDown_Step=5;
Motor[0].PcToM.Acc=1500;
Motor[0].PcToM.Dcc=1500; //避免停止时过大晃动
break;
case 5: //判断升降类型
if(AGV.Type3_Ac==2)AGV.UpDown_Step=40;//到最高机械位
else if(AGV.Type3_Ac==3)AGV.UpDown_Step=50;//到最低机械位
else if(AGV.Type3_Ac==4)AGV.UpDown_Step=10; //到指定的设定位置
else if(AGV.Type3_Ac==5)AGV.UpDown_Step=20; //信号交互确定升降高低位置
break;
case 10: //到指定的设定位置
if (F_Un.Dis_Height_T>=Highmin && F_Un.Dis_Height_T<=HighMax)AGV.UpDown_Step=30;
break;
case 20: //信号交互确定升降高低位置
AGV.UpDown_Step++;
VaIndexB_W(&AGV.OtheSta2,1,0);
break;
case 21: //获取到最新的料台高度信息
if(VaIndexB_R(&AGV.OtheSta2,1))
AGV.UpDown_Step++;
break;
case 22: //计算高度
break;
case 30://升降到指定高度动作
if (ToSetHeight_Fc(&F_Un.Dis_Height_T))AGV.UpDown_Step=80;
break;
case 40://到最高机械位
AGV.UpDown_Step++;
break;
case 41:
if (IN_UpLimit)
{
Fc_Matrix_Agv(0,0,0,0,0,0);
if (Motor[0].MToPc.Tim4_AxisDelayNum>2)AGV.UpDown_Step=80;;
}
else
{ Motor[0].MToPc.Tim4_AxisDelayNum=0;
Fc_Matrix_Agv(0,0,0,0,(float)F_OverrideUP*0.01f*Motor[0].Max_Speed,0);
Motor[0].PcToM.Acc=2000;
Motor[0].PcToM.Dcc=1500; //避免停止时过大晃动
}
break;
case 50://到最低机械位
AGV.UpDown_Step++;
break;
case 51:
if(VaIndexDW_R(&F_AgvContr,1))//液压
{ if (IN_DownLimit){Out_DownValve=0;AGV.UpDown_Step=80;}
else Out_DownValve=1;
Fc_Matrix_Agv(0,0,0,0,0,0);}
else
{ if (IN_DownLimit)
{
Fc_Matrix_Agv(0,0,0,0,0,0);
if (Motor[0].MToPc.Tim4_AxisDelayNum>2)AGV.UpDown_Step=80;;
}
else
{ Motor[0].MToPc.Tim4_AxisDelayNum=0;
Fc_Matrix_Agv(0,0,0,0,-(float)F_OverrideDown*0.01f*Motor[0].Max_Speed,0);
Motor[0].PcToM.Acc=2000;
Motor[0].PcToM.Dcc=1500;
}
}
break;
case 80://
Motor[0].MToPc.Tim4_AxisDelayNum=0;
AGV.UpDown_Step++;
break;
case 81://
if(Motor[0].MToPc.Tim4_AxisDelayNum>10)
{ if(VaIndexDW_R(&F_AgvContr,16)|VaIndexDW_R(&F_AgvContr,1))Motor[0].PcToM.PowerOn=0;
AGV.UpDown_Step=90;
}
break;
case 90://
AGV.UpDown_Step=100;
break;
case 100://完成动作
break;
}
}