Untitled

mail@pastecode.io avatar
unknown
python
a year ago
3.5 kB
2
Indexable
Never
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 = pd.Series(dtype=float)

# Variable for stopping the threads
stop_threads = False


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

    while not stop_threads:
        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 = pd.concat([distances, pd.Series(distance)], ignore_index=True)
            # 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):
        self.last_index = -1  # Initialize last_index to -1

    def notify(self, sensor_data):
        global distances
        # 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)
            print (self.last_index)
            if self.last_index >= 0:
                d_i = distances[self.last_index + 1] - distances[self.last_index]  # distance travelled
            else:
                d_i = distances[0]
            skk.forward(d_i * 10)
            df.loc[len(df.index)] = row
            self.last_index += 1  # Update the last index


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

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

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