2021-1-capstone-design2 / 2014104077

Go to a project
Toggle navigation Toggle navigation pinning
Wonseok Kwak
1ccdc661 1 parent 7ff9bc00

Update : IMU Serial logging 및 monitoring을 위한 소스

Inline Side-by-side
Showing 1 changed file with 44 additions and 7 deletions
...@@ -4,16 +4,27 @@ import math ...@@ -4,16 +4,27 @@ import math
4 ac_x = 0 4 ac_x = 0
5 ac_y = 0 5 ac_y = 0
6 ac_z = 0 6 ac_z = 0
7 +shock_level = 0
7 deltha_x = [0,0,0] 8 deltha_x = [0,0,0]
8 deltha_y = [0,0,0] 9 deltha_y = [0,0,0]
9 deltha_z = [0,0,0] 10 deltha_z = [0,0,0]
11 +Emergency_value = 300
12 +State_Parameter = False
13 +shock_sum = 0
10 14
11 def map(x,input_min,input_max,output_min,output_max): 15 def map(x,input_min,input_max,output_min,output_max):
12 return (x-input_min)*(output_max-output_min)/(input_max-input_min)+output_min 16 return (x-input_min)*(output_max-output_min)/(input_max-input_min)+output_min
13 17
18 +def Emergency_state_():
19 + 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)))
20 +
21 +
14 22
15 23
16 def main(): 24 def main():
25 + global State_Parameter
26 + global shock_sum
27 + global shock_level
17 drone = CoDrone.CoDrone() 28 drone = CoDrone.CoDrone()
18 drone.pair() 29 drone.pair()
19 30
...@@ -30,6 +41,10 @@ def main(): ...@@ -30,6 +41,10 @@ def main():
30 41
31 42
32 43
44 +
45 +
46 + State_Parameter = False
47 + shock_level = 0
33 for i in range(4): 48 for i in range(4):
34 acceleration = drone.get_accelerometer() 49 acceleration = drone.get_accelerometer()
35 normal_x = 0 50 normal_x = 0
...@@ -39,9 +54,9 @@ def main(): ...@@ -39,9 +54,9 @@ def main():
39 ac_y = acceleration.Y 54 ac_y = acceleration.Y
40 ac_z = acceleration.Z 55 ac_z = acceleration.Z
41 56
42 - normal_x = map(ac_x, -32768,32767,-10000,10000) 57 + normal_x = map(int(ac_x), -16384, 16383, -5000, 5000)
43 - normal_y = map(ac_y, -32768,32767,-10000,10000) 58 + normal_y = map(int(ac_y), -16384, 16383, -5000, 5000)
44 - normal_z = map(ac_z, -32768,32767,-5000,5000) 59 + normal_z = map(int(ac_z), -16384, 16383, -5000, 5000)
45 60
46 61
47 deltha_x[1] = deltha_x[1]+(normal_x) 62 deltha_x[1] = deltha_x[1]+(normal_x)
...@@ -63,9 +78,9 @@ def main(): ...@@ -63,9 +78,9 @@ def main():
63 ac_y = acceleration.Y 78 ac_y = acceleration.Y
64 ac_z = acceleration.Z 79 ac_z = acceleration.Z
65 80
66 - normal_x = map(ac_x, -32768,32767,-10000,10000) 81 + normal_x = map(int(ac_x), -16384, 16383, -5000, 5000)
67 - normal_y = map(ac_y, -32768,32767,-10000,10000) 82 + normal_y = map(int(ac_y), -16384, 16383, -5000, 5000)
68 - normal_z = map(ac_z, -32768,32767,-5000,5000) 83 + normal_z = map(int(ac_z), -16384, 16383, -5000, 5000)
69 84
70 85
71 deltha_x[2] = deltha_x[2]+(normal_x) 86 deltha_x[2] = deltha_x[2]+(normal_x)
...@@ -80,7 +95,29 @@ def main(): ...@@ -80,7 +95,29 @@ def main():
80 deltha_y[0] = abs(deltha_y[1]-deltha_y[2]) 95 deltha_y[0] = abs(deltha_y[1]-deltha_y[2])
81 deltha_z[0] = abs(deltha_z[1]-deltha_z[2]) 96 deltha_z[0] = abs(deltha_z[1]-deltha_z[2])
82 deltha = deltha_x[0] + deltha_y[0] + deltha_z[0] 97 deltha = deltha_x[0] + deltha_y[0] + deltha_z[0]
83 - print("AcX : ", deltha_x[0], "AcY : ", deltha_y[0], "AcZ : ", deltha_z[0], "Total : ", deltha) 98 + if deltha > Emergency_value:
99 + State_Parameter = True
100 +
101 + shock_level = deltha
102 +
103 +
104 +
105 +
106 +
107 +
108 +
109 + if State_Parameter == True:
110 + Emergency_state_()
111 + else:
112 + print("no emergency\n")
113 +
114 + if State_Parameter == True:
115 + shock_sum += deltha
116 + if State_Parameter == False & shock_sum != 0:
117 + print("누적 충격값 : ", shock_sum)
118 + shock_level = shock_sum
119 + shock_sum = 0;
120 +
84 121
85 drone.close() 122 drone.close()
86 if __name__ == '__main__': 123 if __name__ == '__main__':
......