added rust local sender

controller/sender_rust_local.py

+#!/usr/bin/python
+
+import serial
+import struct
+import pygame
+import math
+import socket
+
+# IP setup
+
+HOST = '10.180.65.18'    # The remote host
+PORT = 50007              # The same port as used by the server
+
+
+#################################################
+#   BEGIN SETTINGS
+#################################################
+
+# Pygame
+WIDTH = 800
+HEIGHT = 600
+FRAMERATE = 10
+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
+    dataLoss = 0
+
+    def __init__(self):
+        self.lmotor = (0, 0)
+        self.rmotor = (0, 0)
+        MotorData.numMotors += 1
+
+
+
+
+# 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):
+    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
+
+    else:
+        pass
+
+# Sets mData based on jData
+def processInput(jData, mData):
+    sfactor = 200 #100
+    turnfactor = 180
+    drive = (0, 0)
+    #print(drive)
+    
+    if jData.axis[jAxis['LSy']] > 0 + JTHRESH:
+        drive = (int( math.floor(sfactor * jData.axis[jAxis['LSy']]) ), 2)
+    elif jData.axis[jAxis['LSy']] < 0 - JTHRESH:
+        drive = (int(-math.floor(sfactor * jData.axis[jAxis['LSy']]) ), 1)
+    else:
+        drive = (0, 0)
+
+    mData.lmotor = drive
+    mData.rmotor = drive
+
+    turn = (0,0)
+
+    if jData.axis[jAxis['RSx']] > 0 + JTHRESH_TURN: ## turning right
+        turn = (int( math.floor(turnfactor * jData.axis[jAxis['RSx']]) ), 2)
+    elif jData.axis[jAxis['RSx']] < 0 - JTHRESH_TURN:
+        turn = (int(-math.floor(turnfactor * jData.axis[jAxis['RSx']]) ), 1) # turning left
+    else:
+        turn = (0, 0)
+
+    if drive != (0, 0):
+    	mData.lmotor = (sorted([0, (mData.lmotor[0] + pow(-1,turn[1])*turn[0]), 200])[1], mData.lmotor[1])
+    	mData.rmotor = (sorted([0, (mData.rmotor[0] - pow(-1,turn[1])*turn[0]), 200])[1], mData.rmotor[1])
+    else:
+    	mData.rmotor = (sorted([0, ( turn[0]), 200])[1], turn[1] )
+    	mData.lmotor = (sorted([0, ( turn[0]), 200])[1], sorted([0, (3-turn[1]), 2])[1] )   	
+
+
+
+def writeSockData(sock, mData):
+    # b (speed, direction)*2 servo*6 e
+    print([250, mData.lmotor[1], mData.lmotor[0], mData.rmotor[1], mData.rmotor[0], 251])
+    data = bytearray()
+    data.append(250)
+    data.append(mData.lmotor[1])
+    data.append(mData.lmotor[0])
+    data.append(mData.rmotor[1])
+    data.append(mData.rmotor[0])
+    data.append(251)
+    #print(data)
+    #data = ','.join(str(e) for e in data)
+
+    print(data)
+    sock.send(data)
+
+
+def drawText(s, jData, mData):
+    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))
+
+
+    string = "Data loss: " + str(mData.dataLoss)
+    text = font.render(string, True, color)
+    s.blit(text, (50, 540))
+
+def drawMeters(s, jData, 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['RSx']] > 0 + JTHRESH
+        or jData.axis[jAxis['RSx']] < 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['RSx']] != 0:
+        pos = (rx + 1 + rgap, ry + rh/2 + 1, rw - 2, (rh/2) * -jData.axis[jAxis['RSx']])
+        pygame.draw.rect(s, rscolor, pos, 0)
+
+def main():
+    # Initialize PyGame
+    pygame.init()
+    s = pygame.display.set_mode((WIDTH, HEIGHT))
+    pygame.display.set_caption("ROSTIG_VOLVO")
+    clock = pygame.time.Clock()
+
+    # Initialize Joystick(s)
+    j = pygame.joystick.Joystick(0)
+    j.init()
+
+    # Timer for macros
+    clock = pygame.time.Clock()
+
+    # Data
+    jData = JoystickData()
+    mData = MotorData()
+
+    
+    # Initialize Socket
+    if LIVE:
+        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+	sock.connect((HOST, PORT))
+	#sock.sendall('Hello, permoPi! Regards Sender.')
+
+
+    # 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)
+
+
+        # Set mData based on jData
+        processInput(jData, mData)
+
+        # Write data to serial
+        if LIVE:
+            writeSockData(sock, mData)
+
+
+        drawText(s, jData, mData)
+        drawMeters(s, jData, 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()