-
Notifications
You must be signed in to change notification settings - Fork 0
/
launcCalc
79 lines (51 loc) · 1.32 KB
/
launcCalc
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
import smbus #import SMBus module of I2C
from time import sleep #import
import time
from mpu6050 import mpu6050
import math
sensor = mpu6050(0x68)
accelerometer_data = sensor.get_accel_data()
gyro_data = sensor.get_gyro_data()
# find initial acceleration, set it as velocity
vx = 0
vy = 0
vz = 0
gdeltax = 0
gdeltay = 0
gdeltaz = 0
start_time = time.time()
seconds = 1
i = 0
last_time = 0
this_time = 0
while True:
dt = last_time - this_time
this_time = time.time()
current_time = time.time()
elapsed_time = current_time - start_time
gx, gy, gz, = gyro_data.values()
gdeltax = (gx * dt) + gdeltax
gdeltay = (gy * dt) + gdeltay
gdeltaz = (gz * dt) + gdeltaz
ax, ay, az = accelerometer_data.values()
vx = (ax * dt) + vx
vy = (ay * dt) + vy
vz = (az * dt) + vz
vnet = abs(math.sqrt(((vx)**2) + ((vy)**2) + ((vz)**2)))
gnet = abs(math.sqrt(((gdeltax)**2) + ((gdeltay)**2) + ((gdeltaz)**2)))
i += 1
last_time = time.time()
if elapsed_time > seconds:
print("Done")
break
print("vx :" + str(vx))
print("vy :" + str(vy))
print("vz :" + str(vz))
print("vnet :" + str(vnet))
print("gdeltax :" + str(gdeltax))
print("gdeltay :" + str(gdeltay))
print("gdeltaz :" + str(gdeltaz))
print("gdeltanet :" + str(gnet))
#print(i)
#print("dt :" + str(dt))
#print("elapsed time :" + str(elapsed_time))