Commit f5eb833a authored by Robert Hedman's avatar Robert Hedman

controller update for rust

parent 813a832e
#!/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 = 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]), 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])
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()
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment