Untitled

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

# 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 = 'COM5'  # Replace with the port name or device path
baud_rate = 250000  # 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=250000, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                    bytesize=serial.EIGHTBITS, timeout=1)
# Create an empty Pandas Series to store the distances
distances = []
turns = []

# Variable for stopping the threads
stop_threads = False


# Thread function to read data from the serial port
def read_serial():

    while not stop_threads:
        global distances
        if ser.in_waiting > 0:
            line = ser.readline().decode('latin-1').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.append(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:
    last_index = -1
    def __init__(self):
        pass

    def notify(self, sensor_data):
        # Customize the notify method to process the data
        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
            print ("I have entered taking data from sensors")

            # Move the turtle according to the given data
            skk.left(o_i)
            if len(distances) == 0:
                print("distances is empty")
                continue
            if len(distances) == 1:
                d_i = distances[0]
            else:
                d_i = distances[-1] - distances[-2]
            
            print(d_i)
            skk.forward(d_i * 10)
            df.loc[len(df.index)] = row
            MyCustomListener.last_index += 1  # Update the last index


# Start the serial thread
serial_thread = threading.Thread(target=read_serial)
serial_thread.start()

# 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)

# Wait for user input to stop the program
input("Press Enter to stop...")

# Stop the threads and close the serial connection
stop_threads = True
serial_thread.join()
ser.close()

# Print the collected sensor data
print(df)

turtle.done()