getAccel.py
2.37 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
import CoDrone
import math
ac_x = 0
ac_y = 0
ac_z = 0
deltha_x = [0,0,0]
deltha_y = [0,0,0]
deltha_z = [0,0,0]
def map(x,input_min,input_max,output_min,output_max):
return (x-input_min)*(output_max-output_min)/(input_max-input_min)+output_min
def main():
drone = CoDrone.CoDrone()
drone.pair()
# print the acceleration of drone
while drone.isConnected():
normal_x = 0
normal_y = 0
normal_z = 0
for i in range(3):
deltha_x[i] = 0
deltha_y[i] = 0
deltha_z[i] = 0
for i in range(4):
acceleration = drone.get_accelerometer()
normal_x = 0
normal_y = 0
normal_z = 0
ac_x = acceleration.X
ac_y = acceleration.Y
ac_z = acceleration.Z
normal_x = map(ac_x, -32768,32767,-10000,10000)
normal_y = map(ac_y, -32768,32767,-10000,10000)
normal_z = map(ac_z, -32768,32767,-5000,5000)
deltha_x[1] = deltha_x[1]+(normal_x)
deltha_y[1] = deltha_y[1]+(normal_y)
deltha_z[1] = deltha_z[1]+(normal_z)
deltha_x[1] = int(deltha_x[1]/4)
deltha_y[1] = int(deltha_y[1]/4)
deltha_z[1] = int(deltha_z[1]/4)
for i in range(4):
acceleration = drone.get_accelerometer()
normal_x = 0
normal_y = 0
normal_z = 0
ac_x = acceleration.X
ac_y = acceleration.Y
ac_z = acceleration.Z
normal_x = map(ac_x, -32768,32767,-10000,10000)
normal_y = map(ac_y, -32768,32767,-10000,10000)
normal_z = map(ac_z, -32768,32767,-5000,5000)
deltha_x[2] = deltha_x[2]+(normal_x)
deltha_y[2] = deltha_y[2]+(normal_y)
deltha_z[2] = deltha_z[2]+(normal_z)
deltha_x[2] = int(deltha_x[2]/4)
deltha_y[2] = int(deltha_y[2]/4)
deltha_z[2] = int(deltha_z[2]/4)
deltha_x[0] = abs(deltha_x[1]-deltha_x[2])
deltha_y[0] = abs(deltha_y[1]-deltha_y[2])
deltha_z[0] = abs(deltha_z[1]-deltha_z[2])
deltha = deltha_x[0] + deltha_y[0] + deltha_z[0]
print("AcX : ", deltha_x[0], "AcY : ", deltha_y[0], "AcZ : ", deltha_z[0], "Total : ", deltha)
drone.close()
if __name__ == '__main__':
main()