Merge branch 'master' of gitlab.henriktjader.com:Trobolit/permocar

controller/relayer.py

+#!/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()

controller/sender_udp_v2.py

+#!/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()

controller/sender_v2.py

+#!/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()