def __init__(self):
print("init")
port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/rfcomm0")
self.create = Create(port)
self.bump = False
self.hz = 30
self.rate = 1. / self.hz
self.speed = 200
self.turn = 50
self.distance = 85
self.threshold = .5
self.angle = 18
self.lock = Lock()
self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)
self.odomPub = rospy.Publisher('odom', Odometry)
self.odomBroadcaster = TransformBroadcaster()
self.then = datetime.now()
self.x = 0
self.y = 0
self.th = 0
self.fields = [
'wheeldropCaster', 'wheeldropLeft', 'wheeldropRight', 'bumpLeft',
'bumpRight', 'wall', 'cliffLeft', 'cliffFronLeft',
'cliffFrontRight', 'cliffRight', 'virtualWall', 'infraredByte',
'advance', 'play', 'distance', 'angle', 'chargingState', 'voltage',
'current', 'batteryTemperature', 'batteryCharge',
'batteryCapacity', 'wallSignal', 'cliffLeftSignal',
'cliffFrontLeftSignal', 'cliffFrontRightSignal',
'cliffRightSignal', 'homeBase', 'internalCharger', 'songNumber',
'songPlaying'
]
self.create.update = self.sense
def __init__(self):
port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
self.create = Create(port)
self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)
self.odomPub = rospy.Publisher('odom',Odometry)
self.odomBroadcaster = TransformBroadcaster()
self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
self.then = datetime.now()
self.x = 0
self.y = 0
self.th = 0
#self.create.update = self.sense
self.create.update = None
def __init__(self):
# If /create/use_host_name is True, the topic names used by
# this driver will be /hostname/cmd_vel, /hostname/odom, etc.
self.hn = HName(usename = rospy.get_param('/create/use_host_name', False))
port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
self.create = Create(port)
self.packetPub = rospy.Publisher(self.hn.topic('sensorPacket'), SensorPacket)
self.odomPub = rospy.Publisher(self.hn.topic('odom'),Odometry)
self.odomBroadcaster = TransformBroadcaster()
self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
self.then = datetime.now()
self.x = 0
self.y = 0
self.th = 0
self.idealTh = 0
self.create.update = self.sense
# SW
self.create.storeSong(2,67,32,74,32,72,11,71,11,69,11,79,32,74,32,72,11,71,11,69,11,79,32,74,32,72,11,71,11,72,11,69,32)
# CE3K
self.create.storeSong(3,74,16,81,24,83,24,79,24,67,32,74,40)
# HN
self.create.storeSong(4,74,64,78,20,75,20,74,20,78,56,81,17,79,17,78,17,79,48,82,14,81,14,79,14,78,28,75,32,74,40)
# Js
self.create.storeSong(5,42,32,43,32,42,32,43,32,42,32,43,32,42,32,43,32)
# ROLA
self.create.storeSong(6,64,24,65,8,67,16,72,40,62,24,64,8,65,48,67,24,69,8,71,16,77,40,69,24,71,10,72,24,74,24,76,48)
# JJMD
self.create.storeSong(7,79,12,81,12,83,12,86,12,84,12,84,12,88,12,86,12,86,12,91,12,90,12,91,12,86,12,83,12,79,40)
# SaaHC
self.create.storeSong(8,84,25,79,18,79,12,81,25,79,40,83,28,84,12)
# Angry men
self.create.storeSong(9,64,24,62,12,60,24,62,12,64,24,65,12,67,36,64,12,62,12,60,12,59,24,57,12,59,24,60,12,55,48)
# IGR
self.create.storeSong(10,77,24,79,24,82,24,84,40)
#,84,24,82,24,79,24,77,40,77,24,79,24,82,24,84,18,87,18,84,10,86,32,84,42)
self.create.storeSong(11,50,10,51,10)
self.create.storeSong(12,51,10,50,10)
self.create.storeSong(13,52,10,51,10,50,10)
self.create.storeSong(14,53,10,54,10,55,10)
self.create.storeSong(15,54,10,54,10)
def cos(model, sample):
a = 0.0
b = 0.0
c = 0.0
for i in xrange(0, 30):
for j in xrange(0, 3):
a = a + model[i][j] * sample[i][j]
b = b + model[i][j] * model[i][j]
c = c + sample[i][j] * sample[i][j]
return 1.0 - (a / (sqrt(b) * sqrt(c)))
#create = Create("/dev/rfcomm0")
create = Create()
a = cwiid.Wiimote()
allLEDS = cwiid.LED1_ON ^ cwiid.LED2_ON ^ cwiid.LED3_ON ^ cwiid.LED4_ON
a.led = allLEDS
a.rpt_mode = cwiid.RPT_ACC ^ cwiid.RPT_BTN
cal = a.get_acc_cal(0)
zero = cal[0]
one = cal[1]
print zero, one
create.start()
global bump, rate, lock, speed, turn, create, distance, angle, norms, threshold
bump = False
hz = 30
rate = 1. / hz
speed = 200
turn = 50
distance = 85
threshold = .5
angle = 18
lock = Lock()
rospy.init_node('discreteController')
port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
print(port)
create = Create(port)
service = rospy.Service('act', Act, act)
try:
sleep(1)
create.start()
sleep(1)
norms = [
create.cliffLeftSignal, create.cliffFrontLeftSignal,
create.cliffFrontRightSignal, create.cliffRightSignal
]
print "\nrunning...\n"
rospy.spin()
except:
pass
finally:
