-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathalgorithm.c
114 lines (106 loc) · 2.7 KB
/
algorithm.c
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
#include"algorithm.h"
#include"main.h"
#include <stdio.h>
//int16_t value_last=0;
//int8_t value_state_n=0;
//int8_t value_state_l=0;
//int8_t value_state_storage[3]={0};
//int8_t act=0;
//int value_count=0;
//uint8_t value_state_valid=0;
int value_last=0;
int value_state_n=0;
int value_state_l=0;
int value_state_storage[3]={0};
int value_count=0;
int value_state_valid=0;
int act_storage=0;
void qvar_digital(int16_t value,int * act_output)
{
int value_deviation=value-value_last;
int act=0;
if (value_deviation>=54600)
value_state_n=1;
else if(value_deviation>=23400)
value_state_n=value_state_l+1;
else if(value_deviation<=-54600)
value_state_n=-1;
else if(value_deviation<=-23400)
value_state_n=value_state_l-1;
else
value_state_n=value_state_l;
value_last= value_state_n * 32760;
if (value_state_n==value_state_l)
{
value_count+=1;
if ((value_count>=20)&&(value_state_n!=0)&&(value_state_n!=value_state_storage[0]))
{
value_count=0;
value_state_valid=1;
value_state_storage[2]=value_state_storage[1];
value_state_storage[1]=value_state_storage[0];
value_state_storage[0]=value_state_n;
}
else if((value_count>=25)&&(value_state_n==0))
{
value_count=0;
value_state_valid=1;
value_state_storage[2]=value_state_storage[1];
value_state_storage[1]=value_state_storage[0];
value_state_storage[0]=value_state_n;
}
}
else
value_count=0;
if (value_state_valid)
{
value_state_valid=0;
if ((value_state_storage[2]==0)&&(value_state_storage[0]==0)&&(act_storage==0))
{
act_storage=value_state_storage[1];
act=0;
}
else if((value_state_storage[1]==-1)&&(value_state_storage[2]==1))
{
act=-1;
act_storage=0;
}
else if((value_state_storage[1]==1)&&(value_state_storage[2]==-1))
{
act=1;
act_storage=0;
}
else if((value_state_storage[2]==act_storage)&&(value_state_storage[1]==0)&&(value_state_storage[0]==0))
{
act=act_storage+act_storage;
act_storage=0;
}
else if((value_state_storage[2]*2==act_storage)&&(value_state_storage[1]==0)&&(value_state_storage[0]==0))
{
act=act_storage+act_storage;
act_storage=0;
}
else if((value_state_storage[0]==value_state_storage[2])&&(value_state_storage[0]==act_storage)&&(act_storage!=0)&&(value_state_storage[1]==0))
{
act_storage=act_storage+act_storage;
}
else if((value_state_storage[2]==0)&&(value_state_storage[0]==0)&&(act_storage!=0))
{
;
}
else
{
act=0;
act_storage=0;
}
}
else
act=0;
value_state_l=value_state_n;
// if((act!=2)&&(act!=-2))
// {
*act_output=act;
// *act_output=act_storage;
// }
// return value_last;
}