mirror of
https://github.com/PotentiaRobotics/control-system.git
synced 2025-04-18 18:20:18 -04:00
90 lines
2.4 KiB
Python
90 lines
2.4 KiB
Python
# Create priority queue
|
|
# In main create three threads
|
|
# 1 for managing actions (stores priority queue)
|
|
# 1 for reading sensor data
|
|
# Updates sensor data through wifi and propiosense system
|
|
# 1 for executing the first action in the priority queue
|
|
|
|
from Propioception import *
|
|
from commands import *
|
|
|
|
import threading
|
|
import socket
|
|
import heapq
|
|
import time
|
|
import serial
|
|
|
|
class Receiver:
|
|
def __init__(self, host, port):
|
|
self.actions = ["Password L"]
|
|
self.timer = 0
|
|
self.HOST = host
|
|
self.PORT = port
|
|
|
|
def priorityQueue(self):
|
|
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
print('Socket created')
|
|
|
|
#managing error exception
|
|
try:
|
|
s.bind((self.HOST, self.PORT))
|
|
except socket.error:
|
|
print ('Bind failed ')
|
|
|
|
s.listen(5)
|
|
print ('Socket awaiting messages')
|
|
(conn, addr) = s.accept()
|
|
print ('Connected')
|
|
|
|
# awaiting for message
|
|
while True:
|
|
instruction = conn.recv(1024)
|
|
action = instruction.decode('UTF-8')
|
|
print("Action received:", action)
|
|
if "Password" in action:
|
|
heapq.heappush(self.actions,action)
|
|
print(self.actions)
|
|
conn.send(action.encode()+bytes(" works",'utf-8'))
|
|
else:
|
|
print("Incorrect Command")
|
|
conn.send(bytes("Incorrect Commandc",'utf-8'))
|
|
|
|
# Sending reply
|
|
|
|
|
|
def execute(self):
|
|
while len(self.actions) > 0:
|
|
if self.timer == 0:
|
|
command = heapq.heappop(self.actions)
|
|
if command == "Password A_on_LED":
|
|
if onAndOfffLed():
|
|
print("Executed: ", command)
|
|
else:
|
|
print("Did not execute correctly ", command)
|
|
|
|
time.sleep(5)
|
|
if "Password Balancing" not in self.actions:
|
|
heapq.heappush(self.actions, "Password Balancing")
|
|
print("Inside execute",self.actions)
|
|
|
|
def sensorData(self):
|
|
# Test
|
|
print("Inside")
|
|
ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
|
|
ser.reset_input_buffer()
|
|
while True:
|
|
print("Inside data")
|
|
# Read from Arduinos to know what motors and sensors there are
|
|
ser.write("Send ****s plz\n".encode('utf-8'))
|
|
line = ser.readline().decode('utf-8').rstrip()
|
|
print(line)
|
|
|
|
def runSimul(self):
|
|
threading.Thread(target=self.priorityQueue).start()
|
|
threading.Thread(target=self.execute).start()
|
|
threading.Thread(target=self.sensorData).start()
|
|
|
|
def startBoot():
|
|
simulation = Receiver('10.235.1.148',12345)
|
|
simulation.runSimul()
|