192 lines
6.7 KiB
Python
Executable File
192 lines
6.7 KiB
Python
Executable File
#!/usr/bin/env python2
|
|
|
|
# nog geen 3 omdat serial er nog niet bij zit. (nog steeds?)
|
|
|
|
# To control Nexus robot car
|
|
# Via /dev/ttyUSB?
|
|
#
|
|
|
|
import sys, os, serial, time, traceback
|
|
from struct import pack, unpack
|
|
import numpy as np
|
|
import re
|
|
|
|
def key_description(character):
|
|
"""generate a readable description for a key"""
|
|
ascii_code = ord(character)
|
|
if ascii_code < 32:
|
|
return 'Ctrl+%c' % (ord('@') + ascii_code)
|
|
else:
|
|
return repr(character)
|
|
|
|
# voorlopig alleen posix
|
|
if os.name == 'posixnee':
|
|
import termios, sys, os
|
|
class Console:
|
|
def __init__(self):
|
|
self.fd = sys.stdin.fileno()
|
|
|
|
def setup(self):
|
|
self.old = termios.tcgetattr(self.fd)
|
|
new = termios.tcgetattr(self.fd)
|
|
# new[3] = new[3] & ~termios.ICANON & ~termios.ECHO & ~termios.ISIG
|
|
new[3] = new[3] & ~termios.ICANON # & ~termios.ISIG
|
|
new[6][termios.VMIN] = 1
|
|
new[6][termios.VTIME] = 0
|
|
termios.tcsetattr(self.fd, termios.TCSANOW, new)
|
|
|
|
# Uiteindelijk willen we meer tegelijk lezen..
|
|
# zeker als er grotere hoeveelheden data verstuurd worden door het target
|
|
def getkey(self):
|
|
c = os.read(self.fd, 1)
|
|
return c
|
|
|
|
def cleanup(self):
|
|
termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old)
|
|
|
|
console = Console()
|
|
|
|
def cleanup_console():
|
|
console.cleanup()
|
|
|
|
console.setup()
|
|
sys.exitfunc = cleanup_console #terminal modes have to be restored on exit...
|
|
|
|
#else:
|
|
# raise NotImplementedError( "Sorry no implementation for your platform (%s) available." % sys.platform)
|
|
|
|
|
|
CONVERT_CRLF = 2
|
|
CONVERT_CR = 1
|
|
CONVERT_LF = 0
|
|
NEWLINE_CONVERISON_MAP = ('\n', '\r', '\r\n')
|
|
LF_MODES = ('LF', 'CR', 'CR/LF')
|
|
|
|
REPR_MODES = ('raw', 'some control', 'all control', 'hex')
|
|
|
|
class CarControl:
|
|
def __init__(self, port, baudrate, parity, rtscts, xonxoff, echo=False, convert_outgoing=CONVERT_CRLF, repr_mode=0):
|
|
self.serial = serial.Serial(port, baudrate, parity=parity, rtscts=rtscts, xonxoff=xonxoff, timeout = 1)
|
|
self.echo = echo
|
|
self.repr_mode = repr_mode
|
|
self.convert_outgoing = convert_outgoing
|
|
self.newline = NEWLINE_CONVERISON_MAP[self.convert_outgoing]
|
|
self.dtr_state = True
|
|
self.rts_state = True
|
|
self.break_state = False
|
|
|
|
# Het lijkt er op of er na een power up van de Nexus twee characters (waarde 0) gezonden worden. FTDI chip?
|
|
# De writer pas wat laten doen als we daar geen last meer van hebben.
|
|
tmp = self.serial.read(1)
|
|
#print 'pretmp', len(tmp)
|
|
#if len(tmp) > 0:
|
|
# print ord(tmp[0])
|
|
time.sleep(1)
|
|
tmp = self.serial.read(1)
|
|
#print 'prettmp', len(tmp)
|
|
#if len(tmp) > 0:
|
|
# print ord(tmp[0])
|
|
|
|
def stop(self):
|
|
print 'Waar is de stop?'
|
|
self.serial.close()
|
|
|
|
def getinfopacket(self):
|
|
try:
|
|
d = self.serial.read(24)
|
|
print len(d) #, ord(d[0])
|
|
if ord(d[0]) <> 2 or ord(d[23]) <> 3: # STX and ETX
|
|
print 'Packet framing error!'
|
|
else:
|
|
print 'New info packet received.'
|
|
if ord(d[1]) == 1: # scope data packet (try that first)
|
|
status_control = ord(d[3]);
|
|
error_code = ord(d[4]);
|
|
status_car = ord(d[5]);
|
|
#(sensor,) = unpack('h', d[10:12]);
|
|
(speed,) = unpack('h', d[6:8]);
|
|
(debtime,) = unpack('h', d[10:12]);
|
|
# we gebruiken dit in de write thread en bij autoreply.
|
|
# evt. korte lock bij uitpakken data
|
|
waitreq = 1
|
|
else:
|
|
print 'Packet type %x received, not implemented.' % d[1]
|
|
print 'Info packet received with error code %x %x %d and %d' % (error_code, status_car, speed, debtime)
|
|
except:
|
|
print 'Wanneer hier?'
|
|
traceback.print_exc(file=sys.stdout)
|
|
|
|
|
|
"""
|
|
Packets are 8 bytes.
|
|
{ stx, command, par's ..., etx }
|
|
parameters dependant upon the command
|
|
|
|
Nexus robot functions of type void(*function[11])(unsigned int speedMMPS) :
|
|
{goAhead, turnLeft, turnRight, rotateRight, rotateLeft, allStop, backOff, upperLeft, lowerLeft, upperRight, lowerRight};
|
|
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 }
|
|
this wil go in command.
|
|
Parameter speedMMPS in par
|
|
|
|
-700 < speedMMPS < 700
|
|
speedMMPS = 200 komt overeen met 0.21 m/sec
|
|
maximale snelheid 0.735 m/sec
|
|
van m/s naar speedMMPS : maal 952
|
|
|
|
|
|
Other commands:
|
|
12: setWheelspeedcommand (set speed of 4 wheels)
|
|
13: set speedMMPS on Nexus
|
|
20: ask info packet
|
|
a 24 byte packet will be received
|
|
21: autoshutdown off (default)
|
|
22: autoshutdown on
|
|
23: autoreply off (default)
|
|
24: autoreply on. reply with a infopacket
|
|
25: start standalone?
|
|
26: stop standalone
|
|
27: keep alive
|
|
28: repeat mode info packets off (default)
|
|
29: repeat mode info packete on
|
|
|
|
"""
|
|
|
|
def sendpacket(self, command, par):
|
|
outpkt = bytearray(16)
|
|
outpkt[0] = 2 # STX
|
|
outpkt[1] = command
|
|
outpkt[2:4] = pack('h', par)
|
|
outpkt[15] = 3 # ETX
|
|
self.serial.write(outpkt) # send packet
|
|
print 'sendpacket', outpkt[0], outpkt[1], outpkt[2], outpkt[3], outpkt[4], outpkt[5], outpkt[6], ' ... ', outpkt[15]
|
|
|
|
def sendwheelscommand(self, frontleft, backleft, frontright, backright):
|
|
outpkt = bytearray(16)
|
|
outpkt[0] = 2 # STX
|
|
outpkt[1] = 12 # setwheels command
|
|
outpkt[2:4] = pack('h', frontleft)
|
|
outpkt[4:6] = pack('h', backleft)
|
|
outpkt[6:8] = pack('h', frontright)
|
|
outpkt[8:10] = pack('h', backright)
|
|
|
|
outpkt[15] = 3 # ETX
|
|
self.serial.write(outpkt) # send packet
|
|
#print 'outpkt: ', unpack('h', outpkt[2:4]), unpack('h', outpkt[4:6]), unpack('h', outpkt[6:8]), unpack('h',outpkt[8:10])
|
|
|
|
# def setspeed(self, lin,rot):
|
|
# # lineaire (m/sec) and rotatie (rad/sec) snelheid
|
|
# # convert to nexus (calibrated with nexus1)
|
|
# speed = lin * 1024
|
|
# rot = rot * 300
|
|
# self.sendwheelscommand(speed -rot, speed - rot, speed + rot , speed + rot)
|
|
|
|
def setspeed(self, linx, liny, rot):
|
|
# lineaire x and y (m/sec) and rotatie (rad/sec) snelheid
|
|
# convert to nexus (calibrated with nexus1)
|
|
speedx = linx * -1024
|
|
speedy = liny * -1024
|
|
rot = rot * 300
|
|
self.sendwheelscommand(speedx -speedy -rot, speedx +speedy -rot, speedx -speedy +rot, speedx +speedy +rot)
|
|
|
|
|