-
Notifications
You must be signed in to change notification settings - Fork 0
/
util.ino
88 lines (85 loc) · 1.41 KB
/
util.ino
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
int exptoservo(float x)
{
if (x < (DEADPER * 1023.0))
{
return CSERVOMIN;
}
else if (x < 1023.0 - (DEADPER * 1023.0))
{
float linx = x * (1.0 + (float) DEADPER);
linx /= 1023.0;
float expn = (-1.0/exp(linx*EXPFACTOR)+1.0)/(1.0-1.0/exp(EXPFACTOR));
float frac = ((1 - DIV) * linx) + ((DIV) * expn);
return frac * (CSERVOMAX - CSERVODEAD) + CSERVODEAD;
}
else
{
return CSERVOMAX;
}
}
int tocfake(int x)
{
int chosenmax = 0;
if (MAX > MAX2 && MIN < MAX && MIN > MAX2)
{
if (x > MIN)
{
chosenmax = MAX;
}
else if (x < MIN)
{
chosenmax = MAX2;
}
else
{
return CLUTCHFAKEMIN;
}
}
else if (MAX < MAX2 && MIN > MAX && MIN < MAX2)
{
if (x < MIN)
{
chosenmax = MAX;
}
else if (x > MIN)
{
chosenmax = MAX2;
}
else
{
return CLUTCHFAKEMIN;
}
}
else if (MAX == MAX2)
{
if (MIN != MAX)
{
chosenmax = MAX;
}
else
{
return CLUTCHFAKEMAX;
}
}
else
{
return CLUTCHFAKEMAX;
}
return ((float) x - (float) MIN) * (1023.0 / ((float)chosenmax - (float)MIN));
}
void cleardats()
{
Serial.println("Please confirm you want to erase stored data");
while (Serial.available() == 0)
{
delay(10);
}
char in = Serial.read();
if (in == '!')
{
EEPROM.put(addr_WRITTEN, 0);
Serial.println("Done");
reset();
}
return;
}