def H2D(x):
    if x >8388607:
        x = x - 16777216


    return x


import os
import serial
import sys
import time


read_command = "$0wn00,70$1"

get_x = "$0wnA4rm$1"

get_y = "$0wnA7rm$1"

get_z = "$0wnAArm$1"

get_all = "$0wnA4mmm$1"

port = 'COM3'

fx = open("datax.txt", "w+")
fy = open("datay.txt", "w+")
fz = open("dataz.txt", "w+")



XYZ = open("dataXYZ_152am.txt","w+")



command = " "
s = serial.Serial(port, 115200, timeout=1)




if s.isOpen() == False:
    s.open()
else:
    s.close()
    s.open()

    k = 100

#-----------------------------------------------------------

for i in range(0,7):
    #------Get one Measurement command----
    for k in range(0,100):
        command = read_command
        s.write(command.encode())
        time.sleep(.05)
        #------------ Ends Measurement Command---

        #retrieve data

        command = get_x
        s.write(command.encode())
        respuesta = s.read(6)
        time.sleep(.05)
        x = H2D(int(respuesta.decode('utf-8'),16))
        time.sleep(.05)
        fx.write(str(x)+"\n")

        command = get_y
        s.write(command.encode())
        respuesta = s.read(6)
        time.sleep(.05)
        y = H2D(int(respuesta.decode('utf-8'),16))
        fy.write(str(y) +'\n')

        time.sleep(.05)
        command = get_z
        s.write(command.encode())
        respuesta = s.read(6)
        time.sleep(.05)
        z = H2D(int(respuesta.decode('utf-8'),16))
        fz.write(str(z)+ "\n")

        XYZ.write(str(x)+ " " + str(y) + " " +str(z)+"\n")

        print(  "x= " + str(x) + "  y= " + str(y) + "  z= " + str(z))

        k = k - 1
    print("Ronda " + str(i) + "terminada \n")
    time.sleep(60)
#---------





XYZ.close()
fx.close()
fy.close()
fz.close()