my_judge.c
2.66 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
#include "math.h"
#include "sys.h"
#include "my_math.h"
///**********************IO输入判断*********************/
//u8 Judge_io(u32 a,u8 b)//读取A中的第B位，然后放入一个字节的末位，这点和倍福的In口很像，也是一个byte变量的末位来辨别真假
//{
// u8 i;
// i=(a>>(b-1))&0x01;
// return i;
//}
///*****************车头方向判断************************/
//u8 Judge_dir_car(float a)
//{
// u8 i=0;
// if(a>=350||a<=10)i=1;else
// if(a>=80&&a<=100)i=2;else
// if(a>=170&&a<=190)i=3;else
// if(a>=260&&a<=280)i=4;
// return i;
//}
///***********************托盘方向判断**************************/
//u8 Judge_dir_tray(double a)//
//{
// u8 i=0;
// if(80<a&&a<100)i=2;else
// if(a<10||a>170)i=1;
// return i;
//}/*****************行走方向判断************************/
//u8 Judge_dir_run(int x1,int y1,int x2,int y2)//x1 y1第一个点坐标，x2 y2第二点坐标
//{
// u8 i=0;
// if(y1==y2&&x1<x2)i=1;else
// if(x1==x2&&y1<y2)i=2;else
// if(y1==y2&&x1>x2)i=3;else
// if(x1==x2&&y1>y2)i=4;else i=0;
// return i;
//}
///*****************垂直方向判断************************/
//u8 Judge_dir_vertical(float a)
//{
// u8 i;
// if(a>=0&&a<45)i=2;else
// if(a>=315)i=4;else
//
// if(a>=45&&a<90)i=1;else
// if(a>=90&&a<135)i=3;else
//
// if(a>=135&&a<180)i=2;else
// if(a>180&&a<225)i=4;else
//
// if(a>=225&&a<270)i=3;else
// if(a>=270&&a<315)i=1;else i=0;
// return i;
//}
///*****************旋转方向判断************************/
//u8 Judge_dir_rot(u8 a,float b)//a是目标方向，b当前方向;1是顺，2是逆
//{
// u8 i=0;
// float phi=0;
// if(a==0)
// {
// i=0;
// }else
// {
// phi=a*90-90;
// if(phi>b)
// {
// if(phi-b<=180)i=1;else
// if(phi-b>180)i=2;
// }else
// {
// if(b-phi<=180)i=2;else
// if(b-phi>180)i=1;
// }
// }
// return i;
//}
///*****************旋转到位判断************************/
//u8 Judge_arrive_rot(u8 a,float b,float c)//a是目标方向，b当前方向,c可容偏差;
//{
// u8 i=0;
// float phi=0;
// if(a==0)
// {
// i=1;
// }else
// {
// phi=a*90-90;
// if((phi==0||phi==360)&&(b<c||b>360-c))i=1;else
// if(b<phi+c&&b>phi-c)i=1;else i=0;
// }
// return i;
//}
///*****************托盘到位判断*************/
//u8 Judge_arrive_tray(double a,double b,double c)//a目标角度，b当前角度，c可容偏差
//{
// u8 i=0;
// if(fabs(b)>fabs(a)-c&&fabs(b)<fabs(a)+c)i=1;else i=0;
// return i;
//}
///*****************y轴调整判断************************/
//u8 Judge_adjust_y(float a,float b)//a是偏差值，b可容偏差
//{
// u8 i;
// if(a>=-b&&a<=b)i=1;else i=0;
// return i;
//}
///*****************托盘调整判断************************/
//u8 Judge_adjust_tray(float a,float b)//a 托盘的角度 b 可容偏差
//{
// u8 i=0;
// if(a>=0&&a<b)
// {
// i=1;
// }else
// if(a>(90-b)&&a<=90)
// {
// i=1;
// }
// return i;
//}