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()