161 lines
4.6 KiB
Python
161 lines
4.6 KiB
Python
import serial
|
|
import time
|
|
|
|
|
|
class NexusRobot:
|
|
'''
|
|
Nexus Robot Library for moving
|
|
'''
|
|
def __init__(self, port):
|
|
self.ser = serial.serial_for_url(port, 9600, do_not_open=True)
|
|
self.ser.open()
|
|
self.pause(3)
|
|
self._spd_calib = 1
|
|
self._ang_calib = 1
|
|
self._dis_calib = 1
|
|
|
|
def _calib_spd(self, speed):
|
|
return int(speed * self._spd_calib)
|
|
|
|
def _calib_ang(self, angle):
|
|
return int(angle * self._ang_calib)
|
|
|
|
def _calib_dis(self, distance):
|
|
return int(distance * self._dis_calib)
|
|
|
|
def _wait(self):
|
|
self.ser.readline()
|
|
self.ser.readline()
|
|
|
|
def pause(self, duration=1):
|
|
time.sleep(duration)
|
|
|
|
def forward(self, speed, dist):
|
|
self.ser.write(
|
|
"g{0},{1};".format(self._calib_spd(speed), self._calib_dis(dist))
|
|
)
|
|
if dist > 0:
|
|
self._wait()
|
|
|
|
def backward(self, speed, dist):
|
|
self.ser.write(
|
|
"b{0},{1};".format(self._calib_spd(speed), self._calib_dis(dist))
|
|
)
|
|
if dist > 0:
|
|
self._wait()
|
|
|
|
def turnRight(self, speed, angle=-1):
|
|
self.ser.write(
|
|
"rr{0},{1};".format(self._calib_spd(speed), self._calib_ang(angle))
|
|
)
|
|
if angle > 0:
|
|
self._wait()
|
|
|
|
def turnLeft(self, speed, angle=-1):
|
|
self.ser.write(
|
|
"rl{0},{1};".format(self._calib_spd(speed), self._calib_ang(angle))
|
|
)
|
|
if angle > 0:
|
|
self._wait()
|
|
|
|
def rotateLeft(self, speed, angle=-1):
|
|
self.ser.write(
|
|
"r-{0},{1},-1;".format(
|
|
self._calib_spd(speed), self._calib_spd(speed))
|
|
)
|
|
# self._wait()
|
|
|
|
def rotateRight(self, speed, angle=-1):
|
|
self.ser.write(
|
|
"r{0},-{1},-1;".format(
|
|
self._calib_spd(speed), self._calib_spd(speed))
|
|
)
|
|
# self._wait()
|
|
|
|
def stop(self, speed=0):
|
|
self.ser.write("s{0};".format(self._calib_spd(speed)))
|
|
|
|
def getInfo(self):
|
|
self.ser.write("h;")
|
|
actual_l_speed = int(self.ser.readline().rstrip().split(':')[-1])
|
|
target_l_speed = int(self.ser.readline().rstrip().split(':')[-1])
|
|
actual_r_speed = int(self.ser.readline().rstrip().split(':')[-1])
|
|
target_r_speed = int(self.ser.readline().rstrip().split(':')[-1])
|
|
return actual_l_speed, target_l_speed, actual_r_speed, target_r_speed
|
|
|
|
|
|
if __name__ == '__main__':
|
|
import sys
|
|
import tty
|
|
import termios
|
|
|
|
class _Getch:
|
|
def __call__(self):
|
|
fd = sys.stdin.fileno()
|
|
old_settings = termios.tcgetattr(fd)
|
|
try:
|
|
tty.setraw(sys.stdin.fileno())
|
|
ch = sys.stdin.read(1)
|
|
finally:
|
|
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
|
|
return ch
|
|
|
|
def get():
|
|
inkey = _Getch()
|
|
while 1:
|
|
k = inkey()
|
|
if k != '':
|
|
break
|
|
return k
|
|
|
|
robot = None
|
|
speed = 200
|
|
try:
|
|
print('Initializing..')
|
|
robot = NexusRobot('/dev/cu.usbserial-AL00YYCA')
|
|
print('Initialized')
|
|
prev_k = None
|
|
while 1:
|
|
k = get()
|
|
if k == 'w' and prev_k != k:
|
|
print('Going Forward')
|
|
robot.forward(speed, -1)
|
|
elif k == 's' and prev_k != k:
|
|
print('Going Backward')
|
|
robot.backward(speed, -1)
|
|
elif k == 'a' and prev_k != k:
|
|
print('Turing left')
|
|
robot.rotateLeft(speed, -1)
|
|
elif k == 'd' and prev_k != k:
|
|
print('Turing right')
|
|
robot.rotateRight(speed, -1)
|
|
elif k == ' ':
|
|
print('Stopping')
|
|
robot.stop()
|
|
elif k == 'h':
|
|
print('Printing info..')
|
|
print(robot.getInfo())
|
|
elif k == '+':
|
|
speed += 20
|
|
if speed > 400:
|
|
speed = 400
|
|
print('Config speed to: {0}'.format(speed))
|
|
elif k == '-':
|
|
speed -= 20
|
|
if speed < 20:
|
|
speed = 20
|
|
print('Config speed to: {0}'.format(speed))
|
|
elif k == '\x03':
|
|
raise KeyboardInterrupt
|
|
else:
|
|
print('unrecognized key: {0}'.format(k))
|
|
prev_k = k
|
|
# robot.pause()
|
|
except serial.serialutil.SerialException, e:
|
|
print(str(e))
|
|
except KeyboardInterrupt:
|
|
if robot:
|
|
robot.stop()
|
|
except Exception:
|
|
raise
|