def __init__(self, port='/dev/ttyUSB0'):
self.bot = pycreate2.Create2(port)
# self.js = Joystick()
self.camera = Camera(cam='cv')
self.camera.init(win=(320, 240), cameraNumber=0)
# only create if on linux, because:
# imu needs access to i2c
# adc needs access to spi
# if platform.system() == 'Linux':
# self.imu = IMU()
# self.adc = MCP3208()
print('Start')
print(self.bot.inputCommands([35]))
def main():
port = '/dev/ttyUSB0'
bot = bot = pycreate2.Create2(port)
bot.start()
bot.safe()
bot.drive_stop()
image_size = (320, 240)
camera = Camera(cam='pi')
camera.init(win=image_size)
imu = IMU()
bag = BagWriter()
bag.open(['camera', 'accel', 'mag', 'cr'])
bag.stringify('camera')
bag.use_compression = True
try:
for i in range(100):
ret, frame = camera.read()
if ret:
bag.push('camera', frame)
else:
print('>> bad frame', i)
sen = bot.get_sensors()
bag.push('cr', sen)
a, m, g = imu.get()
bag.push('accel', a)
bag.push('mag', m)
# time.sleep(0.025)
print(i)
except KeyboardInterrupt:
print(' ctrl-c pressed, bye ...')
except Exception as e:
print('')
print('Something else happened ... :(')
print('Maybe the robot is not on ... press the start button')
print('-'*30)
print(e)
bag.write('test.json')
def __init__(self, port='/dev/ttyUSB0'):
self.bot = pycreate2.Create2(port)
# only create if on linux:
image_size = (640, 480)
if platform.system() == 'Linux':
self.camera = Camera(cam='pi')
self.camera.init(win=image_size)
self.imu = IMU()
else:
self.camera = Camera(cam='cv')
self.camera.init(win=image_size, cameraNumber=0)
self.sensors = {'create': None, 'imu': None, 'camera': None}
self.distance = 0.0
self.ahrs = AHRS()
self.modes = {
'js': JoyStickMode(self.bot),
'demo': DemoMode(self.bot),
'auto': AutoMode(self.bot),
'idle': IdleMode(self.bot),
'sensors': Sensors(self.bot)
}
# self.bag = Bag('./data/robot-{}.json'.format(time.ctime().replace(' ', '-')), ['imu', 'camera', 'create'])
self.current_mode = 'idle'
self.last_time = time.time()
self.data = {
# 'r': CircularBuffer(30),
# 'p': CircularBuffer(30),
'y': CircularBuffer(30),
}
# calibration matrix
if args['numpy'] is not None:
cam_cal = args['numpy']
d = read(cam_cal)
m = d['camera_matrix']
k = d['dist_coeff']
print('Using supplied camera calibration matrix: {}'.format(cam_cal))
# print size
# print cam_cal
save = SaveVideo()
# open camera
cap = Camera()
cap.init(win=size, cameraNumber=source)
print('---------------------------------')
print(' ESC/q to quit')
print(' v to start/stop video capture')
print(' f to grab a frame')
print('---------------------------------')
shot_idx = 0
video_idx = 0
video = False
vfn = None
FPS = 30
sleep_time = 1.0/float(FPS) # 30 FPS
#!/usr/bin/env python
from __future__ import print_function
import cv2
from opencvutils import Camera
cnt = 0
cam = Camera()
# cam.gray = True
cam.init(win=(320, 240), cameraNumber=0)
while True:
ret, im = cam.read()
if ret:
cv2.imshow('image', im)
key = cv2.waitKey(1) & 0xFF
if key == ord("q"):
break
elif key == ord("s"):
cv2.imwrite("image-{}.png".format(cnt), im)
print('>> Wrote: {}'.format("image-{}.png".format(cnt)))
cnt += 1
def setUpCameraCV(win=(320, 240), cv=0):
global camera
camera = Camera('cv')
camera.init(cameraNumber=cv, win=win)
def setUpCameraPi(win=(320, 240)):
global camera
camera = Camera('pi')
camera.init(win=win)
#!/usr/bin/env python
import cv2
import os
from opencvutils import Camera
import sys
cascPath = 'haarcascade_frontalface_default.xml'
faceCascade = cv2.CascadeClassifier(cascPath)
image_size = (392, 280)
camera = Camera(cam='pi')
camera.init(win=image_size)
while True:
ret, img = camera.read()
img = cv2.flip(img, 0)
##img = 255*img
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
faces = faceCascade.detectMultiScale(
gray,
scaleFactor=1.1,
minNeighbors=5,
minSize=(30, 30),
)
for (x, y, w, h) in faces:
class Create2(object):
bot = None
imu = None
camera = None
sensors = {}
def __init__(self, port='/dev/ttyUSB0'):
self.bot = pycreate2.Create2(port)
# only create if on linux:
image_size = (640, 480)
if platform.system() == 'Linux':
self.camera = Camera(cam='pi')
self.camera.init(win=image_size)
self.imu = IMU()
else:
self.camera = Camera(cam='cv')
self.camera.init(win=image_size, cameraNumber=0)
self.sensors = {'create': None, 'imu': None, 'camera': None}
self.distance = 0.0
self.ahrs = AHRS()
self.modes = {
'js': JoyStickMode(self.bot),
'demo': DemoMode(self.bot),
'auto': AutoMode(self.bot),
'idle': IdleMode(self.bot),
'sensors': Sensors(self.bot)
}
# self.bag = Bag('./data/robot-{}.json'.format(time.ctime().replace(' ', '-')), ['imu', 'camera', 'create'])
self.current_mode = 'idle'
self.last_time = time.time()
self.data = {
# 'r': CircularBuffer(30),
# 'p': CircularBuffer(30),
'y': CircularBuffer(30),
}
def __del__(self):
# self.bag.close()
self.bot.drive_direct(0, 0)
self.current_mode = 'idle'
print('Create2 robot is exiting ...')
def setMode(self, mode):
if mode in self.modes:
self.current_mode = mode
else:
raise Exception(
'Create2::setMode selected mode is invalid: {}'.format(mode))
def run(self):
self.bot.start()
self.bot.safe()
# self.bot.full()
# self.setMode('js')
self.setMode('demo')
# self.setMode('idle')
# self.setMode('auto')
# self.setMode('sensors')
while True:
self.get_sensors()
self.printInfo()
# control roobma
self.modes[self.current_mode].go(self.sensors)
# time.sleep(0.05)
def processImage(self, img):
pass
def printInfo(self):
sensors = self.sensors['create']
imu = self.sensors['imu']
if sensors is None or imu is None:
print('No valid sensor info')
return
a, m, g = imu
now = time.time()
dt = self.last_time - now
beta = 0.5
q = self.ahrs.updateAGM(a, m, g, beta, dt)
r, p, y = quat2euler(q)
# self.data['r'].push(r)
# self.data['p'].push(p)
self.data['y'].push(y)
self.last_time = now
ir = []
for i in [36, 37, 38, 39, 40, 41]:
ir.append(sensors[i])
cliff = []
for i in [20, 21, 22, 23]:
cliff.append(sensors[i])
po = [
'--------------------------------------------------------',
' Light Bumper: {:6} {:6} {:6} L| {:6} |R {:6} {:6} {:6}'.format(
sensors.light_bumper_left,
sensors.light_bumper_front_left,
sensors.light_bumper_center_left,
sparkline.sparkify(ir).encode('utf-8'),
# '',
sensors.light_bumper_center_right,
sensors.light_bumper_front_right,
sensors.light_bumper_right),
' Cliff: {:6} {:6} L| {:4} |R {:6} {:6}'.format(
sensors.cliff_left_signal,
sensors.cliff_front_left_signal,
sparkline.sparkify(cliff).encode('utf-8'),
# '',
sensors.cliff_front_right_signal,
sensors.cliff_right_signal),
' Encoders: {:7} L|R {:7}'.format(sensors.encoder_counts_left,
sensors.encoder_counts_right),
' Distance Delta: {:8} mm Total: {:10.1f} m'.format(
sensors.distance, self.distance),
# ' Yaw: {:8.1f} {:30} degrees'.format(self.data['y'].get_last(), self.data['y'].spark()),
'--------------------------------------------------------',
' Power: {:6} mAhr [{:3} %]'.format(
sensors.battery_charge,
int(100.0 * sensors.battery_charge /
sensors.battery_capacity)),
' Voltage: {:7.1f} V Current: {:7.1f} A'.format(
sensors.voltage / 1000, sensors.current / 1000)
]
for s in po:
print(s)
def get_sensors(self):
cr = self.bot.get_sensors()
self.sensors['create'] = cr
# self.bag.push('create', cr)
imu = self.imu.get()
self.sensors['imu'] = imu
# self.bag.push('imu', imu)
self.distance += cr.distance / 1000.0
# if 'camera' in self.keys:
self.sensors['camera'] = None
ret, img = self.camera.read()
if ret:
self.sensors['camera'] = img
else:
print("*** No camera image ***")
class Create2(object):
cr = None
adc = None
imu = None
camera = None
sensors = {}
def __init__(self, port='/dev/ttyUSB0'):
self.bot = pycreate2.Create2(port)
# self.js = Joystick()
self.camera = Camera(cam='cv')
self.camera.init(win=(320, 240), cameraNumber=0)
# only create if on linux, because:
# imu needs access to i2c
# adc needs access to spi
# if platform.system() == 'Linux':
# self.imu = IMU()
# self.adc = MCP3208()
print('Start')
print(self.bot.inputCommands([35]))
def __del__(self):
# if self.bot:
# self.bot.stop()
pass
def doPath(self):
path = [['forward', 200, 2, 'for'], ['back', -200, 2, 'back'],
['stop', 0, 0.1, 'stop'], ['turn right', 100, 2, 'rite'],
['turn left', -100, 4, 'left'], ['turn right', 100, 2, 'rite'],
['stop', 0, 0.1, 'stop']]
# path to move
for mov in path:
# move robot
name, vel, dt, string = mov
print('movement:', name)
self.bot.digit_led_ascii(string)
if name in ['forward', 'back', 'stop']:
self.bot.drive_straight(vel)
elif name in ['turn right', 'turn left']:
self.bot.drive_turn(vel, -1)
else:
raise Exception('invalid movement command')
# time.sleep(0.05)
time.sleep(dt)
def processSafety(self, ir, wheel_drops, bumpers):
# print('ir', ir)
level = 200
vel = 200
forward = (vel, 0)
left = (vel, 1)
right = (vel, -1)
dt = 0
direction = forward
reverse = False
# handle wheel drops
for danger in wheel_drops:
if danger:
print('crap ... lost the floor!!!')
return (-vel, 0), 0
# handle physical bumpers
if bumpers[0]:
print('bumper left: hit something on our left')
self.bot.drive_straight(-vel)
time.sleep(0.5)
return right, 2
if bumpers[1]:
print('bumper right: hit something on our right')
self.bot.drive_straight(-vel)
time.sleep(0.5)
return left, 2
# handle light bumpers
if ir[0] > level:
dt = 0.5
direction = right
reverse = True
if ir[1] > level:
dt = 1.0
direction = right
reverse = True
if ir[2] > level:
dt = 1.5
direction = right
reverse = True
if ir[5] > level:
dt = 0.5
direction = left
reverse = True
if ir[4] > level:
dt = 1.0
direction = left
reverse = True
if ir[3] > level:
dt = 1.5
direction = left
reverse = True
if reverse:
self.bot.drive_straight(-vel)
time.sleep(0.5)
return direction, dt
def run(self):
print('run')
print(self.bot.inputCommands([35]))
self.bot.start()
print('after start')
print(self.bot.inputCommands([35]))
self.bot.safe()
print('after safe')
print(self.bot.inputCommands([35]))
# self.bot.full()
exit(0)
# health = [14, 22, 23, 24, 25, 26, 54, 55]
# nav = [19, 20, 107]
# safety = [7, 9, 10, 11, 12, 46, 47, 48, 49, 50, 51]
# path = [
# ['forward', 200, 2, 'for'],
# ['back', -200, 2, 'back'],
# ['stop', 0, 0.1, 'stop'],
# ['turn right', 100, 2, 'rite'],
# ['turn left', -100, 4, 'left'],
# ['turn right', 100, 2, 'rite'],
# ['stop', 0, 0.1, 'stop']
# ]
light_bumper = [
'light bump left signal',
'light bump front left signal',
'light bump center left signal',
'light bump center right signal',
'light bump front right signal',
'light bump right signal',
]
curr_dist = 0.0
# rn for a certain number of steps
for _ in range(200):
# get sensor readings
# health_info = self.bot.inputCommands(health)
# nav_info = self.bot.inputCommands(nav)
# safety_info = self.bot.inputCommands(safety)
# print('sensors:', info)
safety_info = self.bot.inputCommands([100]) # get everything
if not safety_info:
exit(1)
ir = []
for key in light_bumper:
ir.append(safety_info[key])
tmp = safety_info['wheel drop and bumps']
# print('wheel/dump', tmp)
# wheel_drops = ((tmp & (1<<3)) > 0, (tmp & (1<<2)) > 0) # left, right
# bumpers = ((tmp & (1<<1)) > 0, (tmp & (1)) > 0) # left, right
wheel_drops = (tmp['drop left'], tmp['drop right'])
bumpers = (tmp['bump left'], tmp['bump right'])
direction, dt = self.processSafety(ir, wheel_drops, bumpers)
vel = direction[0]
if dt != 0:
self.bot.drive_turn(vel, direction[1])
time.sleep(dt)
else:
self.bot.drive_straight(vel)
time.sleep(0.1)
sensor_data = safety_info
print('=' * 40)
print('stasis', sensor_data['stasis'])
curr_dist += sensor_data['distance']
print('distance', curr_dist)
print('-' * 40)
print('left motor current [mA]', sensor_data['left motor current'])
print('right motor current [mA]',
sensor_data['right motor current'])
print(
'battery status [%]:', 100 * sensor_data['battery charge'] /
sensor_data['battery capacity'])
print('voltage:', sensor_data['voltage'])
print('current:', sensor_data['current'])
# grab camera image
if self.camera:
ok, img = self.camera.read()
if ok: # good image capture
self.processImage(img)
else:
print('<<< bad image >>>')
# time.sleep(0.5)
def command(self, vel, rot, scale=1.0):
"""
vel: 0-1
rot: 0-2pi
scale: scalar multiplied to velocity
"""
vel *= scale
if rot < 0.02: # a little more than a degree
self.bot.drive_straight(vel)
else:
i = int(rot * 10)
i = 0 if i < 0 else i
i = 10 if i > 10 else i
r = range(1100, 0, -100)
radius = r[i]
self.bot.drive_turn(vel, radius)
print('cmd:', vel, radius)
def processImage(self, img):
# find ar code
# print('image size', img.shape)
pass