my_math.c
3.32 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
#include "hardware.h"
#include "all_value.h"
////PID 用于方向控制
//double SetDir=0;
//double ActualDir=0;
//double piderr=0;
//double piderr_next=0;
//double piderr_last=0;
////float Kp=8,Ki=0.00,Kd=0.016;
//double PID_realize(double dir,double Actualvalue)
//{
// double incrementDir;
//// Kp=10,Ki=0.00,Kd=0.016;
// SetDir=dir;
// piderr=SetDir-Actualvalue;
// incrementDir=dataRunKp*(piderr-piderr_next)+dataRunKi*piderr+dataRunKd*(piderr-2*piderr_next+piderr_last);
// ActualDir+=incrementDir;
// piderr_last=piderr_next;
// piderr_next=piderr;
// return ActualDir;
//}
//void PID_clear(void)
//{
// SetDir=0;
// ActualDir=0;
// piderr=0;
// piderr_next=0;
// piderr_last=0;
//}
////u16 Math_ascll(u16 ascll)
////{
//// u16 num=0;
//// num=ascll-0x30;
//// if(ascll==0x20)num=0;
//// return num;
////}
////u32 Math_power10(u8 power)
////{
//// int i;
//// int sum=1;
//// for(i=0;i<power;i++)
//// sum*=10;
//// return sum;
////}
////u16 Math_power16(u8 power)
////{
//// int i;
//// int sum=1;
//// for(i=0;i<power;i++)
//// sum*=16;
//// return sum;
////}
//u16 Math_crc(u16 crc_a,u16 crc_b)//CRC校验 按位亦或
//{
// u8 crc;
// crc=crc_a^crc_b;
// return crc;
//}
////double Math_abs(double num)//计算绝对值
////{
//// double abs;
//// if(num<0) abs=(0-num);
//// else abs=num;
//// return abs;
////}
////double Math_square(double num)//计算平方
////{
//// double square;
//// square=num*num;
//// return square;
////}
//double Math_remainder(double a,int b)//浮点型求余函数
//{
// double num=0;
// long long a_int=0;
// double a_float=0;
// long long num_int=0;
// double num_float=0;
// a_int = a/1;
// a_float = a-a_int;
// num_int = a_int%(int)b;
// num_float = a_float;
// num = num_int+num_float;
// return num;
//}
//double Math_function(double x,double k1,double k2,double k3,double k4,double k5,double k6)//五阶多项式函数，自变量x，k1
//{
// double num;
// num=k1*(x*x*x*x*x)+k2*(x*x*x*x)+k3*(x*x*x)+k4*(x*x)+k5*(x)+k6;
// return num;
//}
////double Math_integral(double x,double k1,double k2,double k3,double k4,double k5,double k6)
////{
//// double num;
//// num=(k1/6.0)*(x*x*x*x*x*x)+(k2/5.0)*(x*x*x*x*x)+(k3/4.0)*(x*x*x*x)+(k4/3.0)*(x*x*x)+(k5/2.0)*(x*x)+k6*x;
//// return num;
////}
////int Math_integer(double num)//四舍五入的取整
////{
//// int a=0;
//// int i;
//// a=(int)num;
//// if(num<(a+0.5f))i=a;else
//// if(num>=(a+0.5f))i=a+1;
//// return i;
////}
//double Math_angle(u8 a,double b) //获得小车旋转角度,a目标方向，b当前方向
//{
// double i=0;
// double phi=a*90-90;
// double A=0;
// if(phi>b)
// {
// A=phi-b;
// if(A<=180){i=A;}
// if(A>180){i=360-A;}
// }
// if(phi<=b)
// {
// A=b-phi;
// if(A<=180){i=A;}
// if(A>180){i=360-A;}
// }
// return i;
//}
//double Math_circle(u8 a,double b,u8 c) //计算小车旋转圆弧周长,a目标方向，b当前方向,c是否耦合
//{
// double i=0;
// double j=0;
// j=Math_angle(a,b);
// i=(double)dataWheelsDistance*PI*j/360;//Wheeldis
////#if (AGV_ROT_MODE == 0u)//条件编译，系统选择耦合以驱动轮为主动托盘为从动的模式
// return i;
////#else //条件编译，系统选择耦合以托盘为主动轮子为从动的模式
////
////
//// if(c==1)return j;else return i; //如果耦合返回托盘需旋转的角度，如果不耦合返回小车旋转轮子需行走的距离
////#endif
//}
//double Math_trayphi(u8 a)//计算托盘旋转角度 a上位机所给的托盘属性
//{
// double i=0;
// if(a==0)
// {
// i=0;
// }else
// {
// i=(a-5)*90;
// }
// return i;
//}