CCD.c
3.81 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
#include "includes.h"
#include "hardware.h"
#include "all_value.h"
u8 F_DirInstall_CCD;//相机安装方向
float AngInstall_CCD;
short DeltaY_LTAdjust;//微调量
short DeltaX_LTAdjust;
Str_CCD CCD_M[2];
Str_AdjustP P_Adjust[2];
u8 USART6_RX_BUF[Length_Usart6_RX];
u8 USART6_RX_CNT=0;
void USART6_IRQHandler(void)//串口6中断服务程序
{
u8 res;
OSIntEnter();
if(USART_GetITStatus(USART6, USART_IT_RXNE) != RESET)
{
res=USART_ReceiveData(USART6);
if(USART6_RX_CNT<Length_Usart6_RX)
{
USART6_RX_BUF[USART6_RX_CNT]=res; //记录接收到的值
if(USART6_RX_BUF[0]==0xAA){ CCD_M[0].Tim4_Usart56Nums=0;USART6_RX_CNT++;}
}
}
USART_ClearITPendingBit(USART6,USART_IT_RXNE);
OSIntExit();
}
void CCD_Data_Analysis(u8 No,u8*Buffer)//相机数据解析
{
u16 Head_Buff;
u16 Va16;
short Va16f;
float Dx1,Dy1;
CCD_M[No].CodeOn=0;
if( ((USART5_RX_CNT==Length_Usart5_RX)&& (No==1))|
((USART6_RX_CNT==Length_Usart6_RX)&& (No==0)) )
{
memcpy(&Head_Buff,Buffer,2);
if( (Head_Buff==Head_CCD) && (Buffer[14]==Sum_Crc(Buffer,14,0)) )
{
memcpy(&(CCD_M[No].T),Buffer+3,1);//相机信息处理时间
memcpy(&(CCD_M[No].Value),Buffer+4,4); //码值
memcpy(&Va16f,Buffer+8,2);//相机原始DX信息
Dx1=(float)Va16f*DisXPixel_CCD;
memcpy(&Va16f,Buffer+10,2);//相机原始DY信息
Dy1=(float)Va16f*DisYPixel_CCD;
if(No){ P_Adjust[No].X_Delta=0;P_Adjust[No].Y_Delta=0;DeltaX_LTAdjust=0;DeltaY_LTAdjust=0;}
CCD_M[No].X=(short)((-Dy1*cos(AngInstall_CCD*Pi_Du)+Dx1*sin(AngInstall_CCD*Pi_Du)-(float)CCD_M[No].X_Delta-P_Adjust[No].X_Delta+(float)DeltaX_LTAdjust)*10);//X位置单位0.1mm
CCD_M[No].Y=(short)((-Dx1*cos(AngInstall_CCD*Pi_Du)-Dy1*sin(AngInstall_CCD*Pi_Du)-(float)CCD_M[No].Y_Delta-P_Adjust[No].Y_Delta+(float)DeltaY_LTAdjust)*10);//Y位置单位0.1mm
memcpy(&Va16,Buffer+12,2);
CCD_M[No].Ang=(float)(Va16)*0.01f-(float)AngInstall_CCD-(float)CCD_M[No].Ang_Delta*0.1f-(float)P_Adjust[No].Ang_Delta*0.1f;//角度=相机角度+起始方位角+安装角度偏差
if(VaIndexDW_R(&F_AgvContr,11)&& No)
{ CCD_M[No].X=0-CCD_M[No].X;
}
AngDeal(&CCD_M[No].Ang);
CCD_M[No].CodeOn=1;
memset(Buffer,0,1);
}
if (No<2)
{ if (No==1)USART5_RX_CNT=0;//上相机
else USART6_RX_CNT=0;//下相机
CCD_M[No].Tim4_Usart56Nums=0;VaIndexB_W(&AGV.OtheSta1,No,0);
}
}
}
u8 TIM5_FanNums;
void FanAction_Pro(void) //风扇控制函数直接操控IO
{
if ( (Out_Fan1Power|Out_Fan2Power)==0 ){TIM5_FanNums=0;}
else
{
if (TIM5_FanNums>=10){Out_Fan1Power=Out_Fan2Power=0;}
}
}
u8 CCDVaInitCalEn=1;
void CCD_Pro(void)//相机
{
if (CCDVaInitCalEn)//第一次计算下位置相对信息
{
AdjustPVal_Fc();
CCDVaInitCalEn=0;
CampareAdjustPEn=0;
}
AdjustPChoice_Fc();
CCD_Data_Analysis(0,USART6_RX_BUF);//中间相机信息解析
if (CCD_M[0].Tim4_Usart56Nums>30)VaIndexW_W(&NodeOnLine,9,0);else VaIndexW_W(&NodeOnLine,9,1);
FanAction_Pro();
}
Str_AdjustP F_AdjustPArr[AdjustPAllNums];
void AdjustPVal_Fc(void)//调节点偏差
{ u8 i;
i=0;
F_AdjustPArr[i].Value=10;
F_AdjustPArr[i].X_Delta=0;
F_AdjustPArr[i].Y_Delta=0;
F_AdjustPArr[i].Ang_Delta=0;
}
u8 CampareAdjustPEn;
void AdjustPChoice_Fc(void)//调节点偏差选取
{ u8 i;
if(CampareAdjustPEn)
{ for(i=0;i<AdjustPAllNums;i++)
{
if( CCD_M[0].Value==F_AdjustPArr[i].Value)
{
P_Adjust[0].X_Delta=F_AdjustPArr[i].X_Delta;//位置调整到位后相机X位置反馈
P_Adjust[0].Y_Delta=F_AdjustPArr[i].Y_Delta;//位置调整到位后相机Y位置反馈
P_Adjust[0].Ang_Delta=F_AdjustPArr[i].Ang_Delta;//=(位置调整到位后相机实际角度反馈-期望角度)*10
break;
}
if( i==(AdjustPAllNums-1) )//若未比对成功则偏差清零
{
memset(&P_Adjust,0,sizeof(P_Adjust));
}
}
}
else
{ memset(&P_Adjust,0,sizeof(P_Adjust));
DeltaX_LTAdjust=0;
DeltaY_LTAdjust=0;
}
if (IN_Stop) CampareAdjustPEn=0;
}