def __init__(self):
self.behaviors = []
self.active_behaviors = []
self.sensobs = []
self.motob = Motob()
self.arbitrator = Arbitrator(self)
self.setup()
def __init__(self):
self.behaviors = []
self.active_behaviors = []
self.sensobs = []
self.motob = Motob()
self.arbitrator = Arbitrator(self)
self.picture_taken = False
def main():
print("Press the button")
Led().light_on()
ZumoButton().wait_for_press()
Led().light_off()
print("Button pressed")
# Sensors
camera_sensor = Camera(img_width=200, img_height=25)
reflectance_sensor = ReflectanceSensors()
ultrasonic_sensor = Ultrasonic()
# Other
led = Led()
led.light_off()
# Sensobs
camera_sensob = CameraSensob(camera_sensor)
reflectance_sensob = ReflectanceSensob(reflectance_sensor)
ultrasonic_sensob = Sensob(ultrasonic_sensor)
# Motobs
motob = Motob()
# Controller
bbcon = Bbcon()
# Behaviors
moveForward_behavior = MoveForward(None, 1, bbcon)
avoidCollision_behavior = AvoidCollision(ultrasonic_sensob, 4, bbcon)
followColor_behavior = FollowColor(camera_sensob, 6, bbcon)
pauseAtLines_behavior = PauseAtLines(reflectance_sensob, 8, bbcon)
stopCloseColor_behavior = StopCloseColor(camera_sensob, 10, bbcon)
# Add all sensobs to controller
bbcon.add_sensob(camera_sensob)
bbcon.add_sensob(reflectance_sensob)
bbcon.add_sensob(ultrasonic_sensob)
# Add all Behaviors to controller
bbcon.add_behavior(avoidCollision_behavior)
bbcon.add_behavior(moveForward_behavior)
bbcon.add_behavior(followColor_behavior)
bbcon.add_behavior(pauseAtLines_behavior)
bbcon.add_behavior(stopCloseColor_behavior)
bbcon.activate_all_behaviors()
# Add all motobs to controller
bbcon.add_motob(motob)
print("starting")
try:
while bbcon.active:
bbcon.run_one_timestep()
except:
print("Failed")
motob.stop()
print(sys.exc_info()[0])
class BBCON:
def __init__(self):
self.behaviors = []
self.sensobs = []
self.motob = Motob()
self.motob.update("brake")
self.arbitrator = Arbitrator()
def add_behavior(self, behavior):
self.behaviors.append(behavior)
def add_sensob(self, sensob):
self.sensobs.append(sensob)
# def activate_behavior(self, behavior):
# self.active_behaviors.add(behavior)
# def deactive_behavior(self, behavior):
# self.active_behaviors.remove(behavior)
def move(self, action):
self.motob.update(action)
def run_one_timestep(self, delay=0.667):
start_time = time()
# Reflectance sensor index 0
# Proximity sensor index 1
# Camera sensor index 2
#self.data = [s.update() for s in self.sensobs]
self.data = []
for s in self.sensobs:
tmp = time()
self.data.append(s.update())
print("Time for {0}: {1}".format(s.sensor, round((time() - tmp)*1000, ndigits = 2)))
mrs = []
for b in self.behaviors:
tmp = time()
mrs.append(b.update())
print("Time for {0}: {1}".format(b, round((time() - tmp)*1000, ndigits = 2)))
#tmp = time()
#mrs = [b.update() for b in self.behaviors]
print(sorted(mrs, key=lambda x: x[1]))
action = self.arbitrator.choose_action(mrs)
print(action)
self.move(action)
execution_time = time() - start_time
#delay = max(delay - execution_time, 0)
delay = (delay - execution_time) if execution_time < delay else 0
print("Delay time: ", delay, "\n")
sleep(delay)
def __init__(self):
self.behaviors = [] #Liste over alle behaviors, aktive og inaktive
self.active_behaviors = [] # Kun aktive behaviors
self.sensobs = [] # Liste over sensorer
self.motobs = Motob(self) # Liste med motorobjekter
self.arbitrator = Arbitrator() # Arbitrator-objektet, velger winning behavior
self.num_timesteps = 0 # Hvor mange timesteps som er kjort
self.can_take_photo = False
def __init__(self):
self.running = True
self.behaviours = []
self.active_behaviours = []
self.inactive_behaviours = []
self.sensobs = []
self.motob = Motob()
self.arbitrator = Arbitrator(self)
self.setup()
class BBCON:
def __init__(self):
self.behaviors = []
self.sensobs = []
self.motob = Motob()
self.arbitrator = Arbitrator()
def add_behavior(self, behavior):
self.behaviors.append(behavior)
def add_sensob(self, sensob):
self.sensobs.append(sensob)
#def activate_behavior(self, behavior):
# self.active_behaviors.add(behavior)
#def deactive_behavior(self, behavior):
# self.active_behaviors.remove(behavior)
def move(self, action):
self.motob.update(action)
def run_one_timestep(self, delay = 0.667):
start_time = time()
# Reflectance sensor index 0
# Proximity sensor index 1
# Camera sensor index 2
print("Updating sensors...", end = "")
#self.data = [s.update() for s in self.sensobs]
self.data = []
for s in self.sensobs:
tmp = time()
self.data.append(s.update())
print("Time for:", s.sensor, ": ", round((time()-tmp)*1000, ndigits = 4), "ms")
print("Updating behaviors...")
tmp = time()
mrs = [b.update() for b in self.behaviors]
print((time()-tmp)*1000, "ms")
action = self.arbitrator.choose_action(mrs)
print("Moving.")
self.move(action)
execution_time = time() - start_time
#delay = max(delay - execution_time, 0)
delay = (delay - execution_time) if execution_time < delay else 0
print("Delay time: ", delay)
sleep(delay)
def __init__(self):
self.behaviors = [
] # behavior-listen, med både inaktive og aktive behaviors
self.active_behaviors = [] # liste med aktive behaviors
self.sensobs = [] # liste med sensor-objekter
self.motobs = Motob(self) # list med motor-objekter
self.arbitrator = Arbitrator(
) # arbitrator-objektet, velger winning-behavior
self.num_timesteps = 0 # antall timesteps som er kjørt
self.can_take_photo = False
def __init__(self):
self.__behaviors = [] # List of all behaviors
self.active_behaviors = [] # List of all _active_ behaviors
self.__sensobs = [] # List of all sensory objects
self.motob = Motob() # List of all motor objects
self.__arbitrator = Arbitrator(
self) # Arbitrator chooses the next behavior
self.timesteps = 0
self.notifications = []
def __init__(self):
self.behaviors = [
] #En liste av alle behavior objektene som brukes av BBCON
self.active_behaviors = [] #En liste med de aktive behavior objektene
self.sensobs = [
] #En liste av alle sensorobjektene som brukes av BBCON
self.motobs = Motob(
) #En liste ac alle motor objektene som brukes av BBCON
self.arbitrator = Arbitrator(
self, False) #Arbitratoren som skal løse requests fra behaviors
self.take_pic = False
class BBCON:
def __init__(self):
""" init """
self.sensobs = []
self.add_sensob(Sensob(Ultrasonic()))
self.add_sensob(Sensob(ReflectanceSensors()))
self.add_sensob(Sensob(Camera()))
self.motob = Motob()
self.behaviors = []
self.add_behavior(Behavior(self, [10000, 10000, [10000, 10000, 10000]], "drive", 1))
self.add_behavior(Behavior(self, [30, 10000, [10000, 10000, 10000]], "stop", 3))
self.add_behavior(Behavior(self, [10000, 0.3, [10000, 10000, 10000]], "turnaround", 2))
self.add_behavior(Behavior(self, [10000, 10000, [210, 10000, 10000]], "turn_left", 5))
#self.add_behavior(Behavior(self, [10000, 10000, [10000, 200, 10000]], "turn_right", 4))
self.active_behaviors = []
self.arbitrator = Arbitrator()
def add_behavior(self, behavior):
"""append a newly-created behavior onto the behaviors list"""
self.behaviors.append(behavior)
def add_sensob(self, sensor):
"""append a newly-created sensob onto the sensobs list"""
self.sensobs.append(sensor)
def activate_behavior(self, behavior):
"""add an existing behavior onto the active-behaviors list"""
self.active_behaviors.append(behavior)
def deactivate_behavior(self, behavior):
"""remove an existing behavior from the active behaviors list"""
self.active_behaviors.remove(behavior)
def run_one_timestep(self):
"""constitutes the core BBCON activity"""
prod_count = 0
for sensob in self.sensobs: #Updates all sensobs
sensob.update()
if(prod_count == 2):
image = Imager(False, sensob.get_value())
print("Camera pixel", image.get_pixel(20, 30))
prod_count += 1
for behavior in self.behaviors: #Update all behaviors
behavior.update(self.sensobs)
fav_behavior = self.arbitrator.choose_action(self.active_behaviors)
self.motob.update(fav_behavior.sense_and_act())
def __init__(self):
# Initialize arbitrator
self.arbitrator = Arbitrator()
# Initialize motobs (single object for both motors on the Zumo)
self.motobs.append(Motob(Motors()))
# Initialize sensors
self.sensors = {
'ultrasonic': Ultrasonic(0.05),
'IR': IRProximitySensor(),
'reflectance': ReflectanceSensors(False, 0, 900),
'camera': Camera(),
}
self.active_sensors = [self.sensors['ultrasonic'], self.sensors['IR'], self.sensors['reflectance']]
# Initialize sensobs
self.sensobs = {
'distance': DistanceSensob([self.sensors['ultrasonic']]),
'line_pos': LinePosSensob([self.sensors['reflectance']]),
'proximity': ProximitySensob([self.sensors['IR']]),
'red_search': RedSearchSensob([self.sensors['camera']]),
}
self.active_sensobs = [self.sensobs['distance'], self.sensobs['line_pos'], self.sensobs['proximity']]
time.sleep(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 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 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 __init__(self):
self.behaviours = [] #list of all behavior objects (BHRs)
self.active_behaviours = [] #a list of all currently-active BHRs.
self.sensobs = [] #a list of all sensory objects
self.motobs = [Motob()] #a list of all motor objects
self.arb = Arbitrator(
) #this resolves motor requests produced by the behaviors.
def __init__(self,
behaviors=None,
active_behaviors=None,
sensobs=None,
motobs=Motob(),
arbitrator=Arbitrator()):
# Make arguments non-mutable (because PyCharm says so)
if sensobs is None:
sensobs = []
if active_behaviors is None:
active_behaviors = []
if behaviors is None:
behaviors = []
# Checks if bbcon pointer is initialized
if arbitrator.bbcon is None:
arbitrator.bbcon = self
# Save arguments
self.behaviors = behaviors
self.active_behaviors = active_behaviors
self.sensobs = sensobs
self.motobs = motobs
self.arbitrator = arbitrator
# Save halt flag
self.halt_request = False
def trackTest():
ZumoButton().wait_for_press()
motor = Motors()
ultra = Ultrasonic()
camera = Camera()
stuck = CheckStuck()
motob = Motob(motor)
arbitrator = Arbitrator(motob=motob)
sensob = Sensob()
sensob.set_sensors([ultra, camera])
bbcon = BBCON(arbitrator=arbitrator, motob=motob)
b = TrackObject(priority=1, sensobs=[sensob])
bbcon.set_checkStucker(stuck)
bbcon.add_behavior(b)
bbcon.activate_behavior(0)
bbcon.add_sensob(sensob)
timesteps = 0
while timesteps < 25:
bbcon.run_one_timestep()
timesteps += 1
def __init__(self):
self.behaviors = []
self.sensobs = []
self.motob = Motob()
self.arbitrator = Arbitrator()
def __init__(self):
"""Initializes BBCON variables"""
self.behaviors = []
self.active_behaviors = []
self.sensobs = []
self.motobs = [Motob()]
self.arbitrator = Arbitrator(self)
self.new_picture = False
class BBCON:
def __init__(self):
self.behaviors = []
self.active_behaviors = []
self.sensobs = []
self.motob = Motob()
self.arbitrator = Arbitrator(self)
self.picture_taken = False
self.reset = False
def get_active_behaviors(self):
return self.active_behaviors
def add_behavior(self, behavior):
if behavior not in self.behaviors:
self.behaviors.append(behavior)
def add_sensob(self, sensob):
if sensob not in self.sensobs:
self.sensobs.append(sensob)
def activate_behavior(self, behavior):
if behavior not in self.active_behaviors:
self.active_behaviors.append(behavior)
def deactivate_behavior(self, behavior):
if behavior in self.active_behaviors:
self.active_behaviors.remove(behavior)
def is_picture_taken(self):
return self.picture_taken
def reset_sensobs(self):
for sensob in self.sensobs:
sensob.reset()
def run_one_timestep(self):
for sensob in self.sensobs:
sensob.update()
for behavior in self.behaviors:
behavior.update()
motor_recommendations = self.arbitrator.choose_action()
self.motob.update(motor_recommendations)
if self.reset:
self.reset_sensobs()
sleep(0.1)
class BBCON:
def __init__(self):
self.behaviours = [] # liste med alle oppførslene til bbcon
self.active_behaviours = [] # liste med alle oppførsler som er aktive
self.sensobs = [] # liste med alle sensorobejekt brukt av bbcon
self.motobs = Motob() # liste med alle motorobjekt brukt av bbcon
self.arbitrator = Arbitrator() # ikke laget ennå
# legger til en oppførsel
def add_behaviour(self, behaviour):
if behaviour not in self.behaviours:
self.behaviours.append(behaviour)
# legger til en sensor
def add_sensob(self, sensob):
if sensob not in self.sensobs:
self.sensobs.append(sensob)
# legger til en oppførsel i aktivert-listen
def activate_behaviour(self, behaviour):
if behaviour in self.behaviours:
self.active_behaviours.append(behaviour)
# fjerner en aktiv oppførsel fra listen
def deactivate_behaviour(self, behaviour):
if behaviour in self.active_behaviours:
self.active_behaviours.remove(behaviour)
# kjerneaktiviteten til bbcon
def run_one_timestep(self):
# oppdaterer oppførsler som også oppdaterer sensobs
for behaviour in self.behaviours:
behaviour.update()
# returnerer
print("Active behaviours: ", self.active_behaviours)
motor_recoms = self.arbitrator.choose_action(self.active_behaviours)
# oppdaterer motobs
self.motobs.update(motor_recoms)
# reseter sensorverdier
for sensor in self.sensobs:
sensor.reset()
def __init__(self):
cam = CamUltra()
ref = ReflectanceSob()
self.sensobs = [cam, ref]
self.active_behaviours = [AttackRed(cam), BeScared(cam), StayInMap(ref), Wander()]
self.motobs = [Motob(self)]
self.arbitrator = Arbitrator(self)
#self.active_behaviours = []
self.inactive_behaviours = []
def __init__(self):
""" init """
self.sensobs = []
self.add_sensob(Sensob(Ultrasonic()))
self.add_sensob(Sensob(ReflectanceSensors()))
self.add_sensob(Sensob(Camera()))
self.motob = Motob()
self.behaviors = []
self.add_behavior(Behavior(self, [10000, 10000, [10000, 10000, 10000]], "drive", 1))
self.add_behavior(Behavior(self, [30, 10000, [10000, 10000, 10000]], "stop", 3))
self.add_behavior(Behavior(self, [10000, 0.3, [10000, 10000, 10000]], "turnaround", 2))
self.add_behavior(Behavior(self, [10000, 10000, [210, 10000, 10000]], "turn_left", 5))
#self.add_behavior(Behavior(self, [10000, 10000, [10000, 200, 10000]], "turn_right", 4))
self.active_behaviors = []
self.arbitrator = Arbitrator()
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)
class BBCON:
def __init__(self):
self.behaviors = []
self.active_behaviors = []
self.sensobs = []
self.motob = Motob()
self.arbitrator = Arbitrator(self)
self.picture_taken = False
def get_active_behaviors(self):
return self.active_behaviors
def add_behavior(self, behavior):
if behavior not in self.behaviors:
self.behaviors.append(behavior)
def add_sensob(self, sensob):
if sensob not in self.sensobs:
self.sensobs.append(sensob)
def activate_behavior(self, behavior):
if behavior not in self.active_behaviors:
self.active_behaviors.append(behavior)
def deactivate_behavior(self, behavior):
if behavior in self.active_behaviors:
self.active_behaviors.remove(behavior)
def is_picture_taken(self):
return self.picture_taken
def run_one_timestep(self):
print("RUN ONE TIME STEP")
for sensob in self.sensobs:
sensob.update()
print("SENSOBS FINISHED")
for behavior in self.behaviors:
behavior.update()
print("BEHAVIOUR FINISHED")
#print("%s weight: %s" %(behavior.get_name(),behavior.get_weight()))
motor_recommendations = self.arbitrator.choose_action()
#print("Recommendations: %s",(motor_recommendations))
self.motob.update(motor_recommendations)
sleep(0.5)
def __init__(self):
self.ARB = ARB(self)
self.recommendations = []
self.motob = Motob()
self.behaviors = set() # all behaviors
self.active = set() # active behaviors
self.inactive = set() # inactive behaviors
self.sensobs = set()
self.motobs = [self.motob] # motor object(s)
def test5():
ZumoButton().wait_for_press()
m = Motors()
motob = Motob(m)
sensor = Ultrasonic()
sensob = Sensob()
sensob.set_sensors([sensor])
behavior = CollisionAvoidance(1, [sensob])
print("Behavior sensob:", behavior.sensobs)
count = 0
while True:
sensob.update()
behavior.update()
#print("MR:", behavior.get_sensob_data())
motob.update(behavior.motor_recommendations[0])
count +=1
if count==12:
break
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
def main():
zumo_button = ZumoButton() # Sets up pins and Zumobutton
zumo_button.wait_for_press()
bbcon1 = BBCON() # Create BBCON
#Motors
motor1 = Motors() # Create a Motor
motob1 = Motob(motor1) # Create a motob
bbcon1.motobs = [motob1] # Give Motor to BBCON
# Collision avoidance
ultra_sensor = ObstacleDetectionSensob() # Create obstacle sensob
avoid_object = AvoidObstacleBehavior(bbcon1, 1.5) # Create obstacle Behavior
ultra_sensor.add_behavior(avoid_object) # Give sensob the behavior
bbcon1.add_behavior(avoid_object) # Give BBCON the behavior
bbcon1.add_sensob(ultra_sensor) # Give BBCON the sensor
# Line follow
line_sensor = IRSensob() # Create IR sensob
line_follow = FollowLineBehavior(bbcon1, 0.5) # Create linefollow behavior
line_follow.add_sensob(line_sensor) # Give linefollow sin sensob
bbcon1.add_behavior(line_follow) # Give BBCON the linefollow
bbcon1.add_sensob(line_sensor) # Give BBCON the IR sensob
# Add setup for camera, and add it to BBCON when we want to test everything together
camera_sensor = FindRedSensob() # Create obstacle sensob
find_and_follow_behavior = FindAndFollowRedBallBehavior(bbcon1, 1) # Create obstacle Behavior
camera_sensor.add_behavior(find_and_follow_behavior) # Give sensob the behavior
bbcon1.add_behavior(find_and_follow_behavior) # Give BBCON the behavior
bbcon1.add_sensob(camera_sensor)
motor1.stop() # Stop all motors
print("\nAll creation is done, entering main loop\n")
q = input("Press 'q' to quit: ")
while q is not 'q':
#Runs 100 timesteps. Stops if q is pressed.
for i in range(0, 100):
print("\nIteration " + str(i))
if bbcon1.run_one_timestep():
motor1.stop()
exit()
motor1.stop()
q = input("Press 'q' to quit: ")
def main():
wp.wiringPiSetupGpio()
motors = Motors()
motors.stop()
motob = Motob(motors)
sensors = {
#'ultrasonic': Ultrasonic(0.05),
#'IR': IRProximitySensor(),
#'reflectance': ReflectanceSensors(False, 0, 900),
#'camera': Camera(),
}
# Initialize sensobs
sensobs = {
#'distance': DistanceSensob([sensors['ultrasonic']]),
#'line_pos': LinePosSensob([sensors['reflectance']]),
#'proximity': ProximitySensob([sensors['IR']]),
#'red_search': RedSearchSensob([sensors['camera']]),
}
commands = [(Command.F, 0.5), (Command.R, 0.5), (Command.F, 1)]
i=0
while True:
for sensor in sensors.values():
sensor.update()
for sensob in sensobs.values():
sensob.update()
motob.update(commands[i%len(commands)])
time.sleep(1)
i+=1
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 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)
