mirror of
https://github.com/PotentiaRobotics/control-system.git
synced 2025-04-17 18:00:17 -04:00
126 lines
3.7 KiB
Python
126 lines
3.7 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 re
|
|
import threading
|
|
import socket
|
|
import heapq
|
|
import time
|
|
import serial
|
|
|
|
class Receiver:
|
|
def __init__(self, host, port):
|
|
self.commands = [""]
|
|
self.execute = [""]
|
|
self.transmit = [""]
|
|
self.timer = 0
|
|
self.HOST = host
|
|
self.PORT = port
|
|
|
|
#connecting using scokets
|
|
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')
|
|
(self.conn, addr) = s.accept()
|
|
print('Connected')
|
|
|
|
"""Method receives commands from client and add to queue"""
|
|
def telemetryReceive(self):
|
|
# Commands must be in form "PRIORITY|{COMMAND}|TIMESTAMP|CHECKSUM"
|
|
# awaiting for message
|
|
while True:
|
|
instruction = self.conn.recv(1024)
|
|
action = instruction.decode('UTF-8')
|
|
print("Action received:", action)
|
|
heapq.heappush(self.commands,action)
|
|
heapq.heappush(self.transmit,"Received|"+action)
|
|
|
|
def checkCommand(self):
|
|
while True:
|
|
if len(self.commands) > 0:
|
|
#checking if the checksum of the command
|
|
#equals the sum of all ascii values of every character
|
|
#in command statement
|
|
pattern = "^[0-5]\|.*\|[0-9]{2}:[0-9]{2}\|"
|
|
checksum = "\w+$"
|
|
popped = commands.heappop()
|
|
com = re.findall(pattern, popped)
|
|
numval = re.findall(checksum, popped)
|
|
numval = int(numval,16) #converts hex to int
|
|
if numval == sum([ord(i) for i in com[0]]):
|
|
heapq.heappush(self.transmit, "Correct|"+popped)
|
|
heapq.heappush(self.execute, popped)
|
|
else:
|
|
heappq.heappush(self.transmit, "Incorrect|"+popped)
|
|
|
|
def telemetryTransmit(self):
|
|
while True:
|
|
if len(self.transmit) > 0:
|
|
self.conn.send((heapq.heappop(self.transmit)).encode())
|
|
|
|
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 balance(self):
|
|
print("Nothing")
|
|
|
|
def gaitGen(self):
|
|
print("Nothing")
|
|
|
|
def computerVision(self):
|
|
print("Nothing")
|
|
|
|
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.telemetryReceive).start()
|
|
threading.Thread(target=self.checkCommand).start()
|
|
threading.Thread(target=self.telemetryTransmit).start()
|
|
threading.Thread(target=self.execute).start()
|
|
|
|
threading.Thread(target=self.sensorData).start()
|
|
|
|
threading.Thread(target=self.balance).start()
|
|
threading.Thread(target=self.gaitGen).start()
|
|
threading.Thread(target=self.comuterVision).start()
|
|
|
|
def startBoot():
|
|
simulation = Receiver('10.235.1.148',12345)
|
|
simulation.runSimul()
|