diff --git a/controller/relayer.py b/controller/relayer.py
new file mode 100755
index 0000000000000000000000000000000000000000..6fec5c32b6cbe5587e88e670cfe1137ab9f1a658
--- /dev/null
+++ b/controller/relayer.py
@@ -0,0 +1,96 @@
+#!/usr/bin/python
+
+import serial
+import struct
+import pygame
+import math
+import socket
+
+#################################################
+# BEGIN SETTINGS
+#################################################
+
+# Serial
+#PORT = '/dev/ttyUSB0'
+PORT = '/dev/ttyACM0'
+BAUDRATE = 57600
+TIMEOUT = 0.2
+LIVE = True
+
+# Initialize Serial
+if LIVE:
+ ser = serial.Serial()
+ ser.port = PORT
+ ser.baudrate = BAUDRATE
+ ser.timeout = TIMEOUT
+ ser.write_timeout = 1
+
+ ser.open()
+
+HOST = '' # Symbolic name meaning all available interfaces
+PORT = 50007 # Arbitrary non-privileged port
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+s.bind((HOST, PORT))
+print 'listening for sender...'
+s.listen(1)
+conn, addr = s.accept()
+print 'Connected by', addr
+while 1:
+ data = conn.recv(4096)
+ print data
+ #data = map(int, data.split('\r\n')[0].split(',')) #.rstrip('\r\n')
+ data = data.rstrip('\r\n').split('\r\n') #split each send as item in list
+ if (len(data) > 1 ):
+ print('I am not fast enough')
+ print data
+ print len(data)
+ data = 0
+ continue
+ data = data[0]
+ data = map(int, data.split(','))
+ print data
+ if (data[0] == 98 and data[11] == 101):
+ ser.reset_output_buffer()
+ ser.reset_input_buffer()
+ ser.write(data)
+ if not data: break
+ data = 0
+
+ #conn.sendall(data)
+conn.close()
+
+
+
+# Main program loop
+run = True
+while run:
+
+ # Set mData based on jData
+ processInput(jData, jData2, mData, macros)
+
+ # Make sure data is bounded
+ sanityCheck(mData)
+
+ # Write data to serial
+ if LIVE:
+ writeSerialData(ser, mData)
+
+ # print("Reading")
+ if ser.readline():
+ print("OK")
+ else:
+ print("DATA LOSS")
+ mData.dataLoss += 1
+
+ drawText(s, jData, jData2, mData, macros)
+ drawMeters(s, jData, jData2, mData)
+
+ pygame.display.flip()
+ clock.tick(FRAMERATE)
+
+if LIVE:
+ if(ser.isOpen()):
+ ser.close()
+
+if __name__ == "__main__":
+ main()
diff --git a/controller/sender_udp_v2.py b/controller/sender_udp_v2.py
new file mode 100755
index 0000000000000000000000000000000000000000..9acc9b7dddc4a8a38c3cf123aa2def1a9f047cfa
--- /dev/null
+++ b/controller/sender_udp_v2.py
@@ -0,0 +1,554 @@
+#!/usr/bin/python
+
+import serial
+import struct
+import pygame
+import math
+import socket
+
+# IP setup
+
+HOST = '10.9.0.3' # The remote host
+PORT = 50007 # The same port as used by the server
+
+
+#################################################
+# BEGIN SETTINGS
+#################################################
+
+# Pygame
+WIDTH = 800
+HEIGHT = 600
+FRAMERATE = 20
+
+# Serial
+#PORT = '/dev/ttyUSB0'
+#PORT = '/dev/ttyACM0'
+#BAUDRATE = 57600
+#TIMEOUT = 0.2
+LIVE = True
+
+# Joystick
+JTHRESH = 0.1
+JTHRESH_TURN = 0.01
+
+# Wat
+MACRO = pygame.USEREVENT + 1
+
+#################################################
+# END SETTINGS
+#################################################
+
+# Joystick data
+class JoystickData:
+ numJoys = 0
+ macro = 0
+
+ def __init__(self):
+ self.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ self.axis = [0.0, 0.0, -1.0, 0.0, 0.0, -1.0]
+ self.hat = (0, 0)
+ JoystickData.numJoys += 1
+
+# Motor data / Serial Data
+class MotorData:
+ numMotors = 0
+ macro = 0
+ macroRunning = 0
+ dataLoss = 0
+
+ def __init__(self):
+ self.lmotor = (0, 0)
+ self.rmotor = (0, 0)
+ self.servos = [0, 0, 0, 0, 0, 0]
+ MotorData.numMotors += 1
+
+class Macro:
+ numMacros = 0
+ timestep = 0
+
+ def __init__(self, macroid, name, actions):
+ self.macroid = macroid
+ self.name = name
+ self.actions = actions
+ Macro.numMacros += 1
+
+
+# data = bytearray(b'\x00' * (2 + 4 + 10))
+
+# Joystick Dictionary
+jBtnName = ['A', 'B', 'X', 'Y', 'LB', 'RB', 'Select', 'Start', 'Logo', 'LS', 'RS']
+jBtn = {jBtnName[x]: x for x in range(len(jBtnName))}
+
+jAxisName = ['LSx', 'LSy', 'LT', 'RSx', 'RSy', 'RT']
+jAxis = {jAxisName[x]: x for x in range(len(jAxisName))}
+
+# Sets jData based on detected events
+def handleJoyEvent(e, jData, jData2):
+ if e.joy == 0:
+ if e.type == pygame.JOYAXISMOTION:
+ jData.axis[e.axis] = e.value
+
+ if e.type == pygame.JOYBUTTONDOWN:
+ jData.buttons[e.button] = 1
+
+ if e.type == pygame.JOYBUTTONUP:
+ jData.buttons[e.button] = 0
+
+ if e.type == pygame.JOYHATMOTION:
+ jData.hat = e.value
+
+ if e.joy == 1:
+ if e.type == pygame.JOYAXISMOTION:
+ jData2.axis[e.axis] = e.value
+
+ if e.type == pygame.JOYBUTTONDOWN:
+ jData2.buttons[e.button] = 1
+
+ if e.type == pygame.JOYBUTTONUP:
+ jData2.buttons[e.button] = 0
+
+ if e.type == pygame.JOYHATMOTION:
+ jData2.hat = e.value
+
+ else:
+ pass
+
+# Sets mData based on jData
+def processInput(jData, jData2, mData, macros):
+ sfactor = 235 #100
+ turnfactor = 180
+
+ # arm, claw, bottom, top, cagelock, droplock
+
+ #---------------------
+ # JOYSTICK 0
+ #---------------------
+
+ # Hat
+ # if jData.hat[1] == 1:
+ # mData.servos[3] += 2
+ # elif jData.hat[1] == -1:
+ # mData.servos[3] -= 2
+ # elif jData.hat[0] == 1:
+ # mData.servos[2] += 2
+ # mData.servos[3] += 2
+ # elif jData.hat[0] == -1:
+ # mData.servos[2] -= 2
+ # mData.servos[3] -= 2
+
+ # Joystick buttons
+ if jData.buttons[jBtn['A']] == 1:
+ mData.servos[1] = 100
+ elif jData.buttons[jBtn['B']] == 1:
+ mData.servos[1] = 45
+
+ # Joystick triggers
+ if jData.axis[jAxis['LT']] > -1 + JTHRESH:
+ mData.servos[0] += 2
+ elif jData.axis[jAxis['RT']] > -1 + JTHRESH:
+ mData.servos[0] -= 2
+
+ # Prevent multiple macros
+ if mData.macroRunning == 0:
+ # Macros
+ if jData.buttons[jBtn['Y']] == 1:
+ mData.macro = 0
+ mData.macroRunning = 1
+ playMacro(macros[mData.macro], mData)
+ pygame.time.set_timer(MACRO, 100)
+ elif jData.buttons[jBtn['Start']] == 1:
+ mData.macro = 1
+ mData.macroRunning = 1
+ playMacro(macros[mData.macro], mData)
+ pygame.time.set_timer(MACRO, 100)
+ elif jData.buttons[jBtn['Select']] == 1:
+ mData.macro = 2
+ mData.macroRunning = 1
+ playMacro(macros[mData.macro], mData)
+ pygame.time.set_timer(MACRO, 100)
+
+ # Turbo mode
+ if jData.buttons[jBtn['RB']] == 1:
+ offset = 255 - sfactor
+ else:
+ offset = 10
+
+ '''
+ # R/L stick
+ if jData.axis[jAxis['LSy']] > 0 + JTHRESH:
+ mData.lmotor = (int(math.floor(sfactor * jData.axis[jAxis['LSy']]) + offset), 0)
+ elif jData.axis[jAxis['LSy']] < 0 - JTHRESH:
+ mData.lmotor = (int(-math.floor(sfactor * jData.axis[jAxis['LSy']]) + offset - 1), 1)
+ else:
+ mData.lmotor = (0, 0)
+
+ if jData.axis[jAxis['RSy']] > 0 + JTHRESH:
+ mData.rmotor = (int(math.floor(sfactor * jData.axis[jAxis['RSy']]) + offset), 0)
+ elif jData.axis[jAxis['RSy']] < 0 - JTHRESH:
+ mData.rmotor = (int(-math.floor(sfactor * jData.axis[jAxis['RSy']]) + offset - 1), 1)
+ else:
+ mData.rmotor = (0, 0)
+ '''
+ drive = (0, 0)
+ #print(drive)
+
+ if jData.axis[jAxis['LSy']] > 0 + JTHRESH:
+ drive = (int(math.floor(sfactor * jData.axis[jAxis['LSy']]) + offset), 0)
+ elif jData.axis[jAxis['LSy']] < 0 - JTHRESH:
+ drive = (int(-math.floor(sfactor * jData.axis[jAxis['LSy']]) + offset - 1), 1)
+ else:
+ drive = (0, 1)
+
+ mData.lmotor = drive
+ mData.rmotor = drive
+
+ turn = (0,0)
+
+ if jData.axis[jAxis['RSx']] > 0 + JTHRESH_TURN:
+ turn = (int(math.floor(turnfactor * jData.axis[jAxis['RSx']])), 0)
+ elif jData.axis[jAxis['RSx']] < 0 - JTHRESH_TURN:
+ turn = (int(-math.floor(turnfactor * jData.axis[jAxis['RSx']]) - 1), 1)
+ else:
+ turn = (0, 1)
+
+ if drive != (0, 1):
+ mData.lmotor = (sorted([0, (mData.lmotor[0] + pow(-1,turn[1])*turn[0]), 255])[1], mData.lmotor[1])
+ mData.rmotor = (sorted([0, (mData.rmotor[0] - pow(-1,turn[1])*turn[0]), 255])[1], mData.rmotor[1])
+ else:
+ mData.lmotor = (sorted([0, ( turn[0]), 245])[1], 1-turn[1])
+ mData.rmotor = (sorted([0, ( turn[0]), 245])[1], turn[1])
+
+ #---------------------
+ # JOYSTICK 1
+ #---------------------
+
+ # Arm on LT and RT
+ if jData2.axis[jAxis['LT']] > -1 + JTHRESH:
+ mData.servos[0] += 2
+ elif jData2.axis[jAxis['RT']] > -1 + JTHRESH:
+ mData.servos[0] -= 2
+
+ # Claw on A and B
+ if jData2.buttons[jBtn['A']] == 1:
+ mData.servos[1] += 2
+ elif jData2.buttons[jBtn['B']] == 1:
+ mData.servos[1] -= 2
+
+ # Drop lock on X and Y
+ if jData2.buttons[jBtn['X']] == 1:
+ mData.servos[5] -= 2
+ elif jData2.buttons[jBtn['Y']] == 1:
+ mData.servos[5] += 2
+
+ # Cage control on HAT
+ if jData2.hat[1] == 1:
+ mData.servos[2] += 2
+ mData.servos[3] += 2
+ elif jData2.hat[1] == -1:
+ mData.servos[2] -= 2
+ mData.servos[3] -= 2
+
+ # Cage control on LStick
+ if jData2.axis[jAxis['LSy']] > 0 + JTHRESH:
+ mData.servos[3] += 1
+ elif jData2.axis[jAxis['LSy']] < 0 - JTHRESH:
+ mData.servos[3] -= 1
+
+ # Cage lock on RStick
+ if jData2.axis[jAxis['RSy']] > 0 + JTHRESH:
+ mData.servos[4] -= 2
+ elif jData2.axis[jAxis['RSy']] < 0 - JTHRESH:
+ mData.servos[4] += 2
+
+
+def playMacro(macro, mData):
+ # [i](servoid, pos, timestep)
+
+ for i in range(len(macro.actions)):
+ if macro.actions[i][2] == macro.timestep:
+ mData.servos[macro.actions[i][0]] = macro.actions[i][1]
+
+ macro.timestep += 1
+
+ temp = 0
+ for i in range(len(macro.actions)):
+ if temp < macro.actions[i][2]:
+ temp = macro.actions[i][2]
+
+ for i in range(len(macro.actions)):
+ if macro.timestep > temp:
+ pygame.time.set_timer(MACRO, 0)
+ macro.timestep = 0
+ mData.macroRunning = 0
+
+def sanityCheck(mData):
+ # Make sure servos are bounded
+
+ #minValues = [22, 45, 0, 0, 50, 30]
+ #maxValues = [160, 100, 180, 180, 140, 110]
+
+ minValues = [22, 0, 0, 0, 50, 30]
+ maxValues = [160, 180, 180, 180, 140, 110]
+
+ for i in range(len(mData.servos)):
+ if mData.servos[i] > maxValues[i]:
+ mData.servos[i] = maxValues[i]
+ elif mData.servos[i] < minValues[i]:
+ mData.servos[i] = minValues[i]
+
+#def writeSerialData(ser, mData):
+# ser.reset_output_buffer()
+# ser.reset_input_buffer()
+#
+# # b (speed, direction)*2 servo*6 e
+# data = [98, mData.lmotor[0], mData.lmotor[1], mData.rmotor[0], mData.rmotor[1]]
+# data += list(mData.servos[x] for x in range(len(mData.servos)))
+# data.append(101)
+#
+# #print(data)
+#
+# ser.write(data)
+
+def writeSockData(sock, mData):
+ # b (speed, direction)*2 servo*6 e
+ data = [98, mData.lmotor[0], mData.lmotor[1], mData.rmotor[0], mData.rmotor[1]]
+ data += list(mData.servos[x] for x in range(len(mData.servos)))
+ data.append(101)
+ data = ','.join(str(e) for e in data)
+ data = data+'\r\n'
+
+ print(data)
+ #sock.sendall(data)
+ sock.sendto(data, (HOST, PORT))
+
+def drawText(s, jData, jData2, mData, macro):
+ font = pygame.font.SysFont("DejaVu Sans Mono", 18)
+
+ color = (255, 255, 255)
+ bgcolor = (0, 0, 0)
+
+ for i in range(len(jData.buttons)):
+ string = str(jData.buttons[i])
+ text = font.render(string, True, color)
+ s.blit(text, (50, 200 + 20 * i))
+
+ for i in range(len(jData.axis)):
+ string = jData.axis[i]
+ text = font.render("{: 03.2f}".format(string), True, color)
+ s.blit(text, (50, 50 + 20 * i))
+
+ string = str(jData.hat)
+ text = font.render(string, True, color)
+ s.blit(text, (50, 460))
+
+ for i in range(len(jData2.buttons)):
+ string = str(jData2.buttons[i])
+ text = font.render(string, True, color)
+ s.blit(text, (150, 200 + 20 * i))
+
+ for i in range(len(jData2.axis)):
+ string = jData2.axis[i]
+ text = font.render("{: 03.2f}".format(string), True, color)
+ s.blit(text, (150, 50 + 20 * i))
+
+ string = str(jData2.hat)
+ text = font.render(string, True, color)
+ s.blit(text, (150, 460))
+
+ for i in range(len(mData.servos)):
+ string = mData.servos[i]
+ text = font.render("{:03.2f}".format(string), True, color)
+ s.blit(text, (250, 50 + 20 * i))
+
+ string = "M: " + str(mData.macro) + " T: " + str(macro[mData.macro].timestep)
+ text = font.render(string, True, color)
+ s.blit(text, (250, 200))
+
+ string = "Data loss: " + str(mData.dataLoss)
+ text = font.render(string, True, color)
+ s.blit(text, (50, 540))
+
+def drawMeters(s, jData, jData2, mData):
+ color = (255, 255, 255)
+
+ if (jData.axis[jAxis['LSy']] > 0 + JTHRESH
+ or jData.axis[jAxis['LSy']] < 0 - JTHRESH):
+
+ lscolor = (255, 0, 0)
+ else:
+ lscolor = (0, 255, 0)
+
+ if (jData.axis[jAxis['RSy']] > 0 + JTHRESH
+ or jData.axis[jAxis['RSy']] < 0 - JTHRESH):
+
+ rscolor = (255, 0, 0)
+ else:
+ rscolor = (0, 255, 0)
+
+ rh = 500
+ rw = 150
+ rgap = 200
+
+ rx = WIDTH - (2 * rw + 2 * (rgap - rw))
+ ry = (HEIGHT - rh)/2
+
+ for i in range(0, 2):
+ pos = (rx + i * rgap, ry, rw, rh)
+ pygame.draw.rect(s, color, pos, 1)
+
+ if jData.axis[jAxis['LSy']] != 0:
+ pos = (rx + 1, ry + rh/2 + 1, rw - 2, (rh/2) * jData.axis[jAxis['LSy']] - 1)
+ pygame.draw.rect(s, lscolor, pos, 0)
+
+ if jData.axis[jAxis['RSy']] != 0:
+ pos = (rx + 1 + rgap, ry + rh/2 + 1, rw - 2, (rh/2) * jData.axis[jAxis['RSy']])
+ pygame.draw.rect(s, rscolor, pos, 0)
+
+def main():
+ # Initialize PyGame
+ pygame.init()
+ s = pygame.display.set_mode((WIDTH, HEIGHT))
+ pygame.display.set_caption("VOLVO")
+ clock = pygame.time.Clock()
+
+ # Initialize Joystick(s)
+ j = pygame.joystick.Joystick(0)
+ j.init()
+
+ #j2 = pygame.joystick.Joystick(1)
+ #j2.init()
+
+ # Timer for macros
+ clock = pygame.time.Clock()
+
+ # Data
+ jData = JoystickData()
+ jData2 = JoystickData()
+ mData = MotorData()
+
+ for i in range(len(mData.servos)):
+ mData.servos[i] = 90
+
+# # Initialize Serial
+# if LIVE:
+# ser = serial.Serial()
+# ser.port = PORT
+# ser.baudrate = BAUDRATE
+# ser.timeout = TIMEOUT
+# ser.write_timeout = 1
+#
+# ser.open()
+
+ # Initialize Socket
+ if LIVE:
+ sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ #sock.connect((HOST, PORT))
+ #sock.sendall('Hello, permoPi! Regards Sender.')
+
+
+ # Define macros
+ # 0: Arm
+ # 1: Claw
+ # 2: Bottom
+ # 3: Top
+ # 4: Cage
+ # 5: Drop
+ macros = []
+ macros.append(Macro(1, "load", [(0, 150, 5),
+ (2, 110, 0),
+ (3, 110, 0),
+ (4, 50, 0),
+ (5, 30, 0),
+ (1, 45, 15),
+ (4, 140, 25),
+ (0, 90, 15),
+ (3, 90, 20),
+ (3, 110, 30)]))
+
+ macros.append(Macro(2, "drop", [(2, 170, 0),
+ (3, 82, 5),
+ (5, 110, 10),
+ (4, 50, 15)]))
+
+ macros.append(Macro(3, "default", [(0, 45, 0),
+ (1, 90, 0),
+ (2, 90, 0),
+ (3, 90, 0),
+ (4, 50, 0),
+ (5, 30, 0)]))
+
+ # macros.append(Macro(1, "transport",
+ # [(0, 118, 0),
+ # (1, 90, 0),
+ # (2, 8, 0),
+ # (3, 0, 0),
+ # (4, 142, 0),
+ # (5, 28, 0)]
+ # ))
+
+ # macros.append(Macro(2, "drop",
+ # [(2, 170, 0),
+ # (3, 76, 0),
+ # (5, 112, 5),
+ # (4, 48, 7)]
+ # ))
+
+ # macros.append(Macro(3, "postdrop",
+ # [(2, 120, 0),
+ # (3, 26, 0),
+ # (3, 90, 5)]
+ # ))
+
+ # Main program loop
+ run = True
+ while run:
+ # Clear screen
+ s.fill(0)
+
+ # Check for keyboard and joystick events
+ for e in pygame.event.get():
+ if (e.type == pygame.QUIT or
+ e.type == pygame.KEYDOWN and
+ e.key == pygame.K_ESCAPE):
+ run = False
+
+ if (e.type == pygame.JOYAXISMOTION or
+ e.type == pygame.JOYBUTTONDOWN or
+ e.type == pygame.JOYBUTTONUP or
+ e.type == pygame.JOYHATMOTION):
+ handleJoyEvent(e, jData, jData2)
+
+ if e.type == MACRO:
+ playMacro(macros[mData.macro], mData)
+
+ # Set mData based on jData
+ processInput(jData, jData2, mData, macros)
+
+ # Make sure data is bounded
+ sanityCheck(mData)
+
+ # Write data to serial
+ if LIVE:
+ writeSockData(sock, mData)
+
+ # print("Reading")
+ #if ser.readline():
+ # print("OK")
+ #else:
+ # print("DATA LOSS")
+ # mData.dataLoss += 1
+
+ drawText(s, jData, jData2, mData, macros)
+ drawMeters(s, jData, jData2, mData)
+
+ pygame.display.flip()
+ clock.tick(FRAMERATE)
+
+ if LIVE:
+ sock.close() #TODO ADD CHECK LIKE BELOW
+ #if(ser.isOpen()):
+ # ser.close()
+
+if __name__ == "__main__":
+ main()
diff --git a/controller/sender_v2.py b/controller/sender_v2.py
new file mode 100755
index 0000000000000000000000000000000000000000..3c596ee23fe29e59b62de814325ecf5fa0934e5e
--- /dev/null
+++ b/controller/sender_v2.py
@@ -0,0 +1,522 @@
+#!/usr/bin/python
+
+import serial
+import struct
+import pygame
+import math
+import socket
+
+# IP setup
+
+HOST = '10.19.0.2' # The remote host
+PORT = 50007 # The same port as used by the server
+
+
+#################################################
+# BEGIN SETTINGS
+#################################################
+
+# Pygame
+WIDTH = 800
+HEIGHT = 600
+FRAMERATE = 10
+
+# Serial
+#PORT = '/dev/ttyUSB0'
+#PORT = '/dev/ttyACM0'
+#BAUDRATE = 57600
+#TIMEOUT = 0.2
+LIVE = True
+
+# Joystick
+JTHRESH = 0.1
+
+# Wat
+MACRO = pygame.USEREVENT + 1
+
+#################################################
+# END SETTINGS
+#################################################
+
+# Joystick data
+class JoystickData:
+ numJoys = 0
+ macro = 0
+
+ def __init__(self):
+ self.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ self.axis = [0.0, 0.0, -1.0, 0.0, 0.0, -1.0]
+ self.hat = (0, 0)
+ JoystickData.numJoys += 1
+
+# Motor data / Serial Data
+class MotorData:
+ numMotors = 0
+ macro = 0
+ macroRunning = 0
+ dataLoss = 0
+
+ def __init__(self):
+ self.lmotor = (0, 0)
+ self.rmotor = (0, 0)
+ self.servos = [0, 0, 0, 0, 0, 0]
+ MotorData.numMotors += 1
+
+class Macro:
+ numMacros = 0
+ timestep = 0
+
+ def __init__(self, macroid, name, actions):
+ self.macroid = macroid
+ self.name = name
+ self.actions = actions
+ Macro.numMacros += 1
+
+
+# data = bytearray(b'\x00' * (2 + 4 + 10))
+
+# Joystick Dictionary
+jBtnName = ['A', 'B', 'X', 'Y', 'LB', 'RB', 'Select', 'Start', 'Logo', 'LS', 'RS']
+jBtn = {jBtnName[x]: x for x in range(len(jBtnName))}
+
+jAxisName = ['LSx', 'LSy', 'LT', 'RSx', 'RSy', 'RT']
+jAxis = {jAxisName[x]: x for x in range(len(jAxisName))}
+
+# Sets jData based on detected events
+def handleJoyEvent(e, jData, jData2):
+ if e.joy == 0:
+ if e.type == pygame.JOYAXISMOTION:
+ jData.axis[e.axis] = e.value
+
+ if e.type == pygame.JOYBUTTONDOWN:
+ jData.buttons[e.button] = 1
+
+ if e.type == pygame.JOYBUTTONUP:
+ jData.buttons[e.button] = 0
+
+ if e.type == pygame.JOYHATMOTION:
+ jData.hat = e.value
+
+ if e.joy == 1:
+ if e.type == pygame.JOYAXISMOTION:
+ jData2.axis[e.axis] = e.value
+
+ if e.type == pygame.JOYBUTTONDOWN:
+ jData2.buttons[e.button] = 1
+
+ if e.type == pygame.JOYBUTTONUP:
+ jData2.buttons[e.button] = 0
+
+ if e.type == pygame.JOYHATMOTION:
+ jData2.hat = e.value
+
+ else:
+ pass
+
+# Sets mData based on jData
+def processInput(jData, jData2, mData, macros):
+ sfactor = 235 #100
+
+ # arm, claw, bottom, top, cagelock, droplock
+
+ #---------------------
+ # JOYSTICK 0
+ #---------------------
+
+ # Hat
+ # if jData.hat[1] == 1:
+ # mData.servos[3] += 2
+ # elif jData.hat[1] == -1:
+ # mData.servos[3] -= 2
+ # elif jData.hat[0] == 1:
+ # mData.servos[2] += 2
+ # mData.servos[3] += 2
+ # elif jData.hat[0] == -1:
+ # mData.servos[2] -= 2
+ # mData.servos[3] -= 2
+
+ # Joystick buttons
+ if jData.buttons[jBtn['A']] == 1:
+ mData.servos[1] = 100
+ elif jData.buttons[jBtn['B']] == 1:
+ mData.servos[1] = 45
+
+ # Joystick triggers
+ if jData.axis[jAxis['LT']] > -1 + JTHRESH:
+ mData.servos[0] += 2
+ elif jData.axis[jAxis['RT']] > -1 + JTHRESH:
+ mData.servos[0] -= 2
+
+ # Prevent multiple macros
+ if mData.macroRunning == 0:
+ # Macros
+ if jData.buttons[jBtn['Y']] == 1:
+ mData.macro = 0
+ mData.macroRunning = 1
+ playMacro(macros[mData.macro], mData)
+ pygame.time.set_timer(MACRO, 100)
+ elif jData.buttons[jBtn['Start']] == 1:
+ mData.macro = 1
+ mData.macroRunning = 1
+ playMacro(macros[mData.macro], mData)
+ pygame.time.set_timer(MACRO, 100)
+ elif jData.buttons[jBtn['Select']] == 1:
+ mData.macro = 2
+ mData.macroRunning = 1
+ playMacro(macros[mData.macro], mData)
+ pygame.time.set_timer(MACRO, 100)
+
+ # Turbo mode
+ if jData.buttons[jBtn['RB']] == 1:
+ offset = 255 - sfactor
+ else:
+ offset = 10
+
+ # R/L stick
+ if jData.axis[jAxis['LSy']] > 0 + JTHRESH:
+ mData.lmotor = (int(math.floor(sfactor * jData.axis[jAxis['LSy']]) + offset), 0)
+ elif jData.axis[jAxis['LSy']] < 0 - JTHRESH:
+ mData.lmotor = (int(-math.floor(sfactor * jData.axis[jAxis['LSy']]) + offset - 1), 1)
+ else:
+ mData.lmotor = (0, 0)
+
+ if jData.axis[jAxis['RSy']] > 0 + JTHRESH:
+ mData.rmotor = (int(math.floor(sfactor * jData.axis[jAxis['RSy']]) + offset), 0)
+ elif jData.axis[jAxis['RSy']] < 0 - JTHRESH:
+ mData.rmotor = (int(-math.floor(sfactor * jData.axis[jAxis['RSy']]) + offset - 1), 1)
+ else:
+ mData.rmotor = (0, 0)
+
+ #---------------------
+ # JOYSTICK 1
+ #---------------------
+
+ # Arm on LT and RT
+ if jData2.axis[jAxis['LT']] > -1 + JTHRESH:
+ mData.servos[0] += 2
+ elif jData2.axis[jAxis['RT']] > -1 + JTHRESH:
+ mData.servos[0] -= 2
+
+ # Claw on A and B
+ if jData2.buttons[jBtn['A']] == 1:
+ mData.servos[1] += 2
+ elif jData2.buttons[jBtn['B']] == 1:
+ mData.servos[1] -= 2
+
+ # Drop lock on X and Y
+ if jData2.buttons[jBtn['X']] == 1:
+ mData.servos[5] -= 2
+ elif jData2.buttons[jBtn['Y']] == 1:
+ mData.servos[5] += 2
+
+ # Cage control on HAT
+ if jData2.hat[1] == 1:
+ mData.servos[2] += 2
+ mData.servos[3] += 2
+ elif jData2.hat[1] == -1:
+ mData.servos[2] -= 2
+ mData.servos[3] -= 2
+
+ # Cage control on LStick
+ if jData2.axis[jAxis['LSy']] > 0 + JTHRESH:
+ mData.servos[3] += 1
+ elif jData2.axis[jAxis['LSy']] < 0 - JTHRESH:
+ mData.servos[3] -= 1
+
+ # Cage lock on RStick
+ if jData2.axis[jAxis['RSy']] > 0 + JTHRESH:
+ mData.servos[4] -= 2
+ elif jData2.axis[jAxis['RSy']] < 0 - JTHRESH:
+ mData.servos[4] += 2
+
+
+def playMacro(macro, mData):
+ # [i](servoid, pos, timestep)
+
+ for i in range(len(macro.actions)):
+ if macro.actions[i][2] == macro.timestep:
+ mData.servos[macro.actions[i][0]] = macro.actions[i][1]
+
+ macro.timestep += 1
+
+ temp = 0
+ for i in range(len(macro.actions)):
+ if temp < macro.actions[i][2]:
+ temp = macro.actions[i][2]
+
+ for i in range(len(macro.actions)):
+ if macro.timestep > temp:
+ pygame.time.set_timer(MACRO, 0)
+ macro.timestep = 0
+ mData.macroRunning = 0
+
+def sanityCheck(mData):
+ # Make sure servos are bounded
+
+ #minValues = [22, 45, 0, 0, 50, 30]
+ #maxValues = [160, 100, 180, 180, 140, 110]
+
+ minValues = [22, 0, 0, 0, 50, 30]
+ maxValues = [160, 180, 180, 180, 140, 110]
+
+ for i in range(len(mData.servos)):
+ if mData.servos[i] > maxValues[i]:
+ mData.servos[i] = maxValues[i]
+ elif mData.servos[i] < minValues[i]:
+ mData.servos[i] = minValues[i]
+
+#def writeSerialData(ser, mData):
+# ser.reset_output_buffer()
+# ser.reset_input_buffer()
+#
+# # b (speed, direction)*2 servo*6 e
+# data = [98, mData.lmotor[0], mData.lmotor[1], mData.rmotor[0], mData.rmotor[1]]
+# data += list(mData.servos[x] for x in range(len(mData.servos)))
+# data.append(101)
+#
+# #print(data)
+#
+# ser.write(data)
+
+def writeSockData(sock, mData):
+ # b (speed, direction)*2 servo*6 e
+ data = [98, mData.lmotor[0], mData.lmotor[1], mData.rmotor[0], mData.rmotor[1]]
+ data += list(mData.servos[x] for x in range(len(mData.servos)))
+ data.append(101)
+ data = ','.join(str(e) for e in data)
+ data = data+'\r\n'
+
+ print(data)
+ sock.sendall(data)
+
+
+def drawText(s, jData, jData2, mData, macro):
+ font = pygame.font.SysFont("DejaVu Sans Mono", 18)
+
+ color = (255, 255, 255)
+ bgcolor = (0, 0, 0)
+
+ for i in range(len(jData.buttons)):
+ string = str(jData.buttons[i])
+ text = font.render(string, True, color)
+ s.blit(text, (50, 200 + 20 * i))
+
+ for i in range(len(jData.axis)):
+ string = jData.axis[i]
+ text = font.render("{: 03.2f}".format(string), True, color)
+ s.blit(text, (50, 50 + 20 * i))
+
+ string = str(jData.hat)
+ text = font.render(string, True, color)
+ s.blit(text, (50, 460))
+
+ for i in range(len(jData2.buttons)):
+ string = str(jData2.buttons[i])
+ text = font.render(string, True, color)
+ s.blit(text, (150, 200 + 20 * i))
+
+ for i in range(len(jData2.axis)):
+ string = jData2.axis[i]
+ text = font.render("{: 03.2f}".format(string), True, color)
+ s.blit(text, (150, 50 + 20 * i))
+
+ string = str(jData2.hat)
+ text = font.render(string, True, color)
+ s.blit(text, (150, 460))
+
+ for i in range(len(mData.servos)):
+ string = mData.servos[i]
+ text = font.render("{:03.2f}".format(string), True, color)
+ s.blit(text, (250, 50 + 20 * i))
+
+ string = "M: " + str(mData.macro) + " T: " + str(macro[mData.macro].timestep)
+ text = font.render(string, True, color)
+ s.blit(text, (250, 200))
+
+ string = "Data loss: " + str(mData.dataLoss)
+ text = font.render(string, True, color)
+ s.blit(text, (50, 540))
+
+def drawMeters(s, jData, jData2, mData):
+ color = (255, 255, 255)
+
+ if (jData.axis[jAxis['LSy']] > 0 + JTHRESH
+ or jData.axis[jAxis['LSy']] < 0 - JTHRESH):
+
+ lscolor = (255, 0, 0)
+ else:
+ lscolor = (0, 255, 0)
+
+ if (jData.axis[jAxis['RSy']] > 0 + JTHRESH
+ or jData.axis[jAxis['RSy']] < 0 - JTHRESH):
+
+ rscolor = (255, 0, 0)
+ else:
+ rscolor = (0, 255, 0)
+
+ rh = 500
+ rw = 150
+ rgap = 200
+
+ rx = WIDTH - (2 * rw + 2 * (rgap - rw))
+ ry = (HEIGHT - rh)/2
+
+ for i in range(0, 2):
+ pos = (rx + i * rgap, ry, rw, rh)
+ pygame.draw.rect(s, color, pos, 1)
+
+ if jData.axis[jAxis['LSy']] != 0:
+ pos = (rx + 1, ry + rh/2 + 1, rw - 2, (rh/2) * jData.axis[jAxis['LSy']] - 1)
+ pygame.draw.rect(s, lscolor, pos, 0)
+
+ if jData.axis[jAxis['RSy']] != 0:
+ pos = (rx + 1 + rgap, ry + rh/2 + 1, rw - 2, (rh/2) * jData.axis[jAxis['RSy']])
+ pygame.draw.rect(s, rscolor, pos, 0)
+
+def main():
+ # Initialize PyGame
+ pygame.init()
+ s = pygame.display.set_mode((WIDTH, HEIGHT))
+ pygame.display.set_caption("VOLVO")
+ clock = pygame.time.Clock()
+
+ # Initialize Joystick(s)
+ j = pygame.joystick.Joystick(0)
+ j.init()
+
+ #j2 = pygame.joystick.Joystick(1)
+ #j2.init()
+
+ # Timer for macros
+ clock = pygame.time.Clock()
+
+ # Data
+ jData = JoystickData()
+ jData2 = JoystickData()
+ mData = MotorData()
+
+ for i in range(len(mData.servos)):
+ mData.servos[i] = 90
+
+# # Initialize Serial
+# if LIVE:
+# ser = serial.Serial()
+# ser.port = PORT
+# ser.baudrate = BAUDRATE
+# ser.timeout = TIMEOUT
+# ser.write_timeout = 1
+#
+# ser.open()
+
+ # Initialize Socket
+ if LIVE:
+ sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ sock.connect((HOST, PORT))
+ #sock.sendall('Hello, permoPi! Regards Sender.')
+
+
+ # Define macros
+ # 0: Arm
+ # 1: Claw
+ # 2: Bottom
+ # 3: Top
+ # 4: Cage
+ # 5: Drop
+ macros = []
+ macros.append(Macro(1, "load", [(0, 150, 5),
+ (2, 110, 0),
+ (3, 110, 0),
+ (4, 50, 0),
+ (5, 30, 0),
+ (1, 45, 15),
+ (4, 140, 25),
+ (0, 90, 15),
+ (3, 90, 20),
+ (3, 110, 30)]))
+
+ macros.append(Macro(2, "drop", [(2, 170, 0),
+ (3, 82, 5),
+ (5, 110, 10),
+ (4, 50, 15)]))
+
+ macros.append(Macro(3, "default", [(0, 45, 0),
+ (1, 90, 0),
+ (2, 90, 0),
+ (3, 90, 0),
+ (4, 50, 0),
+ (5, 30, 0)]))
+
+ # macros.append(Macro(1, "transport",
+ # [(0, 118, 0),
+ # (1, 90, 0),
+ # (2, 8, 0),
+ # (3, 0, 0),
+ # (4, 142, 0),
+ # (5, 28, 0)]
+ # ))
+
+ # macros.append(Macro(2, "drop",
+ # [(2, 170, 0),
+ # (3, 76, 0),
+ # (5, 112, 5),
+ # (4, 48, 7)]
+ # ))
+
+ # macros.append(Macro(3, "postdrop",
+ # [(2, 120, 0),
+ # (3, 26, 0),
+ # (3, 90, 5)]
+ # ))
+
+ # Main program loop
+ run = True
+ while run:
+ # Clear screen
+ s.fill(0)
+
+ # Check for keyboard and joystick events
+ for e in pygame.event.get():
+ if (e.type == pygame.QUIT or
+ e.type == pygame.KEYDOWN and
+ e.key == pygame.K_ESCAPE):
+ run = False
+
+ if (e.type == pygame.JOYAXISMOTION or
+ e.type == pygame.JOYBUTTONDOWN or
+ e.type == pygame.JOYBUTTONUP or
+ e.type == pygame.JOYHATMOTION):
+ handleJoyEvent(e, jData, jData2)
+
+ if e.type == MACRO:
+ playMacro(macros[mData.macro], mData)
+
+ # Set mData based on jData
+ processInput(jData, jData2, mData, macros)
+
+ # Make sure data is bounded
+ sanityCheck(mData)
+
+ # Write data to serial
+ if LIVE:
+ writeSockData(sock, mData)
+
+ # print("Reading")
+ #if ser.readline():
+ # print("OK")
+ #else:
+ # print("DATA LOSS")
+ # mData.dataLoss += 1
+
+ drawText(s, jData, jData2, mData, macros)
+ drawMeters(s, jData, jData2, mData)
+
+ pygame.display.flip()
+ clock.tick(FRAMERATE)
+
+ if LIVE:
+ sock.close() #TODO ADD CHECK LIKE BELOW
+ #if(ser.isOpen()):
+ # ser.close()
+
+if __name__ == "__main__":
+ main()