def edge_detection():
ir = ReflectanceSensors(max_reading=3000, min_reading=1000)
button = ZumoButton()
while True:
button.wait_for_press()
reading = ir.update()
print(reading)
time.sleep(0.6)
def tourist(steps=25,shots=5,speed=.25):
ZumoButton().wait_for_press()
rs = ReflectanceSensors(); m = Motors(); c = Camera()
for i in range(steps):
random_step(m,speed=speed,duration=0.5)
vals = rs.update()
if sum(vals) < 1: # very dark area
im = shoot_panorama(c,m,shots)
im.dump_image('vacation_pic'+str(i)+'.jpeg')
def tourist(steps=25,shots=5,speed=.25):
ZumoButton().wait_for_press()
rs = ReflectanceSensors(); m = Motors(); c = Camera()
for i in range(steps):
random_step(m,speed=speed,duration=0.5)
vals = rs.update()
if sum(vals) < 1: # very dark area
im = shoot_panorama(c,m,shots)
im.dump_image('vacation_pic'+str(i)+'.jpeg')
class ReflectanceSob:
def __init__(self):
self.sensor = ReflectanceSensors(auto_calibrate=False)
self.max_val = self.sensor.max_val
def update(self):
print('reflectance sensor updates')
self.sensor.update()
self.value = self.sensor.value
def reset(self):
self.sensor.reset()
class ReflectanceSob:
def __init__(self):
self.sensor = ReflectanceSensors(auto_calibrate=False)
self.max_val = self.sensor.max_val
def update(self):
print('reflectance sensor updates')
self.sensor.update()
self.value = self.sensor.value
def reset(self):
self.sensor.reset()
class ReflectanceSensob(Sensob):
def __init__(self):
super(ReflectanceSensob, self).__init__()
self.sensor = ReflectanceSensors()
self.sensors.append(self.sensor)
def get_value(self):
''' returnerer en liste med verdier: [left, midleft, midright right]'''
return self.value
def update(self):
self.sensor.update()
self.value = self.sensor.get_value()
return self.value
def lineCollision():
reflect = ReflectanceSensors()
ZumoButton().wait_for_press()
motor = Motors()
stuck = CheckStuck()
ultra = Ultrasonic()
proxim = IRProximitySensor()
sensobCol = Sensob()
sensobCol.set_sensors([ultra, proxim])
motob = Motob(motor=motor)
sensobLine = Sensob()
sensobLine.set_sensors([reflect])
arb = Arbitrator(motob=motob)
bbcon = BBCON(arbitrator=arb, motob=motob)
bbcon.set_checkStucker(stuck)
line = LineFollow(1, [sensobLine])
col = CollisionAvoidance(1, [sensobCol])
bbcon.add_behavior(line)
bbcon.add_behavior(col)
bbcon.add_sensob(sensobCol)
bbcon.add_sensob(sensobLine)
count = 0
while count < 20:
bbcon.run_one_timestep()
count += 1
def lineTest():
reflect = ReflectanceSensors()
ZumoButton().wait_for_press()
motor = Motors()
stuck = CheckStuck()
camera = Camera()
motob = Motob(motor)
arbitrator = Arbitrator(motob=motob)
sensob = Sensob()
sensob.set_sensors([reflect])
bbcon = BBCON(arbitrator=arbitrator, motob=motob)
bbcon.add_sensob(sensob)
bbcon.set_checkStucker(stuck)
b = LineFollow(1, [sensob])
bbcon.add_behavior(b)
timesteps = 0
while timesteps < 30:
bbcon.run_one_timestep()
timesteps += 1
def start():
bbcon = BBCON()
arb = Arbitrator(bbcon)
motor = Motors()
reflect_sens = ReflectanceSensors(False)
cam = Camera()
ir = IR()
ultra = Ultrasonic()
bbcon.add_motobs(motor)
bbcon.set_arb(arb)
bbcon.add_behaviour(AvoidObj(bbcon, ultra, ir))
bbcon.add_behaviour(Behaviour_line_follower(bbcon, reflect_sens))
bbcon.add_behaviour(fub(bb=bbcon))
#behaviour avoid blue, has to be added last, do not change this, will screw up bbcon code
bbcon.add_behaviour(Behaviour_avoid_blue(bb=bbcon, cam=cam, ultra=ultra))
bbcon.add_sensob(reflect_sens)
bbcon.add_sensob(ir)
bbcon.add_sensob(ultra)
#cam has to be added last, will screw up bbcon code if not
bbcon.add_sensob(cam)
butt = ZumoButton()
butt.wait_for_press()
print("Start zumo")
while True:
bbcon.run_one_timestep()
def main():
motob = Motob()
bbcon = Bbcon(motob)
arbitrator = Arbitrator(bbcon)
bbcon.set_arbitrator(arbitrator)
# sensorer og sensob
ult_sensor = Ultrasonic()
ref_sensor = ReflectanceSensors(auto_calibrate=False)
reflectance_sensob = ReflectanceSensob(ref_sensor)
ultrasonic_sensob = UltrasonicSensob(ult_sensor)
camera_sensob = CameraSensob(None, color=0)
#behaviors
dont_crash = DontCrash(bbcon, ultrasonic_sensob)
follow_line = FollowLine(bbcon, reflectance_sensob)
find_object = FindColoredObject(bbcon, camera_sensob)
bbcon.add_behavior(dont_crash)
bbcon.add_behavior(follow_line)
bbcon.add_behavior(find_object)
try:
ZumoButton().wait_for_press()
while not bbcon.object_found: # Kjører helt til vi finner objektet
bbcon.run_one_timestep()
except KeyboardInterrupt:
motob.motor.stop()
finally:
GPIO.cleanup()
def startup(self):
# add sensor objects
cam = Sensob(Camera())
ultrasonic = Sensob(Ultrasonic())
ir_sensor = Sensob(ReflectanceSensors())
self.add_sensob(cam)
self.add_sensob(ultrasonic)
self.add_sensob(ir_sensor)
# add behaviors
sb = StandardBehavior(self)
self.add_behavior(sb)
self.activate_behavior(sb)
cb = CameraBehavior(self)
self.add_behavior(cb)
self.activate_behavior(cb)
ub = UltraBehavior(self)
self.add_behavior(ub)
self.activate_behavior(ub)
ir = IRBehavior(self)
self.add_behavior(ir)
self.activate_behavior(ir)
button = ZumoButton()
button.wait_for_press()
self._running = True
while self._running:
self.run_one_timestep()
# wait
time.sleep(0.001)
def main():
# initialisering
ZumoButton().wait_for_press()
bbcon = BBCON()
us = Ultrasonic()
ir_sensob = Sensob(
ReflectanceSensors(True)) # True betyr med auto-kalibrering
usonic_sensob = Sensob(us)
camera_sensob = Sensob(sensor=Camera(), sensor2=us)
avoid_borders = Avoid_borders(ir_sensob, bbcon)
walk_randomly = Walk_randomly(None, bbcon)
clean = Clean(usonic_sensob, bbcon)
approach = Approach(usonic_sensob, bbcon)
take_photo = Take_photo(camera_sensob, bbcon)
# setup
bbcon.add_sensob(ir_sensob) # legger til IR sensob
bbcon.add_sensob(usonic_sensob) # legger til Ultrasonic sensob
#bbcon.add_sensob(camera_sensob) # legger til Ultrasonic/camera sensob
bbcon.add_behavior(avoid_borders) # legger til avoid_borders
bbcon.add_behavior(walk_randomly) # legger til walk_randomly
#bbcon.add_behavior(clean) # legger til clean
bbcon.add_behavior(approach) # legger til approach
bbcon.add_behavior(take_photo) # legger til take_photo
bbcon.activate_behavior(avoid_borders)
bbcon.activate_behavior(walk_randomly)
bbcon.activate_behavior(approach)
#bbcon.activate_behavior(take_photo)
while True:
bbcon.run_one_timestep()
def setup(self):
# LineBehavior
rs = ReflectanceSensors()
rsob = ReflectanceSensob(rs)
lineb = FollowLineBehavior(self, [rsob], False, 0.7)
self.add_behavior(lineb)
self.activate_behavior(lineb)
self.add_sensob(rsob)
# Forward Behavior
forwardb = ForwardBehavior(self, [rsob], False, 0.2)
self.add_behavior(forwardb)
self.activate_behavior(forwardb)
# Follow green flask
my_camera = Camera()
self.followgreensensob = GreenDirectionSensob(my_camera)
followgreenb = FollowGreenFlask(self, [self.followgreensensob, rsob],
False, 1.0)
self.add_behavior(followgreenb)
self.activate_behavior(followgreenb)
self.add_sensob(self.followgreensensob)
# Avoid Collision
us = Ultrasonic()
ir = IRProximitySensor()
self.irob = IRProximitySensob(ir)
self.usob = UltrasonicSensob(us)
self.add_sensob(self.irob)
self.add_sensob(self.usob)
avoidb = AvoidCollisionBehavior(self, [self.usob, self.irob, rsob],
False, 1.0)
self.add_behavior(avoidb)
self.activate_behavior(avoidb)
def __init__(self, BBCON, priority):
super().__init__(BBCON)
print("We are going in to calibration mode in 2 sec")
time.sleep(2)
self.sensob = ReflectanceSensors(auto_calibrate=True)
self.priority = priority #Preset value that is set by the user
self.sensob.update()
self.old = self.sensob.get_value()
def __init__(self, auto_calibration=True, THRESHOLD=0.9):
self.bbqon = bb
self.THRESHOLD = THRESHOLD
ref_sensors = ReflectanceSensors(auto_calibration)
self.sensobs = [ref_sensors]
self.motor_recommandations = [('f', 0, 0)]
self.priority = 4
self.match_degree = 0.0
self.weight = self.priority * self.match_degree
def bbrun():
# Main function sets up motobs/sensobs and behavors
sensobs = []
# Calibration begins here. Students should slowly spin the robot around the
# line in a circle trying hard not to lift it up off the ground. As of now,
# calibration lasts 15 seconds which is plenty of time. Without proper
# calibration, the line following behavior may not work properly.
# Calibration determines the maximum and minimum values found by each sensor,
# which is necessary for returning the normalized values (reals from 0 to 1)
# of each sensor.
# Edit: I have turned off calibration for now, because hard-coding the configuration
# is working good enough and I don't have to use time on calibrating under testing
motors = Motors()
motors.stop()
sleep(2)
# To exit the program properly on keyboardinterrupt
def signal_handler(signal, frame):
motors.stop()
GPIO.cleanup()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
reflectanceSensob = bbc.Sensob(ReflectanceSensors())
cameraSensob = bbc.Sensob(Camera())
sensobs.append(reflectanceSensob)
sensobs.append(cameraSensob)
motob = bbc.Motob(motors)
bbcon = RobotController(None) # None agent
# Create the behaviors
# The behaviors has priorities of 3,3,5,and 1 respectively
followLineBehavior = FollowLine(bbcon, sensobs[0:1])
camera_search_behavior = Camera_search(bbcon, sensobs[1:2])
stopFollowingLineBehavior = StopFollowLine(bbcon, sensobs[0:1])
wanderBehavior = Wander(bbcon)
bbcon.add_behavior(followLineBehavior)
bbcon.add_behavior(stopFollowingLineBehavior)
bbcon.add_behavior(wanderBehavior)
bbcon.add_behavior(camera_search_behavior)
bbcon.add_motob(motob)
bbcon.add_sensob(reflectanceSensob)
bbcon.add_sensob(cameraSensob)
i = 0
while not bbcon.is_blob_found():
print("Iteration " + str(i))
i += 1
print("Found gate: " + str(bbcon.found_gate))
bbcon.run_one_timestep()
# Called at the end
motors.stop()
def __init__(self):
self.sensors = {
'camera': Camera(img_width=IMG_WIDTH, img_height=IMG_HEIGHT),
'ultrasonic': Ultrasonic(),
'reflectance': ReflectanceSensors()
}
self.values = {
'camera': None,
'ultrasonic': None,
'reflectance': None,
}
def __init__(self, debug=False):
self.motor = Motors()
self.behaviors = []
self.debug = debug
self.sensobs = {
Camera(img_width=IMG_WIDTH, img_height=IMG_HEIGHT): None,
Ultrasonic(): None,
ReflectanceSensors(): None
}
self.arbitrator = None
def test4():
sensor = ReflectanceSensors()
ZumoButton().wait_for_press()
m = Motors()
motob = Motob(m)
sensob = Sensob()
sensob.set_sensors([sensor])
print(sensor.get_value())
behavior = FollowLine(1, [sensob])
#print("Behavior sensob:", behavior.sensobs)
count = 0
while True:
sensob.update()
behavior.update()
print("MR:", behavior.motor_recommendations)
motob.update(behavior.motor_recommendations[0])
count += 1
#time.sleep(3)
if count == 15:
break
class AvoidEdge(Behavior):
def __init__(self, BBCON, priority):
super().__init__(BBCON)
print("We are going in to calibration mode in 2 sec")
time.sleep(2)
self.sensob = ReflectanceSensors(auto_calibrate=True)
self.priority = priority #Preset value that is set by the user
self.sensob.update()
self.old = self.sensob.get_value()
def printName(self):
return "Behavior AvoidEdge "
def update(self):
super().considerState()
if self.active_flag:
self.sense_and_act()
self.weight = self.priority * self.match_degree
def sense_and_act(self):
self.sensob.update()
self.new = self.sensob.get_value()
print(sum(self.old)/len(self.old) - sum(self.new)/len(self.new) > 0.1)
if (sum(self.old)/len(self.old) - sum(self.new)/len(self.new) > 0.1 ):
self.motor_recommendations = [-1,-1]
self.match_degree = 1
else:
self.match_degree = 0
self.motor_recommendations = [1,1]
self.old = self.new
def test(self):
return True
class FollowLine:
def __init__(self):
self.reflectanseSensor = ReflectanceSensors()
self.values = self.getValueList()
self. values = self.isOnLine()
def getValueList(self):
self.reflectanseSensor.reset()
self.reflectanseSensor.update()
return self.reflectanseSensor.get_value()
def isOnLine(self):
nowValue = self.getValueList()
offLineList = [0,1,4,5]
for i in range(6):
if i in offLineList and nowValue[i] < 0.2:
return [(1000-(nowValue[i]*1000)),i]
return [0,10]
def getPriValues(self):
return self.isOnLine()
def underside(auto_C=False, max=100000, min=0):
try:
ref_sensor = ReflectanceSensors(auto_calibrate=auto_C,
min_reading=min,
max_reading=max)
reflectance_sensob = ReflectanceSensob(ref_sensor)
while True:
print(reflectance_sensob.update())
sleep(0.1)
except KeyboardInterrupt:
pass
finally:
GPIO.cleanup()
class FallingOut(Sensob):
def __init__(self):
super(FallingOut, self).__init__()
self.sensor = ReflectanceSensors(max_reading=3000, min_reading=1000)
self.value = []
def update(self):
self.sensor.update()
self.value = self.sensor.get_value()
def reset(self):
pass
def calibrate(self):
self.sensor = ReflectanceSensors(auto_calibrate=True)
def interpret(self):
danger = 0
if self.value[0] < 0.65:
danger = -1
elif self.value[5] < 0.65:
danger = 1
return danger
class Reflectance(Sensob):
def __init__(self):
super().__init__("Reflectance")
self.reflect = ReflectanceSensors()
def get_sensor_current_value(self):
value = self.reflect.update()
return value
def update(self):
self.value = self.get_sensor_current_value()
def get_value(self):
return self.value
def reflectTest():
#motor = Motors()
#motob = Motob(motor=motor)
reflect = ReflectanceSensors()
sensob = Sensob()
sensob.set_sensors([reflect])
ZumoButton().wait_for_press()
count = 0
while count < 10:
sensob.update()
print(sensob.get_values())
count += 1
time.sleep(2.5)
def __init__(self):
self.arbitrator = Arbitrator(self)
#Oppretter fire sensob objekter. Kamera, ir, reflectance og ultra.
cam = Sensob()
cam.add_sensor(Camera())
ir = Sensob()
ir.add_sensor(IRProximitySensor())
reflect = Sensob()
reflect.add_sensor(ReflectanceSensors())
ultra = Sensob()
ultra.add_sensor(Ultrasonic())
self.sensob = [cam,ir,reflect,ultra]
self.motobs = [Motob()]
self.behaviors = []
self.wall_detected = False
self.wall_checked = False
def init_bbcon(bbcon):
reflectance_sensob = Sensob(ReflectanceSensors())
camera_sensob = Sensob(Camera())
ultrasonic = Sensob(Ultrasonic())
bbcon.add_sensob(reflectance_sensob)
bbcon.add_sensob(camera_sensob)
bbcon.add_sensob(ultrasonic)
take_picture = TakePicture(bbcon)
collision_avoidance = CollisionAvoidance(bbcon)
image_behavior = ImageBehavior(bbcon)
bbcon.add_behavior(take_picture)
bbcon.add_behavior(collision_avoidance)
bbcon.add_behavior(image_behavior)
def rett_fram():
ir = ReflectanceSensors(True)
s = Sensob(ir)
ab = Avoid_borders(s, s)
wr = Walk_randomly(s, s)
a = Arbitrator()
m = Motob()
print("Motob set")
ZumoButton().wait_for_press()
print("Button pressed")
while True:
ab.update()
wr.update()
print("Vekt: ", ab.weight)
print("Rec: ", ab.motor_recommendations)
winner_rec = a.choose_action(ab, wr)
print("recom: ", winner_rec)
m.update(winner_rec)
def main():
#pdb.set_trace()
ZumoButton().wait_for_press()
bbcon = BBCON()
# Sensors:
reflectance_sensor = ReflectanceSensors()
ultrasonic_sensor = Ultrasonic()
camera_sensor = Camera()
# Sensobs
reflectance_sensob = Sensob(reflectance_sensor)
ultrasonic_sensob = Sensob(ultrasonic_sensor)
camera_sensob = Sensob(camera_sensor)
trackline_sensobs = reflectance_sensob
distance_adjust_sensobs = ultrasonic_sensob
follow_red_sensobs = camera_sensob
# Add to bbcon:
# Add sensobs
bbcon.add_sensob(trackline_sensobs)
bbcon.add_sensob(distance_adjust_sensobs)
bbcon.add_sensob(follow_red_sensobs)
# Add motobs:
bbcon.motobs = Motob()
# Add behaviors
bbcon.add_behavior(TrackLine(bbcon, trackline_sensobs))
bbcon.add_behavior(DistanceAdjust(bbcon, distance_adjust_sensobs))
bbcon.add_behavior(FollowRed(bbcon, follow_red_sensobs))
# Set all behaviors to active at start; disable if not needed
for behavior in bbcon.behaviors:
bbcon.activate_behavior(behavior)
# Run as long as robot is not halted
# not bbcon.halt_request
#pdb.set_trace()
while True:
bbcon.run_one_timestep()
def systemTest():
motor = Motors()
ultra = Ultrasonic()
proxim = IRProximitySensor()
camera = Camera()
reflectance = ReflectanceSensors()
motob = Motob(motor)
arbitrator = Arbitrator(motob)
stuck = CheckStuck()
collisionSensob = Sensob()
collisionSensob.set_sensors([ultra, proxim])
lineSensob = Sensob()
lineSensob.set_sensors([reflectance])
trackSensob = Sensob()
trackSensob.set_sensors([ultra, camera])
b = CollisionAvoidance(1, [collisionSensob])
f = LineFollow(1, [lineSensob])
t = TrackObject(1, [trackSensob])
#print(collisionSensob.sensors)
#print(lineSensob.sensors)
bbcon = BBCON(arbitrator=arbitrator, motob=motob)
bbcon.set_checkStucker(stuck)
bbcon.add_behavior(b)
bbcon.add_behavior(f)
bbcon.add_behavior(t)
#bbcon.activate_behavior(0)
#bbcon.activate_behavior(1)
#bbcon.activate_behavior(2)
bbcon.add_sensob(collisionSensob)
bbcon.add_sensob(lineSensob)
bbcon.add_sensob(trackSensob)
runTimesteps(bbcon, 100)
def main():
ZumoButton().wait_for_press()
sensorUS = Ultrasonic()
sensorIR = IRProximitySensor()
sensorReflect = ReflectanceSensors()
sensob0 = US_Sensob(sensorUS)
sensob1 = IRP_Sensob(sensorIR)
sensob2 = Reflect_snap_Sensob(sensorReflect)
bbcon = BBCON([sensob0, sensob1, sensob2])
drive = Move_straight_ahead(bbcon)
avoid_shit = Avoid_front_collision(bbcon, [sensob0, sensob1])
snap_by_line = Snap_by_line(bbcon, [sensob2])
bbcon.add_behavior(avoid_shit)
bbcon.add_behavior(drive)
bbcon.add_behavior(snap_by_line)
keep_going = True
while keep_going:
keep_going = bbcon.run_one_timestep()
def test():
sensor = ReflectanceSensors()
for x in range(30):
print(sensor.update())
def __init__(self):
super(ReflectanceSensob, self).__init__()
self.sensor = ReflectanceSensors()
self.sensors.append(self.sensor)
# for all motors.
def update_all_motobs(self, tuple):
print(tuple)
for i in range(len(self.motob)):
# Motob can also check the halt_flag
self.motob[i].update(tuple)
if __name__ == '__main__':
bbcon = Bbcontroller()
arbitrator = Arbitrator(bbcon)
zumo = ZumoButton()
zumo.wait_for_press()
r = ReflectanceSensors()
u = Ultrasonic()
c = Camera()
sensobs = [r, u, c]
bbcon.arbitrator = arbitrator
fl = FollowLine(bbcon, sensobs)
ac = AvoidCollisions(bbcon, sensobs)
sp = SnapPhoto(sensobs)
ro = Rotate(sensobs)
st = Stop(bbcon, sensobs)
dc = DoCircles(sensobs)
bbcon.add_behavior(dc)
bbcon.add_behavior(fl)
def __init__(self):
self.sensor = ReflectanceSensors(auto_calibrate=False)
self.max_val = self.sensor.max_val