all_value.c
1.83 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
#include "hardware.h"
#include "all_value.h"
u8 F_WheelTy;//轮系0-差速；1-舵轮
u16 Tim5_SystemTimeNums;
//float PulseAng_Helm;
//float Pulsemm_Helm;
float F_XDis_Helm;//舵轮中心在车体坐标系下的X方向的距离
float F_YDis_Helm;//舵轮中心在车体坐标系下的Y方向的距离
float F_DX_ScanLaInstall;//雷达中心相对车中心的位置X
float F_DY_ScanLaInstall;//雷达中心相对车中心的位置Y
float Dis_AgvC_Helm;//舵轮中心距离车中心的距离
float Dis_LaToAgvInStall;//雷达中心相对车中心距离
float R_DisMin;//最小半径
float AngCw_Helm;//旋转时的舵轮转角度
short F_Ang_HelmZerToAgvX;//舵轮的安装偏角
float Voltage1;//采集的电压
float Voltage2;//采集的电压
u8 Tim3_XCNums[1];
Union_u8 Unionu8_1;
Union_u8 Unionu8_2;
Union_u16 Union16_1;
u8 Ceshi[30] ;
u8 Electrify_En;
char F_Agvname[8];//AGV名称 A02_0112
void Value1Init(void)//参数初始化1
{
u8 i;
for(i=0;i<MaxMotNums;i++)
{
memset(&Motor[i].MToPc,0,sizeof(Motor[i].MToPc));
memset(&Motor[i].PcToM,0,sizeof(Motor[i].PcToM));
Motor[i].PcToM.Acc=2000;
Motor[i].PcToM.Dcc=1500;
if(Motor[i].Y)VaIndexB_W(&AGV.Axis_No,i,1);
}
NodeOnLine=0;
Electrify_Step=0;
MessSpecialClear();
Value2Init();
Tim5_SystemTimeNums=0;
}
void Value2Init(void)//参数初始化2
{ u8 i;
if(F_YDis_Helm!=0)AngCw_Helm=atan(F_XDis_Helm/F_YDis_Helm)*Du_Pi;
Dis_AgvC_Helm=DisK_AgvC_Helm*sqrt( pow(F_XDis_Helm,2)+pow(F_YDis_Helm,2) );
Dis_LaToAgvInStall=sqrt( pow(F_DX_ScanLaInstall,2)+pow(F_DY_ScanLaInstall,2) );
for(i=0;i<MaxMotNums;i++)
{ if(i>2&&i<5)
{
Motor[i].PulseS_Unit=Motor[i-2].PulseS_Unit;
Motor[i].Max_rpm=Motor[i-2].Max_rpm;
}
if(Motor[i].PulseS_Unit!=0)
Motor[i].Max_Speed=(float)Motor[i].Max_rpm/60.0f*(float)PulseS_Circle/(float)Motor[i].PulseS_Unit;
}
if(F_DirInstall_CCD>1&&F_DirInstall_CCD<5)AngInstall_CCD=90.0f*(F_DirInstall_CCD-2);
else AngInstall_CCD=270.0f;
}