Skip to content

Instantly share code, notes, and snippets.

@jarulsamy

jarulsamy/scribbler.py

Created December 10, 2018 03:48
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save jarulsamy/08dcf16da7d672530ebbcc88d2b184b8 to your computer and use it in GitHub Desktop.
Save jarulsamy/08dcf16da7d672530ebbcc88d2b184b8 to your computer and use it in GitHub Desktop.
Download ZIP
Essential movement portions of scribbler2 robot ported to python3.
Raw
scribbler.py
import time
import serial
import threading
from past.utils import old_div
class commands(object):
SOFT_RESET = 33
GET_ALL = 65
GET_ALL_BINARY = 66
GET_LIGHT_LEFT = 67
GET_LIGHT_CENTER = 68
GET_LIGHT_RIGHT = 69
GET_LIGHT_ALL = 70
GET_IR_LEFT = 71
GET_IR_RIGHT = 72
GET_IR_ALL = 73
GET_LINE_LEFT = 74
GET_LINE_RIGHT = 75
GET_LINE_ALL = 76
GET_STATE = 77
GET_NAME1 = 78
GET_NAME2 = 64
GET_STALL = 79
GET_INFO = 80
GET_DATA = 81
GET_PASS1 = 50
GET_PASS2 = 51
GET_RLE = 82 # a segmented and run-length encoded image
GET_IMAGE = 83 # the entire 256 x 192 image in YUYV format
GET_WINDOW = 84 # the windowed image (followed by which window)
GET_DONGLE_L_IR = 85 # number of returned pulses when left emitter is turned on
GET_DONGLE_C_IR = 86 # number of returned pulses when center emitter is turned on
GET_DONGLE_R_IR = 87 # number of returned pulses when right emitter is turned on
GET_WINDOW_LIGHT = 88 # average intensity in the user defined region
GET_BATTERY = 89 # battery voltage
GET_SERIAL_MEM = 90 # with the address returns the value in serial memory
GET_SCRIB_PROGRAM = 91 # with offset, returns the scribbler program buffer
GET_CAM_PARAM = 92 # with address, returns the camera parameter at that address
GET_BLOB = 95
SET_PASS1 = 55
SET_PASS2 = 56
SET_SINGLE_DATA = 96
SET_DATA = 97
SET_ECHO_MODE = 98
SET_LED_LEFT_ON = 99
SET_LED_LEFT_OFF = 100
SET_LED_CENTER_ON = 101
SET_LED_CENTER_OFF = 102
SET_LED_RIGHT_ON = 103
SET_LED_RIGHT_OFF = 104
SET_LED_ALL_ON = 105
SET_LED_ALL_OFF = 106
SET_LED_ALL = 107
SET_MOTORS_OFF = 108
SET_MOTORS = 109
SET_NAME1 = 110
SET_NAME2 = 119 # set name2 byte
SET_LOUD = 111
SET_QUIET = 112
SET_SPEAKER = 113
SET_SPEAKER_2 = 114
SET_DONGLE_LED_ON = 116 # turn binary dongle led on
SET_DONGLE_LED_OFF = 117 # turn binary dongle led off
SET_RLE = 118 # set rle parameters
SET_DONGLE_IR = 120 # set dongle IR power
SET_SERIAL_MEM = 121 # set serial memory byte
SET_SCRIB_PROGRAM = 122 # set scribbler program memory byte
SET_START_PROGRAM = 123 # initiate scribbler programming process
SET_RESET_SCRIBBLER = 124 # hard reset scribbler
SET_SERIAL_ERASE = 125 # erase serial memory
SET_DIMMER_LED = 126 # set dimmer led
SET_WINDOW = 127 # set user defined window
SET_FORWARDNESS = 128 # set direction of scribbler
SET_WHITE_BALANCE = 129 # turn on white balance on camera
SET_NO_WHITE_BALANCE = 130 # diable white balance on camera (default)
# with address and value, sets the camera parameter at that address
SET_CAM_PARAM = 131
GET_JPEG_GRAY_HEADER = 135
GET_JPEG_GRAY_SCAN = 136
GET_JPEG_COLOR_HEADER = 137
GET_JPEG_COLOR_SCAN = 138
SET_PASS_N_BYTES = 139
GET_PASS_N_BYTES = 140
GET_PASS_BYTES_UNTIL = 141
GET_VERSION = 142
GET_IR_MESSAGE = 150
SEND_IR_MESSAGE = 151
SET_IR_EMITTERS = 152
SET_START_PROGRAM2 = 153 # initiate scribbler2 programming process
SET_RESET_SCRIBBLER2 = 154 # hard reset scribbler2
SET_SCRIB_BATCH = 155 # upload scribbler2 firmware
GET_ROBOT_ID = 156
SET_VOLUME = 160 # Format 160 volume (0-100) Percent Volume Level
SET_PATH = 161 # Format 161 begin_or_end speed 0 1 2
# begin=0 end=1 hSpeedByte lSpeedByte
SET_MOVE = 162 # Format 162 type hXByte lXByte hYByte lYByte
SET_ARC = 163 # Format 163 type hXByte lXByte hYByte lYByte hRadByte lRadByte
SET_TURN = 164 # Format 164 type hAngleByte lAngleByte
GET_POSN = 165 # Format 165
SET_POSN = 166 # Format 166 x0Byte x1Byte x2Byte x3Byte y0Byte y1Byte y2Byte y3Byte
GET_ANGLE = 167 # Format 167
SET_ANGLE = 168 # Format 168 angle0Byte angle1Byte angle2Byte angle3Byte
GET_MIC_ENV = 169 # Format 169
GET_MOTOR_STATS = 170 # Format 170
GET_ENCODERS = 171 # Format 171 type
GET_IR_EX = 172 # Format 172 side type thres
GET_LINE_EX = 173 # Format 173 side type thres
GET_DISTANCE = 175 # Format 175 side
PACKET_LENGTH = 9
BEGIN_PATH = 0 # Used with SET_PATH to say beginning of a path
END_PATH = 1 # Used with SET_PATH to say end of a path
BY = 4 # Used in movement commands, by means how much you wish to move by
TO = 2 # Used in movement commands, to means the heading you want to turn to
DEG = 1 # Used in movement commands, specifies using degress instead of S2 angle units
class Scribbler(object):
def __init__(self, com):
self.lock = threading.Lock()
self.c = commands()
self.ser = serial.Serial(com, timeout=10)
# self.echo = bytes(chr(98)+' 0 0 0 0 0 0 0 0', 'utf-8')
# self.ser.write(self.echo)
self.empty = ' 0 0 0 0 0 0 0'
self.startupSound()
self.setEchoMode(0)
# self.setForwardness(1)
def reboot(self):
try:
self.lock.acquire()
self.ser.write(chr(self.c.SET_RESET_SCRIBBLER))
finally:
self.lock.release()
def setForwardness(self, direction):
if direction in ["fluke-forward", 1]:
direction = 1
elif direction in ["scribbler-forward", 0]:
direction = 0
else:
raise AttributeError(
"unknown direction: '%s': should be 'fluke-forward' or 'scribbler-forward'" % direction)
try:
self.lock.acquire()
self.ser.write(bytes(chr(self.c.SET_FORWARDNESS), 'utf-8'))
self.ser.write(bytes(chr(direction), 'utf-8'))
finally:
self.lock.release()
def setEchoMode(self, value):
if self.isTrue(value):
self._set(self.c.SET_ECHO_MODE, 1)
else:
self._set(self.c.SET_ECHO_MODE, 0)
time.sleep(.25)
self.ser.flushInput()
self.ser.flushOutput()
return
def startupSound(self):
self.beep(.03, 784)
self.beep(.03, 880)
self.beep(.03, 698)
self.beep(.03, 349)
self.beep(.03, 523)
def blink(self):
on = bytes(chr(self.c.SET_LED_ALL_ON) + self.empty, 'utf-8')
off = bytes(chr(self.c.SET_LED_ALL_OFF) + self.empty, 'utf-8')
for i in range(5):
self.ser.write(on)
time.sleep(1)
self.ser.write(off)
time.sleep(1)
def _write(self, rawdata):
t = [chr(int(x)) for x in rawdata]
data = ''.join(t) + (chr(0) *
(self.c.PACKET_LENGTH - len(t)))[:9]
data = bytes(data, 'utf-8')
time.sleep(.01)
# if self.dongle == None:
# time.sleep(0.01) # HACK! THIS SEEMS TO NEED TO BE HERE!
self.ser.write(data)
def _set_speaker(self, frequency, duration):
self._write([self.c.SET_SPEAKER,
duration >> 8,
duration % 256,
frequency >> 8,
frequency % 256])
def beep(self, duration, frequency, frequency2=None):
if frequency2 == None:
self._set_speaker(int(frequency), int(duration * 1000))
else:
self._set_speaker_2(int(frequency), int(
frequency2), int(duration * 1000))
v = self.ser.read(self.c.PACKET_LENGTH + 11)
def motors(self, left, right):
trans = old_div((right + left), 2.0)
rotate = old_div((right - left), 2.0)
return self.move(trans, rotate)
def forward(self, speed=1, interval=None):
self.move(speed, 0)
if interval != None:
time.sleep(interval)
self.stop()
def backward(self, speed=1, interval=None):
self.move(-speed, 0)
if interval != None:
time.sleep(interval)
self.stop()
def turnLeft(self, speed=1, interval=None):
retval = self.move(0, speed)
if interval != None:
time.sleep(interval)
self.stop()
return retval
def turnRight(self, speed=1, interval=None):
retval = self.move(0, -speed)
if interval != None:
time.sleep(interval)
self.stop()
return retval
def turn(self, direction, value=.8, interval=None):
if type(direction) in [float, int]:
retval = self.move(0, direction)
else:
direction = direction.lower()
if direction == "left":
retval = self.move(0, value)
elif direction == "right":
retval = self.move(0, -value)
elif direction in ["straight", "center"]:
retval = self.move(0, 0) # aka, stop!
else:
retval = "error"
if interval != None:
time.sleep(interval)
self.stop()
return retval
def stop(self):
self._lastTranslate = 0
self._lastRotate = 0
return self._set(self.c.SET_MOTORS_OFF)
def uninit(self):
self.ser.close()
# Helper Functions
def translate(self, amount):
self._lastTranslate = amount
self._adjustSpeed()
def rotate(self, amount):
self._lastRotate = amount
self._adjustSpeed()
def move(self, translate, rotate):
self._lastTranslate = translate
self._lastRotate = rotate
self._adjustSpeed()
def _adjustSpeed(self):
left = min(max(self._lastTranslate - self._lastRotate, -1), 1)
right = min(max(self._lastTranslate + self._lastRotate, -1), 1)
leftPower = (left + 1.0) * 100.0
rightPower = (right + 1.0) * 100.0
print("LEFT: ", left)
print("RIGHT: ", right)
print("LEFT_POWER: ", leftPower)
print("RIGHT_POWER ", rightPower)
self._set(self.c.SET_MOTORS, rightPower, leftPower)
def _set(self, *values):
try:
self.lock.acquire()
self._write(values)
test = self._read(self.c.PACKET_LENGTH)
time.sleep(.01)
finally:
self.lock.release()
def _read(self, bytes=1):
c = self.ser.read(bytes)
while (bytes > 1 and len(c) < bytes):
c = c + self.ser.read(bytes-len(c))
if bytes == 1:
x = -1
if (c != ""):
x = ord(c)
elif self.debug:
print("timeout!")
return x
else:
c = str(c)
return list(map(ord, c))
def isTrue(self, value):
"""
Returns True if value is something we consider to be "on".
Otherwise, return False.
"""
if type(value) == str:
return (value.lower() == "on")
elif value:
return True
return False
robot = Scribbler("COM3")
def test():
print("-----------FORWARD-----------")
robot.forward(1,1)
print("-----------Backward-----------")
robot.backward(1,1)
print("-----------TURN_LEFT-----------")
robot.turnLeft(1,1)
print("-----------TURN_RIGHT-----------")
robot.turnRight(1,1)
robot.stop()
test()
robot.uninit()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment