my_centrealign.c
3.71 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
#include "my_centrealign.h"
#include "hardware.h"
//u8 JudgeNearDir(double phi)//phi相机当前角度，返回临近的方向
//{
// u8 dir=0;
// if(phi>360-45||phi<=0+45)dir=1;else
// if(phi>90-45&&phi<=90+45)dir=2;else
// if(phi>180-45&&phi<=180+45)dir=3;else
// if(phi>270-45&&phi<=270+45)dir=4;
//
// return dir;
//}
//u8 alignLogicStep = 0;
//u8 alignActionStep = 0;
//u8 alignTimes = 0;
//u8 alignActionStep_t=0;
//u8 nextDir = 0;//接下来要转到的方向
//u8 edgeTrigger = 0;
//u8 alignLogic(double codex,double codey,double codep,double trayp)
//{
// u8 alignok = 0;
// static int countdelay=0;
//// static u8 alignActionStep_t=0;
//// static u8 nextDir = 0;//接下来要转到的方向
//// static u8 edgeTrigger = 0;
// switch(alignLogicStep)
// {
// case 1: nextDir = JudgeNearDir(codep);//判断最近的方向
// clri = 0;
// alignLogicStep++;
// break;
// case 2: if(Runclrok==1)//判断距离检测是否初始化
// {
// alignActionStep_t=Basic_Act_Num_Rotate;//旋转动作
// Judge.ouhe_EN=1;//一直耦合
// clri=1;//开启距离检测
// }
// Judge.rotdir=Judge_dir_rot(nextDir,codep);//判断旋转方向
// if(edgeTrigger==0)
// {
// Math.circle=Math_circle(nextDir,codep,Judge.ouhe_EN);//走弧的距离计算，一次
// edgeTrigger=1;
// }//计算旋转弧长
// speedrot(Math.circle,Judge.ouhe_EN,nextDir,codep);//旋转速度分析
// if(Judge.rotok==1)//旋转ok
// {
// rot_speed=0;
// alignLogicStep++;
// edgeTrigger=0;
// clri = 0;
// }
// break;
// case 3: alignActionStep_t=Basic_Act_Num_Stop;//停止动作
// OSTimeDly(300);
// if(Judge_arrive_rot(nextDir,codep,dataRotationAccuracy))
// {
// alignLogicStep++;
// }else
// {
// alignLogicStep=2;
// }
// break;
// case 4: alignActionStep_t=Basic_Act_Num_Yadjust;//y轴调整
// speedy(codey);
// if(Judge.yadjust==1)
// {
// clri=0;
// alignLogicStep++;
// }
// break;
// case 5: alignActionStep_t=Basic_Act_Num_Trayadjust;//托盘调整
// if(Judge_adjust_tray(trayp,dataTrayAdjustAccuracy))
// {
// alignLogicStep++;
// }
// break;
// case 6: alignActionStep_t=Basic_Act_Num_Stop;//停止动作
// if(countdelay<INT32_MAX)countdelay++;//不堵塞线程延时
// if(countdelay>100)
// {
// countdelay=0;
// if((fabs(codex)<=ykk&&fabs(codey)<=ykk)||alignTimes>5)//判断AGV是否在二维码的中心，或是对中次数到达5次
// {
// if(Judge_adjust_tray(trayp,dataTrayAdjustAccuracy))//判断AGV的托盘是否正
// {
// alignLogicStep=0;
// alignok=1;//在中心返回 ok
// }else
// {
// alignLogicStep = 5;//如果托盘不正，就重新调整一次
// }
// }else//对中结果不满足
// {
// alignTimes++;//计数1次
// nextDir = Judge_dir_vertical(codep);//判断垂直方向
// OSTimeDly(300);
// alignLogicStep=2;//重新对中
// }
// }
// break;
// default:break;
// }
//
//// dataToServer(2001,2,dataAgvName,CodeV,0);usart1_senddata(35,tcp_client_sendbuf1);
//// dataToServer(2002,3,dataAgvName,0,CodeX);usart1_senddata(35,tcp_client_sendbuf1);
//// dataToServer(2003,3,dataAgvName,0,CodeY);usart1_senddata(35,tcp_client_sendbuf1);
//// dataToServer(2004,3,dataAgvName,0,CodeP);usart1_senddata(35,tcp_client_sendbuf1);
//
// alignActionStep = alignActionStep_t;
// return alignok;
//}
//void alignAction(void)
//{
// switch(alignActionStep)
// {
// case Basic_Act_Num_Yadjust : adjust_y(yspeed);break; //微调
// case Basic_Act_Num_Rotate : AGV_rot_target(Judge.rotdir,dataCouplingCoefficient,rot_speed,Judge.ouhe_EN);break; //旋转
// case Basic_Act_Num_Trayadjust : adjust_tray(Rot_angle,dataTrayAdjustAccuracy);break; //转盘调整
// default:woodman(1,1,1);break;
// }
//}
//void alignLogicReset(void)
//{
// alignLogicStep = 1;
// alignActionStep = 0;
// alignTimes = 0;
//}