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

@@ -4,16 +4,27 @@ 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()
 
@@ -30,6 +41,10 @@ def main():
        
         
 
+
+
+        State_Parameter = False
+        shock_level = 0        
         for i in range(4):
             acceleration = drone.get_accelerometer()
             normal_x = 0 
@@ -39,9 +54,9 @@ def main():
             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)
+            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)
@@ -63,9 +78,9 @@ def main():
             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)
+            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)
@@ -80,7 +95,29 @@ def main():
         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)
+        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__':