diff --git a/control_systems/.gitignore b/control_systems/.gitignore
new file mode 100644
index 0000000..259148f
--- /dev/null
+++ b/control_systems/.gitignore
@@ -0,0 +1,32 @@
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
diff --git a/control_systems/.replit b/control_systems/.replit
new file mode 100644
index 0000000..6134e30
--- /dev/null
+++ b/control_systems/.replit
@@ -0,0 +1,2 @@
+language = "undefined"
+run = ""
\ No newline at end of file
diff --git a/control_systems/Control System/Propioception.py b/control_systems/Control System/Propioception.py
new file mode 100644
index 0000000..4676e09
--- /dev/null
+++ b/control_systems/Control System/Propioception.py
@@ -0,0 +1,209 @@
+"""
+This class keeps track of all the motors, sensors and import variables needed for the robot.
+"""
+class Propioception:
+ """
+ Standard kwargs to give for this function
+
+ data = {
+ # Motors (all units are in degrees)
+ 'left_hip_pitch': 0,
+ 'left_hip_yaw': 0,
+ 'left_hip_roll': 0,
+ 'left_knee_pitch': 0,
+ 'left_ankle_pitch': 0,
+ 'left_ankle_yaw': 0,
+ 'left_ankle_roll': 0,
+ 'right_hip_pitch': 0,
+ 'right_hip_yaw': 0,
+ 'right_hip_roll': 0,
+ 'right_knee_pitch': 0,
+ 'right_ankle_pitch': 0,
+ 'right_ankle_yaw': 0,
+ 'right_ankle_roll': 0,
+ 'left_shoulder_pitch': 0,
+ 'left_shoulder_yaw': 0,
+ 'left_shoulder_roll': 0,
+ 'left_elbow_pitch': 0,
+ 'right_shoulder_pitch': 0,
+ 'right_shoulder_yaw': 0,
+ 'right_shoulder_roll': 0,
+ 'right_elbow_pitch': 0,
+ 'torso_pitch': 0,
+ 'torso_yaw': 0,
+ 'torso_roll': 0,
+ 'neck_pitch': 0,
+ 'neck_yaw': 0,
+ 'neck_roll': 0,
+
+ # Sensors
+ # Add later
+ }
+
+ This will also have the constants
+ """
+ def __init__(self, **kwargs):
+ self.data = {
+ # Add defualt later
+ }
+ self.constants = {
+ # Add some constants here later
+ }
+ for key, value in kwargs.items():
+ self.data[str(key)] = value
+
+ def update(self, partsAndValues):
+ """
+ This function updates the dictionary
+
+ arg artsAndValues is {string --> int or float}
+
+ Error codes:
+ 1 is success
+ -1 is the error for NoneType variables
+ -2 is for incorrect datatypes
+ -3 is for when the key is not in the dictionary
+ """
+
+ if partsAndValues == None:
+ return -1
+
+ if type(partsAndValues) != dict:
+ return -2
+
+ for key in partsAndValues.keys():
+ if key in self.data:
+ self.data[str(key)] = partsAndValues[key]
+ else:
+ return -3
+ return 1
+
+ def add(self, partsAndValues):
+ """
+ This function will add a part to the system if needed
+
+ arg artsAndValues is {string --> int or float}
+
+ Error codes:
+ 1 is success
+ -1 is the error for NoneType variables
+ -2 is for incorrect datatypes
+ -3 is for when the key is in the dictionary
+ """
+
+ if partsAndValues == None:
+ return -1
+
+ if type(partsAndValues) != dict:
+ return -2
+
+ for key in partsAndValues.keys():
+ if key in self.data:
+ return -3
+ else:
+ self.data[str(key)] = partsAndValues[key]
+ return 1
+
+ def remove(self, partsAndValues):
+ """
+ This function will remove a part to the system if needed
+
+ arg artsAndValues is [string]
+
+ Error codes:
+ 1 is success
+ -1 is the error for NoneType variables
+ -2 is for incorrect datatypes
+ -3 is for when the key is not in the dictionary
+ """
+
+ if partsAndValues == None:
+ return -1
+
+
+ if type(partsAndValues) != list:
+ return -2
+
+ for key in partsAndValues:
+ if key in self.data:
+ self.data.pop(str(key))
+ else:
+ return -3
+ return 1
+
+
+# # Test
+# stuff = Propioception(
+# left_hip_pitch= 0,
+# left_hip_yaw= 0,
+# left_hip_roll= 0,
+# left_knee_pitch= 0,
+# left_ankle_pitch= 0,
+# left_ankle_yaw= 0,
+# left_ankle_roll= 0,
+# right_hip_pitch= 0,
+# right_hip_yaw= 0,
+# right_hip_roll= 0,
+# right_knee_pitch= 0,
+# right_ankle_pitch= 0,
+# right_ankle_yaw= 0,
+# right_ankle_roll= 0,
+# left_shoulder_pitch= 0,
+# left_shoulder_yaw= 0,
+# left_shoulder_roll= 0,
+# left_elbow_pitch= 0,
+# right_shoulder_pitch= 0,
+# right_shoulder_yaw= 0,
+# right_shoulder_roll= 0,
+# right_elbow_pitch= 0,
+# torso_pitch= 0,
+# torso_yaw= 0,
+# torso_roll= 0,
+# neck_pitch= 0,
+# neck_yaw= 0,
+# neck_roll= 0
+# )
+
+# """Update cases"""
+# # Success case
+# print(stuff.data['neck_roll'])
+# stuff.update({'neck_roll': 21})
+# print(stuff.data['neck_roll'])
+
+# # -1 case
+# print(stuff.update(None))
+
+# # -2 case
+# print(stuff.update([]))
+
+# # -3 case
+# print(stuff.update({'test': 21}))
+
+# """Remove cases"""
+# # Success case
+# print(stuff.data['neck_roll'])
+# print(stuff.remove(['neck_roll']))
+
+# # -1 case
+# print(stuff.remove(None))
+
+# # # -2 case
+# print(stuff.remove({}))
+
+# # -3 case
+# print(stuff.remove(['test']))
+
+# """Add cases"""
+# # Success case
+# print('test' in stuff.data.keys())
+# stuff.add({'test': 21})
+# print(stuff.data['test'])
+
+# # -1 case
+# print(stuff.add(None))
+
+# # -2 case
+# print(stuff.add([]))
+
+# # -3 case
+# print(stuff.add({'test': 21}))
\ No newline at end of file
diff --git a/control_systems/Control System/bootup.py b/control_systems/Control System/bootup.py
new file mode 100644
index 0000000..7095d70
--- /dev/null
+++ b/control_systems/Control System/bootup.py
@@ -0,0 +1,135 @@
+# 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.executions = []
+ 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:
+ action = self.conn.recv(1024).decode('UTF-8')
+ if len(action) > 0:
+ heapq.heappush(self.commands,action)
+ print("Received|"+action)
+ heapq.heappush(self.transmit,"Received|"+action)
+
+ """Method checks commands from queue and adds to execution queue"""
+ 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}\|" #everything but the checksum value
+ checksum = "\w+$" #checksum value
+ popped = heapq.heappop(self.commands) #gets smallest value command
+ com = re.findall(pattern, popped)
+ numval = re.findall(checksum, popped)
+ numval = numval[0]
+ numval = int(numval,16) #converts hex to int
+ print(com[0])
+ print(sum([ord(i) for i in com[0]]))
+ if numval == sum([ord(i) for i in com[0]]):
+ print("working")
+ heapq.heappush(self.transmit, "Correct|"+popped)
+ heapq.heappush(self.executions, popped)
+ print(self.transmit)
+ print(self.executions)
+ else:
+ heapq.heappush(self.transmit, "Incorrect|"+popped)
+
+ def telemetryTransmit(self):
+ while True:
+ if len(self.transmit) > 0:
+ print("Transmit queue", self.transmit)
+ self.conn.send(bytes(heapq.heappop(self.transmit),'utf-8'))
+
+ def execute(self):
+ while True:
+ if len(self.executions) > 0:
+ command = heapq.heappop(self.executions)
+ print(command)
+ heapq.heappush(self.transmit, "Executed|"+command)
+ # if command == "Password A_on_LED":
+ # if onAndOfffLed():
+ # print("Executed: ", command)
+ # else:
+ # print("Did not execute correctly ", command)
+ print("Inside execute",self.executions)
+ time.sleep(5)
+ if "Balancing" not in self.executions:
+ heapq.heappush(self.executions, "Balancing")
+
+ def balance(self):
+ print("Nothing")
+
+ def gaitGen(self):
+ print("Nothing")
+
+ def computerVision(self):
+ print("Nothing")
+
+ def sensorData(self):
+ var = ""
+ # 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.145',12345)
+ simulation.runSimul()
diff --git a/control_systems/Control System/commands.py b/control_systems/Control System/commands.py
new file mode 100644
index 0000000..3bd1969
--- /dev/null
+++ b/control_systems/Control System/commands.py
@@ -0,0 +1,24 @@
+# Commands for arduino
+
+import serial
+import time
+
+ser = serial.Serial('/dev/ttyACM0', 9600, timeout=1) # fix
+ser.reset_input_buffer()
+
+def onAndOfffLed():
+ ser.write("ON\n".encode('utf-8'))
+ line = ser.readline().decode('utf-8').rstrip()
+ print(line)
+
+ time.sleep(4)
+
+ ser.write("OFF\n".encode('utf-8'))
+ line = ser.readline().decode('utf-8').rstrip()
+ print(line)
+ time.sleep(4)
+
+ if line == "You sent me: OFF":
+ return 1
+ else:
+ return 0
\ No newline at end of file
diff --git a/control_systems/Control System/gaitGen/footProjection.py b/control_systems/Control System/gaitGen/footProjection.py
new file mode 100644
index 0000000..b20575a
--- /dev/null
+++ b/control_systems/Control System/gaitGen/footProjection.py
@@ -0,0 +1,52 @@
+import math as m
+def createXProjection(length, duration):
+ t = 0;
+ f = open("xValues.txt", "w")
+ f2 = open("timeX.txt", "w")
+ xLambda = length/duration
+ w = m.pi * 2
+ w = w/duration
+ projectedPoints = []
+ deltaT = 0.01 #this is an arbitrary delta t, which can be changed later on
+ while(t < duration+deltaT):
+ value = xLambda*t - (xLambda/w)*m.sin(w*t)
+ projectedPoints.append(value)
+ f.write(str(value) + "\n")
+ f2.write(str(t) + "\n")
+ t += deltaT
+ t = round(t, 2)
+ f.close()
+ f2.close()
+ return projectedPoints
+def createZProjection(height, duration):
+ t = 0;
+ f = open("zValues.txt", "w")
+ f2 = open("timeZ.txt", "w")
+ duration = duration/2
+ zLambda = height/(duration)
+ w = m.pi * 2
+ w = w/duration
+ projectedPoints = []
+ deltaT = 0.01 #this is an arbitrary delta t, which can be changed later on
+ while(t < duration+deltaT):
+ value = zLambda*t - (zLambda/w)*m.sin(w*t)
+ projectedPoints.append(value)
+ f.write(str(value) + "\n")
+ f2.write(str(t) + "\n")
+ t += deltaT
+ t = round(t, 2)
+ t = duration+deltaT
+ while(t < (2 * duration)+deltaT):
+ value = -(zLambda*t - (zLambda/w)*m.sin(w*t)) + (height*2)
+ projectedPoints.append(value)
+ f.write(str(value) +"\n")
+ f2.write(str(t)+"\n")
+ t += deltaT
+ t = round(t, 2)
+ f.close()
+ f2.close()
+ return projectedPoints
+
+#change the values based on the length/height of the step and how long the duration is
+createXProjection(0.5,3)
+createZProjection(0.4,3)
\ No newline at end of file
diff --git a/control_systems/Control System/gaitGen/olympian.urdf b/control_systems/Control System/gaitGen/olympian.urdf
new file mode 100644
index 0000000..ca8c72e
--- /dev/null
+++ b/control_systems/Control System/gaitGen/olympian.urdf
@@ -0,0 +1,927 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ PositionJointInterface
+
+
+ PositionJointInterface
+ 1
+
+
+
+
diff --git a/control_systems/Control System/gaitGen/step.py b/control_systems/Control System/gaitGen/step.py
new file mode 100644
index 0000000..8e3bb37
--- /dev/null
+++ b/control_systems/Control System/gaitGen/step.py
@@ -0,0 +1,299 @@
+import pybullet as p
+import time
+import pybullet_data
+import math
+import matplotlib.pyplot as plt
+import numpy as np
+physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
+
+print("hi")
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
+p.setGravity(0,0,-9.8)
+planeId = p.loadURDF("plane.urdf") #where is plane.urdf
+startPos = [0,0,0]
+#sphereId = p.loadURDF("sphere.urdf")
+startOrientation = p.getQuaternionFromEuler([0,0,0])
+robotId = p.loadURDF("olympian.urdf",startPos, startOrientation,
+ # useMaximalCoordinates=1, ## New feature in Pybullet
+ flags=p.URDF_USE_INERTIA_FROM_FILE)
+
+print("==========SIMULATION ENABLED================")
+
+#GET JOINT INFO
+
+print(p.getNumJoints(robotId))
+for i in range(0, p.getNumJoints(robotId)-1):
+ print(p.getJointInfo(robotId, i)[0:13])
+
+
+listOfJointIndeces = []
+for i in range(0, p.getNumJoints(robotId)):
+ listOfJointIndeces.append(i)
+#why not use list(range())?
+#also why have 0
+
+def calcCOM():
+
+ #CALCULATE COM
+ masstimesxpossum = 0.0
+ masstimesypossum = 0.0
+ masstimeszpossum = 0.0
+ masssum = 0.0
+ for i in range(0, p.getNumJoints(robotId) -1):
+
+ # if(i >= 0):
+ # print(p.getJointInfo(robotId, i)[0:13])
+
+ wheight = p.getDynamicsInfo(robotId, i)[0]
+ xpos = p.getLinkState(robotId, i)[0][0]
+ ypos = p.getLinkState(robotId, i)[0][1]
+ zpos = p.getLinkState(robotId, i)[0][2]
+
+ masstimesxpossum += (wheight * xpos)
+ masstimesypossum += (wheight * ypos)
+ masstimeszpossum += (wheight * zpos)
+ masssum += wheight
+
+ # print(wheight)
+ # print(xpos)
+ # print(ypos)
+ # print(zpos)
+ # print("\n")
+ p.stepSimulation()
+ com = (masstimesxpossum/masssum, masstimesypossum/masssum, masstimeszpossum/masssum)
+
+ #print("mass: " + str(masssum))
+ #print("center of mass: " + str(com))
+ #print("\n")
+ return com
+
+#INVERSE KINEMATICS
+#GET JOINT ANGLES NEEDED TO MOVE LINK 10 to (0. 0.6, 2)
+#FORCE IS 25NM for every link bc im too lazy to custom set it for every link
+#IT FALLS BECAUSE its a lot of torque exerted in a small amount of time
+
+torque = 100
+newi = []
+comPos = []
+comPos2 = []
+
+
+
+holdingTorque = 100
+actuationTorque = 10
+torqueList = [holdingTorque]*23
+
+file = open("xValues.txt", "r")
+xValues = []
+for word in file.readlines():
+ xValues.append(float(word.rstrip('\n')))
+file = open("zValues.txt", "r")
+zValues = []
+for word in file.readlines():
+ zValues.append(float(word.rstrip('\n')))
+
+def shiftFoot():
+
+ leftAnkle = p.getJointState(robotId, 21)[0]
+ leftThigh = p.getJointState(robotId, 18)[0]
+ print(leftAnkle)
+ print(leftThigh)
+
+ for i in range(0, 50):
+ positionsList = [0]*23
+ angleNeeded = calculateAngleNeeded()
+ comPos.append(calcCOM())
+ newi.insert(0, i)
+ positionsList[21] = angleNeeded/50 * i #left ankle
+ positionsList[15] = angleNeeded/50 * i #right ankle
+ positionsList[18] = angleNeeded/50 * i#left thigh
+ positionsList[12] = angleNeeded/50 * i#right thigh
+
+ #positionsList[14] = 0.3/50 * i
+ forceArray = [torque]*23
+ p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray)
+ p.stepSimulation()
+
+ ''
+ for i in range(0, 50):
+ comPos.append(calcCOM())
+ newi.insert(0, i)
+ angleNeeded = 0.5
+
+ positionsList[11] = 0.2598371070140097/50 * i #thigh pitch
+ positionsList[14] = 0.8123194567657278/50 * i #knee
+ positionsList[16] = 0.4891906071331436/50 * i#ankle
+
+ #positionsList[14] = 0.3/50 * i
+ forceArray = [torque]*23
+ p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray)
+ p.stepSimulation()
+
+
+ positionsList = [0]*23
+
+ firstY = p.getLinkState(robotId, 15)[0][1]
+ firstZ = p.getLinkState(robotId, 15)[0][2]
+
+
+ for i in range(0, len(xValues)//20):
+ """
+ comPos.append(calcCOM())
+ newi.insert(0, i)
+ angleNeeded = calculateAngleNeeded()
+ positionsList[21] = angleNeeded/len(xValues) * i #left ankle
+ #positionsList[15] = angleNeeded/len(xValues) * i #right ankle
+ positionsList[18] = angleNeeded/len(xValues) * i#left thigh
+ #positionsList[12] = angleNeeded/len(xValues) * i#right thigh
+ forceArray = [torque]*23
+ p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray)
+ p.stepSimulation()
+ """
+ #thighs are 12 and 18
+ angleNeeded = calculateAngleNeeded()
+ ikList = p.calculateInverseKinematics(robotId, 15, [xValues[20*i], firstY, zValues[20*i]+(len(xValues)-20*i)/len(xValues)*firstZ])
+
+ positionsList = list(ikList)
+ #print("uwu")
+ #print(positionsList[11])
+ #print(positionsList[14])
+ #print(positionsList[16])
+ #positionsList[11] = 0.5*(len(xValues)-i)/len(xValues)+positionsList[11]*(i/len(xValues))
+ #positionsList[14] = 1*(len(xValues)-i)/len(xValues)+positionsList[14]*(i/len(xValues))
+ #positionsList[16] = 0.5*(len(xValues)-i)/len(xValues)+positionsList[16]*(i/len(xValues))
+ positionsList[21] = angleNeeded*(len(xValues)-8*i)/len(xValues)
+ positionsList[18] = angleNeeded*(len(xValues)-8*i)/len(xValues)
+ ankleAngle = calculateAnkleAngle()
+
+ positionsList[15] = (math.pi/2-ankleAngle*np.sign(ankleAngle))*np.sign(ankleAngle)/len(xValues)*i*20
+ ankleAngle = calculateOtherAnkleAngle()
+ positionsList[16] = 0.4891906071331436*(len(xValues)-(i*20))/len(xValues)+ankleAngle/len(xValues)*i*20
+ pelvisAngle = calculatePelvisAngle()
+ print(pelvisAngle)
+ positionsList[0] = -1*pelvisAngle
+ #positionsList[15] = (math.pi-ankleAngle*np.sign(ankleAngle))*np.sign(ankleAngle)/len(xValues)*i
+ '''
+ if((ankleAngle)<0):
+ positionsList[15] = -1*(math.pi-(abs(ankleAngle)))/len(xValues)*i
+ else:
+ positionsList[15] = math.pi-(abs(ankleAngle))/len(xValues)*i
+ '''
+ #positionsList[16] =
+ #positionsList[15] = (math.pi-calculateAnkleAngle())/len(xValues)*i
+ # positionsList[0] = math.pi / 4
+ forceArray = [torque]*23
+ p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray)
+ p.stepSimulation()
+
+ for i in range(50000):
+ p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray)
+ p.stepSimulation()
+
+def calculateAngleNeeded():
+ anklePos = p.getLinkState(robotId, 21)[0]
+ comPos = calcCOM()
+ hipPos = p.getLinkState(robotId, 18)[0]
+ ankleCom = (comPos[1]-anklePos[1], comPos[2]-anklePos[2])
+ ankleHip = (hipPos[1]-anklePos[1], hipPos[2]-anklePos[2])
+ cosAngle = (ankleCom[0]*ankleHip[0] + ankleCom[1]*ankleHip[1])/(np.sqrt((ankleCom[0]**2+ankleCom[1]**2))*np.sqrt((ankleHip[0]**2 + ankleHip[1]**2)))
+ angle = np.arccos(cosAngle)
+ #print(angle)
+ return angle
+
+def calculateAnkleAngle():
+ anklePos = p.getLinkState(robotId, 15)[0]
+ kneePos = p.getLinkState(robotId, 14)[0]
+ angle = np.arctan((kneePos[2]-anklePos[2])/(kneePos[1]-anklePos[1]))
+ print("thing " + str(angle))
+ return angle
+
+def calculateOtherAnkleAngle():
+ anklePos = p.getLinkState(robotId, 15)[0]
+ kneePos = p.getLinkState(robotId, 14)[0]
+ angle = np.arctan((kneePos[0]-anklePos[0])/(kneePos[2]-anklePos[2]))
+ print("thing " + str(angle))
+ return angle
+
+def calculatePelvisAngle():
+ pelvisPos = p.getLinkState(robotId, 0)[0]
+ chestPos = p.getLinkState(robotId, 1)[0]
+ print(chestPos)
+ print(pelvisPos)
+ angle = np.arctan((chestPos[0]-pelvisPos[0])/(chestPos[2]-pelvisPos[2]))
+ return angle
+
+shiftFoot()
+
+print(p.getLinkState(robotId, 21)[0][1]-calcCOM()[1])
+
+print(str(p.getLinkState(robotId, 21)) + "HIIII")
+
+for i in range(1000000):
+ p.stepSimulation()
+
+"""
+for i in range(0, len(xValues), 1):
+ ikList = p.calculateInverseKinematics(robotId, 15, [xValues[i], 0, zValues[i]])
+ p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL,
+ targetPositions = ikList, forces = torqueList)
+ p.stepSimulation()
+ time.sleep(1./240.)
+
+for i in range(10000):
+ p.stepSimulation()
+ time.sleep(1./240.)
+"""
+
+z = list(range(len(xValues)))
+area = 3 # 0 to 15 point radii
+
+plt.scatter(z, zValues, s=area, alpha=0.5)
+#plt.show()
+
+#ikList = p.calculateInverseKinematics(robotId, 10, [0,0.6,2] )
+
+"""
+p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL,
+ targetPositions = ikList, forces = torqueList)
+
+
+#CALCULATE COM
+masstimesxpossum = 0.0
+masstimesypossum = 0.0
+masstimeszpossum = 0.0
+masssum = 0.0
+for i in range(0, p.getNumJoints(robotId) -1):
+
+ if(i >= 0):
+ print(p.getJointInfo(robotId, i)[0:13])
+
+ wheight = p.getDynamicsInfo(robotId, i)[0]
+ xpos = p.getLinkState(robotId, i)[0][0]
+ ypos = p.getLinkState(robotId, i)[0][1]
+ zpos = p.getLinkState(robotId, i)[0][2]
+
+ masstimesxpossum += (wheight * xpos)
+ masstimesypossum += (wheight * ypos)
+ masstimeszpossum += (wheight * zpos)
+ masssum += wheight
+
+ print(wheight) #what is wheight
+ print(xpos)
+ print(ypos)
+ print(zpos)
+ print("\n")
+
+com = (masstimesxpossum/masssum, masstimesypossum/masssum, masstimeszpossum/masssum)
+print("==========COM APROX EQUALS===========")
+print(com)
+print("\n")
+#STEP SIMULATION
+for i in range(0,10000):
+ p.stepSimulation()
+ time.sleep(1./240.)
+
+
+print("========REALTIME SIMULATION DISABLED===============")
+# p.calculateInverseKinematics(robotId, )
+"""
\ No newline at end of file
diff --git a/control_systems/Control System/gaitGen/timeX.txt b/control_systems/Control System/gaitGen/timeX.txt
new file mode 100644
index 0000000..e5a6cce
--- /dev/null
+++ b/control_systems/Control System/gaitGen/timeX.txt
@@ -0,0 +1,301 @@
+0
+0.01
+0.02
+0.03
+0.04
+0.05
+0.06
+0.07
+0.08
+0.09
+0.1
+0.11
+0.12
+0.13
+0.14
+0.15
+0.16
+0.17
+0.18
+0.19
+0.2
+0.21
+0.22
+0.23
+0.24
+0.25
+0.26
+0.27
+0.28
+0.29
+0.3
+0.31
+0.32
+0.33
+0.34
+0.35
+0.36
+0.37
+0.38
+0.39
+0.4
+0.41
+0.42
+0.43
+0.44
+0.45
+0.46
+0.47
+0.48
+0.49
+0.5
+0.51
+0.52
+0.53
+0.54
+0.55
+0.56
+0.57
+0.58
+0.59
+0.6
+0.61
+0.62
+0.63
+0.64
+0.65
+0.66
+0.67
+0.68
+0.69
+0.7
+0.71
+0.72
+0.73
+0.74
+0.75
+0.76
+0.77
+0.78
+0.79
+0.8
+0.81
+0.82
+0.83
+0.84
+0.85
+0.86
+0.87
+0.88
+0.89
+0.9
+0.91
+0.92
+0.93
+0.94
+0.95
+0.96
+0.97
+0.98
+0.99
+1.0
+1.01
+1.02
+1.03
+1.04
+1.05
+1.06
+1.07
+1.08
+1.09
+1.1
+1.11
+1.12
+1.13
+1.14
+1.15
+1.16
+1.17
+1.18
+1.19
+1.2
+1.21
+1.22
+1.23
+1.24
+1.25
+1.26
+1.27
+1.28
+1.29
+1.3
+1.31
+1.32
+1.33
+1.34
+1.35
+1.36
+1.37
+1.38
+1.39
+1.4
+1.41
+1.42
+1.43
+1.44
+1.45
+1.46
+1.47
+1.48
+1.49
+1.5
+1.51
+1.52
+1.53
+1.54
+1.55
+1.56
+1.57
+1.58
+1.59
+1.6
+1.61
+1.62
+1.63
+1.64
+1.65
+1.66
+1.67
+1.68
+1.69
+1.7
+1.71
+1.72
+1.73
+1.74
+1.75
+1.76
+1.77
+1.78
+1.79
+1.8
+1.81
+1.82
+1.83
+1.84
+1.85
+1.86
+1.87
+1.88
+1.89
+1.9
+1.91
+1.92
+1.93
+1.94
+1.95
+1.96
+1.97
+1.98
+1.99
+2.0
+2.01
+2.02
+2.03
+2.04
+2.05
+2.06
+2.07
+2.08
+2.09
+2.1
+2.11
+2.12
+2.13
+2.14
+2.15
+2.16
+2.17
+2.18
+2.19
+2.2
+2.21
+2.22
+2.23
+2.24
+2.25
+2.26
+2.27
+2.28
+2.29
+2.3
+2.31
+2.32
+2.33
+2.34
+2.35
+2.36
+2.37
+2.38
+2.39
+2.4
+2.41
+2.42
+2.43
+2.44
+2.45
+2.46
+2.47
+2.48
+2.49
+2.5
+2.51
+2.52
+2.53
+2.54
+2.55
+2.56
+2.57
+2.58
+2.59
+2.6
+2.61
+2.62
+2.63
+2.64
+2.65
+2.66
+2.67
+2.68
+2.69
+2.7
+2.71
+2.72
+2.73
+2.74
+2.75
+2.76
+2.77
+2.78
+2.79
+2.8
+2.81
+2.82
+2.83
+2.84
+2.85
+2.86
+2.87
+2.88
+2.89
+2.9
+2.91
+2.92
+2.93
+2.94
+2.95
+2.96
+2.97
+2.98
+2.99
+3.0
diff --git a/control_systems/Control System/gaitGen/timeZ.txt b/control_systems/Control System/gaitGen/timeZ.txt
new file mode 100644
index 0000000..e5a6cce
--- /dev/null
+++ b/control_systems/Control System/gaitGen/timeZ.txt
@@ -0,0 +1,301 @@
+0
+0.01
+0.02
+0.03
+0.04
+0.05
+0.06
+0.07
+0.08
+0.09
+0.1
+0.11
+0.12
+0.13
+0.14
+0.15
+0.16
+0.17
+0.18
+0.19
+0.2
+0.21
+0.22
+0.23
+0.24
+0.25
+0.26
+0.27
+0.28
+0.29
+0.3
+0.31
+0.32
+0.33
+0.34
+0.35
+0.36
+0.37
+0.38
+0.39
+0.4
+0.41
+0.42
+0.43
+0.44
+0.45
+0.46
+0.47
+0.48
+0.49
+0.5
+0.51
+0.52
+0.53
+0.54
+0.55
+0.56
+0.57
+0.58
+0.59
+0.6
+0.61
+0.62
+0.63
+0.64
+0.65
+0.66
+0.67
+0.68
+0.69
+0.7
+0.71
+0.72
+0.73
+0.74
+0.75
+0.76
+0.77
+0.78
+0.79
+0.8
+0.81
+0.82
+0.83
+0.84
+0.85
+0.86
+0.87
+0.88
+0.89
+0.9
+0.91
+0.92
+0.93
+0.94
+0.95
+0.96
+0.97
+0.98
+0.99
+1.0
+1.01
+1.02
+1.03
+1.04
+1.05
+1.06
+1.07
+1.08
+1.09
+1.1
+1.11
+1.12
+1.13
+1.14
+1.15
+1.16
+1.17
+1.18
+1.19
+1.2
+1.21
+1.22
+1.23
+1.24
+1.25
+1.26
+1.27
+1.28
+1.29
+1.3
+1.31
+1.32
+1.33
+1.34
+1.35
+1.36
+1.37
+1.38
+1.39
+1.4
+1.41
+1.42
+1.43
+1.44
+1.45
+1.46
+1.47
+1.48
+1.49
+1.5
+1.51
+1.52
+1.53
+1.54
+1.55
+1.56
+1.57
+1.58
+1.59
+1.6
+1.61
+1.62
+1.63
+1.64
+1.65
+1.66
+1.67
+1.68
+1.69
+1.7
+1.71
+1.72
+1.73
+1.74
+1.75
+1.76
+1.77
+1.78
+1.79
+1.8
+1.81
+1.82
+1.83
+1.84
+1.85
+1.86
+1.87
+1.88
+1.89
+1.9
+1.91
+1.92
+1.93
+1.94
+1.95
+1.96
+1.97
+1.98
+1.99
+2.0
+2.01
+2.02
+2.03
+2.04
+2.05
+2.06
+2.07
+2.08
+2.09
+2.1
+2.11
+2.12
+2.13
+2.14
+2.15
+2.16
+2.17
+2.18
+2.19
+2.2
+2.21
+2.22
+2.23
+2.24
+2.25
+2.26
+2.27
+2.28
+2.29
+2.3
+2.31
+2.32
+2.33
+2.34
+2.35
+2.36
+2.37
+2.38
+2.39
+2.4
+2.41
+2.42
+2.43
+2.44
+2.45
+2.46
+2.47
+2.48
+2.49
+2.5
+2.51
+2.52
+2.53
+2.54
+2.55
+2.56
+2.57
+2.58
+2.59
+2.6
+2.61
+2.62
+2.63
+2.64
+2.65
+2.66
+2.67
+2.68
+2.69
+2.7
+2.71
+2.72
+2.73
+2.74
+2.75
+2.76
+2.77
+2.78
+2.79
+2.8
+2.81
+2.82
+2.83
+2.84
+2.85
+2.86
+2.87
+2.88
+2.89
+2.9
+2.91
+2.92
+2.93
+2.94
+2.95
+2.96
+2.97
+2.98
+2.99
+3.0
diff --git a/control_systems/Control System/gaitGen/urdf.py b/control_systems/Control System/gaitGen/urdf.py
new file mode 100644
index 0000000..ceb1fa6
--- /dev/null
+++ b/control_systems/Control System/gaitGen/urdf.py
@@ -0,0 +1,8 @@
+from urdfpy import URDF
+
+robot = URDF.load('olympian.urdf')
+
+for link in robot.links:
+ print(link.name)
+
+print(robot.base_link.name)
\ No newline at end of file
diff --git a/control_systems/Control System/gaitGen/xValues.txt b/control_systems/Control System/gaitGen/xValues.txt
new file mode 100644
index 0000000..93eefbc
--- /dev/null
+++ b/control_systems/Control System/gaitGen/xValues.txt
@@ -0,0 +1,301 @@
+0.0
+1.4621315464749193e-07
+1.1696282759916451e-06
+3.947062560952751e-06
+9.354563835677412e-06
+1.8267026292003927e-05
+3.155780672995691e-05
+5.009834147459355e-05
+7.475776413522206e-05
+0.00010640252437467412
+0.0001458960078558648
+0.00019409815753227908
+0.00025186509644859953
+0.00032004875221680607
+0.00039949648333261964
+0.0004910507074960234
+0.0005955485320989981
+0.0007138213870426305
+0.0008466946600443892
+0.0009949873345958607
+0.0011595116307294845
+0.0013410726487520092
+0.0015404680161009016
+0.0017584875374785994
+0.001995912848418005
+0.0022535170724313977
+0.0025320644818928317
+0.0028323101628030672
+0.0031549996835838617
+0.003500868768047026
+0.003870642972681651
+0.004265037368401052
+0.0046847562268890475
+0.005130492711683053
+0.005602928574129737
+0.006102733854346301
+0.00663056658731874
+0.007187072514265952
+0.007772884799396335
+0.008388623752180926
+0.009034896555265307
+0.009712296998139136
+0.010421405216680515
+0.011162787438689148
+0.011936995735520178
+0.012744567779927413
+0.01358602661022229
+0.014461880400851945
+0.015372622239497244
+0.016318729910788046
+0.01730066568673118
+0.018318876123942687
+0.01937379186777323
+0.020465827463412897
+0.021595381174057945
+0.02276283480621942
+0.02396855354225015
+0.02521288578016373
+0.02649616298081571
+0.027818699522513785
+0.029180792563121144
+0.030582721909713173
+0.0320247498958449
+0.03350712126648277
+0.035030063070651374
+0.03659378456184233
+0.03819847710622874
+0.039844314098725686
+0.04153145088693344
+0.043260024702996905
+0.04503015460341113
+0.04684194141679901
+0.04869546769968476
+0.050590797700281645
+0.052527977330310824
+0.054507034144862784
+0.05652797733031083
+0.05859079770028165
+0.06069546769968477
+0.06284194141679902
+0.06503015460341113
+0.06726002470299691
+0.06953145088693341
+0.07184431409872565
+0.0741984771062287
+0.07659378456184229
+0.07903006307065134
+0.08150712126648274
+0.08402474989584491
+0.08658272190971318
+0.08918079256312114
+0.0918186995225138
+0.09449616298081573
+0.09721288578016374
+0.0999685535422501
+0.10276283480621937
+0.10559538117405791
+0.10846582746341286
+0.1113737918677732
+0.11431887612394266
+0.11730066568673116
+0.12031872991078801
+0.12337262223949721
+0.12646188040085193
+0.12958602661022225
+0.1327445677799274
+0.13593699573552015
+0.13916278743868915
+0.1424214052166805
+0.14571229699813915
+0.1490348965552653
+0.15238862375218093
+0.15577288479939633
+0.1591870725142659
+0.16263056658731867
+0.16610273385434624
+0.16960292857412967
+0.17313049271168296
+0.17668475622688895
+0.180265037368401
+0.18387064297268157
+0.18750086876804695
+0.1911549996835838
+0.194832310162803
+0.19853206448189276
+0.20225351707243133
+0.20599591284841798
+0.20975848753747856
+0.21354046801610088
+0.217341072648752
+0.22115951163072947
+0.22499498733459586
+0.22884669466004437
+0.23271382138704264
+0.23659554853209902
+0.24049105070749602
+0.24439949648333265
+0.24832004875221683
+0.25225186509644854
+0.2561940981575322
+0.2601458960078558
+0.2641064025243746
+0.26807475776413514
+0.27205009834147453
+0.2760315578067299
+0.28001826702629196
+0.28400935456383564
+0.28800394706256094
+0.29200116962827594
+0.2960001462131546
+0.3
+0.3039998537868453
+0.30799883037172393
+0.311996052937439
+0.3159906454361643
+0.31998173297370797
+0.32396844219327003
+0.3279499016585254
+0.3319252422358647
+0.3358935974756253
+0.3398541039921441
+0.34380590184246773
+0.3477481349035514
+0.35167995124778306
+0.3556005035166673
+0.3595089492925039
+0.3634044514679009
+0.3672861786129573
+0.37115330533995555
+0.3750050126654041
+0.3788404883692704
+0.38265892735124796
+0.38645953198389904
+0.39024151246252137
+0.39400408715158197
+0.39774648292756853
+0.4014679355181071
+0.40516768983719686
+0.40884500031641613
+0.4124991312319529
+0.41612935702731835
+0.41973496263159893
+0.4233152437731109
+0.42686950728831696
+0.43039707142587025
+0.43389726614565366
+0.43736943341268125
+0.44081292748573403
+0.44422711520060354
+0.447611376247819
+0.4509651034447346
+0.4542877030018608
+0.4575785947833194
+0.46083721256131077
+0.4640630042644797
+0.46725543222007254
+0.47041397338977764
+0.473538119599148
+0.4766273777605027
+0.4796812700892119
+0.4826993343132687
+0.48568112387605716
+0.48862620813222674
+0.491534172536587
+0.494404618825942
+0.4972371651937805
+0.5000314464577498
+0.5027871142198361
+0.5055038370191842
+0.5081813004774861
+0.5108192074368788
+0.5134172780902867
+0.515975250104155
+0.5184928787335171
+0.5209699369293487
+0.5234062154381576
+0.5258015228937712
+0.5281556859012743
+0.5304685491130665
+0.532739975297003
+0.5349698453965889
+0.5371580585832009
+0.5393045323003153
+0.5414092022997183
+0.5434720226696892
+0.5454929658551372
+0.5474720226696891
+0.5494092022997183
+0.5513045323003152
+0.553158058583201
+0.5549698453965888
+0.556739975297003
+0.5584685491130665
+0.5601556859012743
+0.5618015228937712
+0.5634062154381577
+0.5649699369293486
+0.5664928787335173
+0.5679752501041551
+0.5694172780902869
+0.5708192074368788
+0.5721813004774862
+0.5735038370191843
+0.5747871142198363
+0.5760314464577498
+0.5772371651937807
+0.578404618825942
+0.5795341725365871
+0.5806262081322268
+0.5816811238760573
+0.5826993343132688
+0.5836812700892119
+0.5846273777605028
+0.585538119599148
+0.5864139733897777
+0.5872554322200726
+0.5880630042644799
+0.5888372125613108
+0.5895785947833195
+0.5902877030018608
+0.5909651034447347
+0.591611376247819
+0.5922271152006038
+0.592812927485734
+0.5933694334126813
+0.5938972661456536
+0.5943970714258703
+0.5948695072883169
+0.595315243773111
+0.5957349626315989
+0.5961293570273184
+0.596499131231953
+0.5968450003164162
+0.5971676898371969
+0.5974679355181072
+0.5977464829275686
+0.598004087151582
+0.5982415124625213
+0.5984595319838991
+0.5986589273512479
+0.5988404883692705
+0.5990050126654041
+0.5991533053399556
+0.5992861786129573
+0.599404451467901
+0.599508949292504
+0.5996005035166674
+0.5996799512477832
+0.5997481349035514
+0.5998059018424677
+0.5998541039921441
+0.5998935974756253
+0.5999252422358647
+0.5999499016585254
+0.5999684421932701
+0.5999817329737079
+0.5999906454361643
+0.599996052937439
+0.599998830371724
+0.5999998537868453
+0.6
diff --git a/control_systems/Control System/gaitGen/zValues.txt b/control_systems/Control System/gaitGen/zValues.txt
new file mode 100644
index 0000000..99405c3
--- /dev/null
+++ b/control_systems/Control System/gaitGen/zValues.txt
@@ -0,0 +1,301 @@
+0.0
+7.79752183994141e-07
+6.236375890451319e-06
+2.1038537819973585e-05
+4.9838509423480795e-05
+9.726400523724608e-05
+0.00016791006429906982
+0.0002663309888884177
+0.0003970323547326689
+0.0005644631066962641
+0.0007730077538196621
+0.0010269786774006057
+0.001330608565612009
+0.0016880429879285591
+0.0021033331223892435
+0.0025804286484544456
+0.003123170817926034
+0.0037352857160864936
+0.004420377724879167
+0.005181923199597559
+0.006023264370176876
+0.006947603477787008
+0.00795799715701346
+0.009057351073481532
+0.010248414826331508
+0.011533777124487464
+0.012915861245182153
+0.01439692078270531
+0.015979035694833436
+0.01766410865387715
+0.019453861708747446
+0.02134983326389662
+0.02335337538043425
+0.02546565140415251
+0.0276876339246223
+0.03002010306894075
+0.03246364513312318
+0.03501865155354056
+0.03768531822020722
+0.04046364513312317
+0.0433534364022741
+0.04635430059128895
+0.049465651404152475
+0.05268670871376758
+0.05601649993056328
+0.05945386170874743
+0.0629974419872105
+0.06664570236150008
+0.07039692078270529
+0.07424919457851548
+0.07820044379115412
+0.0822484148263315
+0.08639068440681484
+0.09062466382368012
+0.09494760347778702
+0.0993565977035102
+0.10384858986626423
+0.10842037772487914
+0.11306861904941978
+0.11778983748459265
+0.12258042864845442
+0.12743666645572255
+0.1323547096545952
+0.137330608565612
+0.1423603120107339
+0.1474396744204863
+0.15256446310669627
+0.157730365688066
+0.16293299765555508
+0.16816791006429904
+0.17343059733857052
+0.1787165051760901
+0.18402103853781995
+0.18933956970922375
+0.19466744641885064
+0.2
+0.2053325535811493
+0.2106604302907762
+0.21597896146217999
+0.22128349482390983
+0.22656940266142941
+0.23183208993570092
+0.23706700234444486
+0.24226963431193396
+0.2474355368933037
+0.25256032557951363
+0.25763968798926606
+0.26266939143438794
+0.26764529034540474
+0.2725633335442774
+0.27741957135154555
+0.2822101625154073
+0.2869313809505802
+0.2915796222751208
+0.29615141013373575
+0.3006434022964897
+0.30505239652221294
+0.30937533617631985
+0.3136093155931851
+0.31775158517366847
+0.32179955620884587
+0.3257508054214845
+0.3296030792172947
+0.3333542976384999
+0.3370025580127895
+0.3405461382912526
+0.34398350006943673
+0.3473132912862324
+0.35053434859584753
+0.35364569940871105
+0.3566465635977259
+0.35953635486687685
+0.3623146817797928
+0.3649813484464594
+0.3675363548668768
+0.3699798969310592
+0.37231236607537765
+0.37453434859584755
+0.3766466246195658
+0.37865016673610336
+0.3805461382912526
+0.38233589134612284
+0.3840209643051666
+0.38560307921729475
+0.38708413875481784
+0.3884662228755125
+0.38975158517366854
+0.39094264892651853
+0.39204200284298657
+0.393052396522213
+0.39397673562982316
+0.3948180768004025
+0.3955796222751209
+0.3962647142839135
+0.396876829182074
+0.3974195713515456
+0.39789666687761077
+0.39831195701207145
+0.39866939143438807
+0.39897302132259943
+0.39922699224618036
+0.3994355368933038
+0.39960296764526737
+0.39973366901111157
+0.39983208993570096
+0.3999027359947628
+0.39995016149057655
+0.3999789614621801
+0.39999376362410954
+0.39999922024781603
+0.4
+0.399999220247816
+0.39999376362410954
+0.39997896146218004
+0.3999501614905765
+0.3999027359947627
+0.39983208993570096
+0.39973366901111157
+0.3996029676452673
+0.39943553689330374
+0.3992269922461803
+0.3989730213225994
+0.398669391434388
+0.39831195701207145
+0.39789666687761077
+0.3974195713515456
+0.39687682918207395
+0.3962647142839135
+0.3955796222751209
+0.3948180768004025
+0.39397673562982316
+0.393052396522213
+0.39204200284298657
+0.39094264892651853
+0.38975158517366854
+0.3884662228755126
+0.3870841387548179
+0.38560307921729475
+0.3840209643051666
+0.3823358913461229
+0.3805461382912526
+0.3786501667361034
+0.3766466246195658
+0.37453434859584755
+0.3723123660753777
+0.36997989693105926
+0.36753635486687686
+0.36498134844645946
+0.36231468177979287
+0.35953635486687685
+0.35664656359772606
+0.35364569940871116
+0.35053434859584753
+0.34731329128623256
+0.3439835000694368
+0.3405461382912526
+0.3370025580127896
+0.3333542976385
+0.32960307921729476
+0.3257508054214846
+0.3217995562088459
+0.3177515851736687
+0.3136093155931853
+0.30937533617632007
+0.30505239652221305
+0.30064340229648995
+0.2961514101337358
+0.291579622275121
+0.28693138095058024
+0.28221016251540754
+0.2774195713515456
+0.27256333354427753
+0.26764529034540485
+0.26266939143438817
+0.2576396879892662
+0.2525603255795139
+0.24743553689330366
+0.24226963431193416
+0.2370670023444449
+0.23183208993570104
+0.22656940266142944
+0.22128349482390997
+0.21597896146217999
+0.2106604302907763
+0.20533255358114932
+0.20000000000000007
+0.19466744641885092
+0.18933956970922394
+0.18402103853782015
+0.17871650517609017
+0.1734305973385707
+0.1681679100642991
+0.16293299765555525
+0.1577303656880661
+0.15256446310669647
+0.14743967442048644
+0.14236031201073418
+0.13733060856561208
+0.1323547096545955
+0.1274366664557227
+0.12258042864845453
+0.11778983748459271
+0.11306861904942
+0.10842037772487922
+0.10384858986626444
+0.09935659770351024
+0.09494760347778708
+0.09062466382368017
+0.08639068440681497
+0.0822484148263315
+0.07820044379115432
+0.07424919457851575
+0.07039692078270532
+0.06664570236150025
+0.06299744198721058
+0.05945386170874756
+0.056016499930563346
+0.052686708713767794
+0.04946565140415249
+0.04635430059128898
+0.04335343640227407
+0.04046364513312328
+0.037685318220207265
+0.03501865155354067
+0.032463645133123165
+0.030020103068940762
+0.027687633924622257
+0.02546565140415258
+0.02335337538043425
+0.021349833263896723
+0.01945386170874741
+0.01766410865387713
+0.015979035694833388
+0.01439692078270538
+0.01291586124518218
+0.011533777124487554
+0.010248414826331542
+0.00905735107348149
+0.007957997157013508
+0.006947603477787001
+0.006023264370176973
+0.005181923199597649
+0.004420377724879132
+0.0037352857160865005
+0.003123170817926013
+0.0025804286484544248
+0.002103333122389306
+0.001688042987928573
+0.001330608565611957
+0.0010269786774005363
+0.0007730077538196101
+0.0005644631066962225
+0.0003970323547326515
+0.0002663309888885079
+0.00016791006429905941
+9.726400523724088e-05
+4.983850942352763e-05
+2.1038537819983993e-05
+6.236375890478207e-06
+7.797521840435806e-07
+0.0
diff --git a/control_systems/Control System/instructions.py b/control_systems/Control System/instructions.py
new file mode 100644
index 0000000..3d38c27
--- /dev/null
+++ b/control_systems/Control System/instructions.py
@@ -0,0 +1,4 @@
+# Use methods online to dehash data packages/ instructions
+# Add instruction to priority queue
+
+def readInstruction
\ No newline at end of file
diff --git a/control_systems/Control System/main.py b/control_systems/Control System/main.py
new file mode 100644
index 0000000..5b5d9a9
--- /dev/null
+++ b/control_systems/Control System/main.py
@@ -0,0 +1,8 @@
+import sys
+from bootup import *
+
+def main():
+ startBoot() # when Olympian is booted up for first time
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/control_systems/LICENSE b/control_systems/LICENSE
new file mode 100644
index 0000000..0ad25db
--- /dev/null
+++ b/control_systems/LICENSE
@@ -0,0 +1,661 @@
+ GNU AFFERO GENERAL PUBLIC LICENSE
+ Version 3, 19 November 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc.
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The GNU Affero General Public License is a free, copyleft license for
+software and other kinds of works, specifically designed to ensure
+cooperation with the community in the case of network server software.
+
+ The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works. By contrast,
+our General Public Licenses are intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+ Developers that use our General Public Licenses protect your rights
+with two steps: (1) assert copyright on the software, and (2) offer
+you this License which gives you legal permission to copy, distribute
+and/or modify the software.
+
+ A secondary benefit of defending all users' freedom is that
+improvements made in alternate versions of the program, if they
+receive widespread use, become available for other developers to
+incorporate. Many developers of free software are heartened and
+encouraged by the resulting cooperation. However, in the case of
+software used on network servers, this result may fail to come about.
+The GNU General Public License permits making a modified version and
+letting the public access it on a server without ever releasing its
+source code to the public.
+
+ The GNU Affero General Public License is designed specifically to
+ensure that, in such cases, the modified source code becomes available
+to the community. It requires the operator of a network server to
+provide the source code of the modified version running there to the
+users of that server. Therefore, public use of a modified version, on
+a publicly accessible server, gives the public access to the source
+code of the modified version.
+
+ An older license, called the Affero General Public License and
+published by Affero, was designed to accomplish similar goals. This is
+a different license, not a version of the Affero GPL, but Affero has
+released a new version of the Affero GPL which permits relicensing under
+this license.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ TERMS AND CONDITIONS
+
+ 0. Definitions.
+
+ "This License" refers to version 3 of the GNU Affero General Public License.
+
+ "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+ "The Program" refers to any copyrightable work licensed under this
+License. Each licensee is addressed as "you". "Licensees" and
+"recipients" may be individuals or organizations.
+
+ To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy. The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+ A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+ To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy. Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+ To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies. Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+ An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License. If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+ 1. Source Code.
+
+ The "source code" for a work means the preferred form of the work
+for making modifications to it. "Object code" means any non-source
+form of a work.
+
+ A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+ The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form. A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+ The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities. However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work. For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+ The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+ The Corresponding Source for a work in source code form is that
+same work.
+
+ 2. Basic Permissions.
+
+ All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met. This License explicitly affirms your unlimited
+permission to run the unmodified Program. The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work. This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+ You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force. You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright. Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+ Conveying under any other circumstances is permitted solely under
+the conditions stated below. Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+ 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+ No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+ When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+ 4. Conveying Verbatim Copies.
+
+ You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+ You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+ 5. Conveying Modified Source Versions.
+
+ You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+ a) The work must carry prominent notices stating that you modified
+ it, and giving a relevant date.
+
+ b) The work must carry prominent notices stating that it is
+ released under this License and any conditions added under section
+ 7. This requirement modifies the requirement in section 4 to
+ "keep intact all notices".
+
+ c) You must license the entire work, as a whole, under this
+ License to anyone who comes into possession of a copy. This
+ License will therefore apply, along with any applicable section 7
+ additional terms, to the whole of the work, and all its parts,
+ regardless of how they are packaged. This License gives no
+ permission to license the work in any other way, but it does not
+ invalidate such permission if you have separately received it.
+
+ d) If the work has interactive user interfaces, each must display
+ Appropriate Legal Notices; however, if the Program has interactive
+ interfaces that do not display Appropriate Legal Notices, your
+ work need not make them do so.
+
+ A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit. Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+ 6. Conveying Non-Source Forms.
+
+ You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+ a) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by the
+ Corresponding Source fixed on a durable physical medium
+ customarily used for software interchange.
+
+ b) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by a
+ written offer, valid for at least three years and valid for as
+ long as you offer spare parts or customer support for that product
+ model, to give anyone who possesses the object code either (1) a
+ copy of the Corresponding Source for all the software in the
+ product that is covered by this License, on a durable physical
+ medium customarily used for software interchange, for a price no
+ more than your reasonable cost of physically performing this
+ conveying of source, or (2) access to copy the
+ Corresponding Source from a network server at no charge.
+
+ c) Convey individual copies of the object code with a copy of the
+ written offer to provide the Corresponding Source. This
+ alternative is allowed only occasionally and noncommercially, and
+ only if you received the object code with such an offer, in accord
+ with subsection 6b.
+
+ d) Convey the object code by offering access from a designated
+ place (gratis or for a charge), and offer equivalent access to the
+ Corresponding Source in the same way through the same place at no
+ further charge. You need not require recipients to copy the
+ Corresponding Source along with the object code. If the place to
+ copy the object code is a network server, the Corresponding Source
+ may be on a different server (operated by you or a third party)
+ that supports equivalent copying facilities, provided you maintain
+ clear directions next to the object code saying where to find the
+ Corresponding Source. Regardless of what server hosts the
+ Corresponding Source, you remain obligated to ensure that it is
+ available for as long as needed to satisfy these requirements.
+
+ e) Convey the object code using peer-to-peer transmission, provided
+ you inform other peers where the object code and Corresponding
+ Source of the work are being offered to the general public at no
+ charge under subsection 6d.
+
+ A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+ A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling. In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage. For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product. A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+ "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source. The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+ If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information. But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+ The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed. Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+ Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+ 7. Additional Terms.
+
+ "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law. If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+ When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it. (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.) You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+ Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+ a) Disclaiming warranty or limiting liability differently from the
+ terms of sections 15 and 16 of this License; or
+
+ b) Requiring preservation of specified reasonable legal notices or
+ author attributions in that material or in the Appropriate Legal
+ Notices displayed by works containing it; or
+
+ c) Prohibiting misrepresentation of the origin of that material, or
+ requiring that modified versions of such material be marked in
+ reasonable ways as different from the original version; or
+
+ d) Limiting the use for publicity purposes of names of licensors or
+ authors of the material; or
+
+ e) Declining to grant rights under trademark law for use of some
+ trade names, trademarks, or service marks; or
+
+ f) Requiring indemnification of licensors and authors of that
+ material by anyone who conveys the material (or modified versions of
+ it) with contractual assumptions of liability to the recipient, for
+ any liability that these contractual assumptions directly impose on
+ those licensors and authors.
+
+ All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10. If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term. If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+ If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+ Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+ 8. Termination.
+
+ You may not propagate or modify a covered work except as expressly
+provided under this License. Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+ However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+ Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+ Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License. If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+ 9. Acceptance Not Required for Having Copies.
+
+ You are not required to accept this License in order to receive or
+run a copy of the Program. Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance. However,
+nothing other than this License grants you permission to propagate or
+modify any covered work. These actions infringe copyright if you do
+not accept this License. Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+ 10. Automatic Licensing of Downstream Recipients.
+
+ Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License. You are not responsible
+for enforcing compliance by third parties with this License.
+
+ An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations. If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+ You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License. For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+ 11. Patents.
+
+ A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based. The
+work thus licensed is called the contributor's "contributor version".
+
+ A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version. For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+ Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+ In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement). To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+ If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients. "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+ If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+ A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License. You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+ Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+ 12. No Surrender of Others' Freedom.
+
+ If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all. For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+ 13. Remote Network Interaction; Use with the GNU General Public License.
+
+ Notwithstanding any other provision of this License, if you modify the
+Program, your modified version must prominently offer all users
+interacting with it remotely through a computer network (if your version
+supports such interaction) an opportunity to receive the Corresponding
+Source of your version by providing access to the Corresponding Source
+from a network server at no charge, through some standard or customary
+means of facilitating copying of software. This Corresponding Source
+shall include the Corresponding Source for any work covered by version 3
+of the GNU General Public License that is incorporated pursuant to the
+following paragraph.
+
+ Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU General Public License into a single
+combined work, and to convey the resulting work. The terms of this
+License will continue to apply to the part which is the covered work,
+but the work with which it is combined will remain governed by version
+3 of the GNU General Public License.
+
+ 14. Revised Versions of this License.
+
+ The Free Software Foundation may publish revised and/or new versions of
+the GNU Affero General Public License from time to time. Such new versions
+will be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+ Each version is given a distinguishing version number. If the
+Program specifies that a certain numbered version of the GNU Affero General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation. If the Program does not specify a version number of the
+GNU Affero General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+ If the Program specifies that a proxy can decide which future
+versions of the GNU Affero General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+ Later license versions may give you additional or different
+permissions. However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+ 15. Disclaimer of Warranty.
+
+ THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+ 16. Limitation of Liability.
+
+ IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+ 17. Interpretation of Sections 15 and 16.
+
+ If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+ END OF TERMS AND CONDITIONS
+
+ How to Apply These Terms to Your New Programs
+
+ If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+ To do so, attach the following notices to the program. It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+
+ Copyright (C)
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Affero General Public License as published
+ by the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Affero General Public License for more details.
+
+ You should have received a copy of the GNU Affero General Public License
+ along with this program. If not, see .
+
+Also add information on how to contact you by electronic and paper mail.
+
+ If your software can interact with users remotely through a computer
+network, you should also make sure that it provides a way for users to
+get its source. For example, if your program is a web application, its
+interface could display a "Source" link that leads users to an archive
+of the code. There are many ways you could offer source, and different
+solutions will be better for different programs; see section 13 for the
+specific requirements.
+
+ You should also get your employer (if you work as a programmer) or school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.
+For more information on this, and how to apply and follow the GNU AGPL, see
+.
diff --git a/control_systems/README.md b/control_systems/README.md
new file mode 100644
index 0000000..57bca34
--- /dev/null
+++ b/control_systems/README.md
@@ -0,0 +1,2 @@
+# Control System
+Control loops that monitor and manage the robot
diff --git a/control_systems/client1.py b/control_systems/client1.py
new file mode 100644
index 0000000..dd4a3a5
--- /dev/null
+++ b/control_systems/client1.py
@@ -0,0 +1,27 @@
+import socket
+import threading
+
+HOST = '10.235.1.145' # Enter IP or Hostname of your server
+PORT = 12345 # Pick an open Port (1000+ recommended), must match the server port
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+s.connect((HOST,PORT))
+
+#Lets loop awaiting for your input
+
+def telemetryReceive():
+ while True:
+ reply = s.recv(1024)
+ if reply != None:
+ print(reply)
+
+def telemetrySend():
+ while True:
+ command = input('Enter your command: ')
+ if command != None:
+ s.send(bytes(command, 'utf-8'))
+
+def main():
+ threading.Thread(target=telemetryReceive).start()
+ threading.Thread(target=telemetrySend).start()
+
+main()
\ No newline at end of file
diff --git a/control_systems/server1.py b/control_systems/server1.py
new file mode 100644
index 0000000..6eceaf0
--- /dev/null
+++ b/control_systems/server1.py
@@ -0,0 +1,26 @@
+import socket
+
+HOST = '10.235.1.127' # Use this for client
+PORT = 12345 # Pick an open Port (1000+ recommended), must match the client sport
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+print('Socket created')
+
+#managing error exception
+try:
+ s.bind((HOST, 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:
+ data = conn.recv(1024)
+ print (data)
+
+ # # Sending reply
+ conn.send(data+bytes(" works",'utf-8'))
+ # conn.close() # Close connections