Untitled

from HIMUServer import HIMUServer
import pandas as pd
import turtle
import numpy
import serial


# getting change in orientation from gyroscope value
def get_o_i(gyro):
    return gyro * 0.1


# drawing the route
wn = turtle.Screen()
wn.bgcolor("light green")
wn.title("Turtle")
skk = turtle.Turtle()
skk.shape('turtle')

port = 'COM8'  # Replace with the port name or device path
baud_rate = 115200  # Replace with the appropriate baud rate

# creating dataframe for collecting sensor data
df = pd.DataFrame(columns=['acc_x', 'acc_y', 'acc_z', 'gyro_x', 'gyro_y', 'gyro_z'])

# Open the serial connection
ser = serial.Serial(port, baudrate=115200, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                    bytesize=serial.EIGHTBITS, timeout=1)
# Create an empty Pandas Series to store the distances
distances = pd.Series(dtype=float)

# Read data from the port
while True:
    if ser.in_waiting > 0:
        line = ser.readline().decode().strip()

        # Parse the line and extract distance values
        if line.startswith("Distance"):
            distance_str = line.split("=")[1].strip()
            distance = float(distance_str)

            # Append the distance to the Pandas Series
            distances = distances.append(pd.Series(distance))

        # Go to the next distance if position line is encountered
        elif line.startswith("Position"):
            continue


# listener implementation for collecting accelerometer and gyroscope data
class MyCustomListener:
    def __init__(self):  # can use (self, server_instance) too but need to add it when creating listener later
        pass

    def notify(self, sensor_data):
        # Customize the notify method in order to elaborate data
        # sensorData contains String values (see HIMUServer.__extractSensorData())
        for sensors in sensor_data:
            acc = HIMUServer.strings2Floats(sensors[0])
            gyro = HIMUServer.strings2Floats(sensors[1])
            row = [acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2]]
            o_i = numpy.rad2deg(get_o_i(row[4]))  # change in orientation

            # have moved turtle according to given data each 100ms
            skk.left(o_i)
            if len(df.index) > 1:
                d_i = distances[len(df.index) - 1] - distances[len(df.index) - 2]  # distance travelled
            else:
                d_i = distances[len(df.index)]
            skk.forward(d_i * 10)
            df.loc[len(df.index)] = row


# HIMUServer instance:
myHIMUServer = HIMUServer()

# Creating listener and adding it to the server instance:
myListener = MyCustomListener()
myHIMUServer.addListener(myListener)

# Change the timeout (in seconds) :
myHIMUServer.timeout = 5

# Launch acquisition via UDP on port 2055:
myHIMUServer.start("UDP", 2055)

print(df)

turtle.done()

# Close the serial connection
ser.close()