import time
from sw import constants
from math import pi
from sw.hal import Robot
from sw.gui import Window, ControlPanel
import cv2
""" Make sure the robot has enough clearance to release blocks """
CLOSE_TO_WALL = 12
if __name__ == '__main__':
with tamproxy.TAMProxy() as tamp:
r = Robot(tamp)
w = Window(500, [ControlPanel(r)])
startAngle = r.drive.odometer.val.theta
# If went all the way around without finding anything, pick the best direction
# = combine with side sensors and knowing angles
# Turn around until the front of the robot is clear for N inches or it has gone 360
r.drive.go(0, 0.1)
print r.left_long_ir.distInches, r.right_long_ir.distInches
while (r.left_long_ir.distInches < constants.close_to_wall and \
r.right_long_ir.distInches < constants.close_to_wall) or \
(abs(r.drive.odometer.val.theta - startAngle) <= 2*pi):
print r.left_long_ir.distInches, r.right_long_ir.distInches
r.drive.go(0, 0)
from sw.mapping import Mapper
from sw import constants
import time
import cv2
from tamproxy import TAMProxy
if __name__ == "__main__":
with TAMProxy() as tamproxy:
r = Robot(tamproxy)
m = Mapper(r.drive.odometer)
cam = Camera(geom=constants.camera_geometry, id=2)
v = Vision(cam)
w = Window(500, [m, CameraPanel(v), ControlPanel(r)])
while True:
try:
v.update()
except IOError:
continue
m.update_cubes_from(v)
c = w.get_key()
move_cmd = None
if c == 'q':
break
elif c == 'w':
from tamproxy import TAMProxy
from tamproxy.devices import LongIR
if __name__ == "__main__":
with TAMProxy() as tamproxy:
drive = Drive(tamproxy)
leftIR = LongIR(tamproxy, pins.l_ir_long)
rightIR = LongIR(tamproxy, pins.r_ir_long)
left_pid = sw.util.PID(0.1, setpoint=20)
right_pid = sw.util.PID(0.1, setpoint=20)
m = Mapper(drive.odometer)
cam = Camera(geom=sw.constants.camera_geometry, id=0)
v = Vision(cam)
w = Window(500, [m, CameraPanel(500, v)])
while True:
try:
v.update()
except IOError:
continue
m.update_cubes_from(v)
print "Right: {:.1f}, Left: {:.1f}".format(
rightIR.distInches, leftIR.distInches)
if leftIR.distInches < 14 and rightIR.distInches < 14:
print("corner")
drive.go(steer=0.1)
elif rightIR.distInches < leftIR.distInches:
from tamproxy.devices import *
from tamproxy import TAMProxy
import time
from sw.gui import Window
from sw import pins
from sw.hal.sensors import BreakBeams
if __name__ == "__main__":
with TAMProxy() as tamp:
w = Window(500, [])
b = BreakBeams(tamp)
while True:
print b.blocked, b.l_beam._recv_pin.val, b.r_beam._recv_pin.val
time.sleep(0.1)
elif c == 's':
new_t = drive.go_to([0, 0])
elif c == ' ' and t:
t.cancel()
try:
yield From(t)
except asyncio.CancelledError:
pass
if new_t:
if t:
t.cancel()
try:
yield From(t)
except asyncio.CancelledError:
pass
t = asyncio.ensure_future(new_t)
yield From(asyncio.sleep(0.05))
if __name__ == "__main__":
with TAMProxy() as tamproxy:
drive = RegulatedDrive(tamproxy)
m = Mapper(drive.odometer)
w = Window(500, [m])
loop = asyncio.get_event_loop()
loop.run_until_complete(main_task())
loop.close()
""" np array to integer tuple """
try:
l = len(val)
except Exception:
return int(val)
else:
return tuple(val.astype(int))
if __name__ == "__main__":
with TAMProxy() as tamp:
d = Drive(tamp)
odo = d.odometer
mapper = Mapper(odo, size=500, ppi=250.0 / 20)
w = Window(500, [mapper])
with open('odo.csv', 'w') as logfile:
logwriter = csv.writer(logfile)
logwriter.writerow(('t', ) + odo.Reading._fields)
while True:
# print time.time(), odo.val
logwriter.writerow((time.time(), ) + odo.val)
c = w.get_key()
if c == 'r':
odo.override_position(12, 12, np.pi / 8)
elif c == 'q':
break
from sw.mapping import Mapper
from sw import constants
import time
import cv2
from tamproxy import TAMProxy
if __name__ == "__main__":
with TAMProxy() as tamproxy:
r = Robot(tamproxy)
m = Mapper(r.drive.odometer)
cam = Camera(geom=constants.camera_geometry, id=2)
v = Vision(cam)
w = Window(500, [m, CameraPanel(v), ControlPanel(r)])
j = JoystickInterface()
while True:
try:
v.update()
except IOError:
continue
m.update_cubes_from(v)
j.set_bumpers(r.left_bumper.val, r.right_bumper.val)
if j.left_arm:
r.drive.stop()
r.arms.silo.up()
while j.left_arm: