getAccel.py
3.18 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
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
115
116
117
118
119
120
121
122
123
124
import CoDrone
import math
ac_x = 0
ac_y = 0
ac_z = 0
shock_level = 0
deltha_x = [0,0,0]
deltha_y = [0,0,0]
deltha_z = [0,0,0]
Emergency_value = 300
State_Parameter = False
shock_sum = 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 Emergency_state_():
print("Shock level : ", shock_level, "x : ", deltha_x[0], "y : ", deltha_y[0], "z : ", deltha_z[0], "vib : ", int(map(int(shock_level), 0, 15000, 0, 255)))
def main():
global State_Parameter
global shock_sum
global shock_level
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
State_Parameter = False
shock_level = 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(int(ac_x), -16384, 16383, -5000, 5000)
normal_y = map(int(ac_y), -16384, 16383, -5000, 5000)
normal_z = map(int(ac_z), -16384, 16383, -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(int(ac_x), -16384, 16383, -5000, 5000)
normal_y = map(int(ac_y), -16384, 16383, -5000, 5000)
normal_z = map(int(ac_z), -16384, 16383, -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]
if deltha > Emergency_value:
State_Parameter = True
shock_level = deltha
if State_Parameter == True:
Emergency_state_()
else:
print("no emergency\n")
if State_Parameter == True:
shock_sum += deltha
if State_Parameter == False & shock_sum != 0:
print("누적 충격값 : ", shock_sum)
shock_level = shock_sum
shock_sum = 0;
drone.close()
if __name__ == '__main__':
main()