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 main3():
button = ZumoButton()
distance_ob = Distanceob()
camera_ob = Cameraob()
ir_prox_ob = IRProximityob()
count = 0
bbcon = BBCON()
behavior_stop_red = StopRed(bbcon=bbcon, sensobs=[distance_ob, camera_ob])
behavior_follow_side = FollowSide(bbcon=bbcon, sensobs=[ir_prox_ob])
behavior_drive = DriveAround(bbcon=bbcon)
bbcon.add_behavior(behavior_stop_red)
bbcon.add_behavior(behavior_drive)
bbcon.add_behavior(behavior_follow_side)
bbcon.add_sensob(distance_ob)
bbcon.add_sensob(camera_ob)
bbcon.add_sensob(ir_prox_ob)
bbcon.activate_behavior(behavior_follow_side)
bbcon.activate_behavior(behavior_stop_red)
bbcon.activate_behavior(behavior_drive)
button.wait_for_press()
while count < 100:
bbcon.run_one_timestep()
count += 1
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 runOneTimestep(self):
print("Starts")
button = ZumoButton()
button.wait_for_press()
self.addSenob()
self.addBehavior()
pictureTime = 0
while True:
#print("Runns itteration")
#Reads from active sensors
activeSensors = []
#Adds imageBehaviour
if pictureTime > 3:
self.active_behaviors.append(self.behaviors[3])
for behavior in self.active_behaviors:
sensorList = behavior.getSensobs()
for sensor in sensorList:
if not activeSensors.__contains__(sensor):
activeSensors.append(sensor)
#print("Adds to active sensors")
for sensor in activeSensors:
#print("Adds sensor")
if (sensor != None):
sensor.update()
#print("Update all behaviors")
#Update all behaviors
for behavior in self.active_behaviors:
behavior.updateRecomendation()
#print("About to chose action")
#Requesting the motor recomendation and halt request flag
behavior, requestFlag = self.abitrator.choose_action()
#print("Action:", behavior.getMovement())
if requestFlag:
break
#Update the motobs so that the motors are activated/deactivated
self.motobs.recomend(behavior.getMovement())
#print("Recomended movement")
self.motobs.update()
#print("Uppdated motors")
if self.active_behaviors.__contains__(self.behaviors[3]):
self.active_behaviors.remove(self.behaviors[3])
pictureTime = 0
#Wait. Defines how long the code should wait
sleep(0.05)
pictureTime = pictureTime + 1
#Resets sensors
for sensor in activeSensors:
if sensor != None:
sensor.reset()
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 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 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 test1():
ZumoButton().wait_for_press()
m = Motors()
m.turn_right(90)
m.turn_left(180)
m.turn_right(360)
m.turn_left(360)
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 crashTest():
ZumoButton().wait_for_press()
sensor = Crash_sensor()
motor = Motors()
counter = 0
f_value = sensor.calculateFront()
ir_command = sensor.calculateSides()
while True:
counter += 1
if counter >= 5:
f_value = sensor.calculateFront()
ir_command = sensor.calculateSides()
counter = 0
if ir_command == "LEFT":
motor.left(0.5, 0.1)
elif ir_command == "RIGHT":
motor.right(0.5, 0.1)
elif ir_command == "BACKWARD":
motor.backward(0.3, 0.5)
else:
motor.forward(0.3, 0.01)
if f_value == 1000:
motor.stop()
break
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 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 vsvingsakte():
speed = 0.5
ZumoButton().wait_for_press()
motors = Motors()
motors.set_value((speed / 2, speed / 2), 1)
motors.set_value((speed, -speed), 0.1)
motors.set_value((speed / 2, speed / 2), 1)
def vsvingrask():
speed = 0.5
ZumoButton().wait_for_press()
motors = Motors()
motors.set_value((speed / 2, speed / 2), 1)
motors.set_value((0.3, -0.3), 1)
motors.set_value((speed / 2, speed / 2), 1)
def camera():
camera_sensob = CameraSensob(None, color=0)
for i in range(0, 15):
ZumoButton().wait_for_press()
print('\nPicture:')
image = camera_sensob.update()
image.save('image{}.jpg'.format(i))
def camTest():
ZumoButton().wait_for_press()
sensor = Camera(img_width=128, img_height=96, img_rot=0) #endre disse?
sensor2 = Camera(img_width=256, img_height=192, img_rot=0)
sensor3 = Camera(img_width=512, img_height=384, img_rot=0)
sensor4 = Camera(img_width=1024, img_height=768, img_rot=0)
sensor.update()
sensor2.update()
sensor3.update()
sensor4.update()
pic = sensor.get_value()
pic2 = sensor2.get_value()
pic3 = sensor3.get_value()
pic4 = sensor4.get_value()
b = Imager()
b.image = pic
b.dump_image("test", type="JPEG") #dump as jpeg/jpg/gif?
b.image = pic2
b.dump_image("test2", type="JPEG")
b.image = pic3
b.dump_image("test3", type="JPEG")
b.image = pic4
b.dump_image("test4", type="JPEG")
def sensorTest():
ZumoButton().wait_for_press()
sensor = Ultrasonic()
count = 0
while count < 5:
sensor.update()
print(sensor.get_value())
count += 1
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 test45():
ZumoButton().wait_for_press()
sensor = IRProximitySensor()
count = 0
while count < 10:
sensor.update()
print(sensor.get_value())
count += 1
time.sleep(2.5)
def dancer():
ZumoButton().wait_for_press()
m = Motors()
m.forward(.2, 3)
m.forward(.3, 2)
m.right(.5, 3)
m.left(.5, 3)
m.backward(.3, 2.5)
m.set_value([.5, .1], 10)
m.set_value([-.5, -.1], 10)
def dancer():
ZumoButton().wait_for_press()
m = Motors()
m.forward(0.2, 3)
m.backward(0.2, 3)
m.right(0.5, 3)
m.left(0.5, 3)
m.backward(0.3, 2.5)
m.set_value([0.5, 0.1], 10)
m.set_value([-0.5, -0.1], 10)
def calc():
m = Motors()
while True:
ZumoButton().wait_for_press()
sleep(1)
m.set_value([-1, 1], 90 * 0.00228)
sleep(1)
m.set_value([-1, 1], 90 * 0.00228)
sleep(1)
m.set_value([-1, 1], 90 * 0.00228)
sleep(1)
m.set_value([-1, 1], 90 * 0.00228)
def explorer(dist=10):
ZumoButton().wait_for_press()
m = Motors(); u = Ultrasonic()
while u.update() > dist:
m.forward(.2,0.2)
m.backward(.1,.5)
m.left(.5,3)
m.right(.5,3.5)
sleep(2)
while u.update() < dist*5:
m.backward(.2,0.2)
m.left(.75,5)
def run_robot():
ZumoButton().wait_for_press()
bbcon = BBCON()
init_bbcon(bbcon)
i = 0
while True:
bbcon.run_one_timestep()
print(i)
i += 1
def main():
button = ZumoButton()
ir_prox_ob = IRProximityob()
count = 0
bbcon = BBCON()
behavior_follow_side = FollowSide(bbcon=bbcon, sensobs=[ir_prox_ob])
behavior_drive = DriveAround(bbcon=bbcon)
bbcon.add_behavior(behavior_follow_side)
bbcon.add_behavior(behavior_drive)
bbcon.add_sensob(ir_prox_ob)
bbcon.activate_behavior(behavior_follow_side)
bbcon.activate_behavior(behavior_drive)
button.wait_for_press()
while count < 100:
bbcon.run_one_timestep()
count += 1
def test():
ZumoButton().wait_for_press()
ultra = Ultrasonic()
m = Motors()
ultra.update()
tall = ultra.get_value()
print("tall: ", tall)
while tall < 5.0:
m.backward(0.2, 1)
print(tall)
ultra.update()
tall = ultra.get_value()
print(tall)
def drive():
ZumoButton().wait_for_press()
m = Motors()
dir = ""
while dir != "stop":
dir = input(">")
if dir == "w":
m.forward(speed=1, dur=1)
elif dir == "s":
m.backward(speed=1, dur=1)
elif dir == "d":
m.right(speed=1, dur=5)
elif dir == "a":
m.left(speed=1, dur=5)
def followTest():
ZumoButton().wait_for_press()
m = Motors()
follow = FollowLine()
time = 0
while (time < 100):
values = follow.isOnLine()
if values[0] == 0:
m.forward(0.2, 0.2)
elif values[1] == 0 or values[1] == 1:
m.left(0.2, 0.3)
else:
m.right(0.2, 0.3)
time += 1
def dancer():
ZumoButton().wait_for_press()
m = Motors()
u = Ultrasonic()
print(u.getvalue())
m.forward(.2, 3)
m.backward(.2, 3)
m.right(.5, 3)
m.left(.5, 3)
m.backward(.3, 2.5)
m.set_value([.5, .1], 10)
m.set_value([-.5, -.1], 10)
def main():
bbcon = BBCON()
follow_line = FollowLine(bbcon)
obstruction = Obstruction(bbcon)
bbcon.add_behaviour(follow_line)
bbcon.add_behaviour(obstruction)
print("before button")
ZumoButton().wait_for_press()
print("after button")
while True:
bbcon.run_one_timestep()
