forked from HerrRiebmann/Caravan_Leveler
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Level.ino
52 lines (46 loc) · 1.15 KB
/
Level.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
void getLevel() {
if (!accelInitialized)
return;
if(valuationActive){
Valutation();
return;
}
int x = accel.getX();
int y = accel.getY();
int z = accel.getZ();
//-90 -> +90
//-432 -> 109 / -287 -> 247
//541 / 533
//Divide by 2
//Zero
//-150 / -25
levelX = map(x, (valuationX - calibrationX) * -1, (valuationX + calibrationX), -900, 900);
levelY = map(y, (valuationY - calibrationY) * -1, (valuationY + calibrationY), -900, 900);
Serial.print("X: ");
Serial.print(levelX);
Serial.print(" Y: ");
Serial.print(levelY);
Serial.print("\t(");
Serial.print(x);
Serial.print(" / ");
Serial.print(y);
Serial.print(" / ");
Serial.print(z);
Serial.print(")");
levelX = deepPassX(levelX);
levelY = deepPassY(levelY);
Serial.print("\tDeepPass ");
Serial.print(levelX);
Serial.print(" / ");
Serial.println(levelY);
}
void CalibrateLevel() {
//Get current values, store as "Zero-levelled" and write to eeprom
calibrationX = accel.getX();
calibrationY = accel.getY();
Serial.print("Calibrated to X: ");
Serial.print(calibrationX);
Serial.print(" Y: ");
Serial.println(calibrationY);
StoreLevel();
}