def __init__(self):
# Set channels to the number of servo channels on your kit.
# 8 for FeatherWing, 16 for Shield/HAT/Bonnet.
self.kit = ServoKit(channels=16)
logging.debug('Setting channel to 16')
self.z = 0
logging.debug('Set to 90 degree')
time.sleep(5)
self.x = 0
self.y = 0
self.xValue = 0
self.yValue = 0
self.rotateFistElbowAngle = 0
self.rotateAngle = 0
self.xComplete = False
self.yComplete = False
self.camera = Camera()
self.color = "orange"
self.robot = Robot()
self.ultrasonic = Ultrasonic()
self.Y_limit = {"Lower": 360, "Upper": 363}
self.X_limit = {"Lower": 314, "Upper": 316}
self.dictColor = {"orange": (114, 61, 27), "green": (50, 168, 82)}
# for the api
self.color_coordinates = {}
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 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 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 __init__(self,
reverse_mot1=False,
reverse_mot2=False,
fix_rotate=False,
derivative_fix=0):
""" Initialize the DualHBridge L293D. Allows you to reverse/tune the
spinning commands for the motors when the underlaying plateform does not
move proprely when calling forward() or backward()
:param reverse_mot1: switch the forward/backward commands for motor 1
:param reverse_mot2: switch the forward/backward commands for motor 2
:param fix_rotate: set this True when the robot turn left instead of turning right as requested
:param derivative_fix: use +3 to speedup right motor when forward() does bend the path to the right.
use -3 to slow down the right motor when forward() bend the path to the left."""
mot1 = (self.MOT1_PINS[1],
self.MOT1_PINS[0]) if reverse_mot1 else self.MOT1_PINS
mot2 = (self.MOT2_PINS[1],
self.MOT2_PINS[0]) if reverse_mot2 else self.MOT2_PINS
if not fix_rotate:
super(MotorSkin, self).__init__(mot1, self.MOT1_PWM, mot2,
self.MOT2_PWM, derivative_fix)
else:
# Robot is apparently rotating the wrong way... this is because
# motor1 has been wired in place of the motor 2.
super(MotorSkin, self).__init__(mot2, self.MOT2_PWM, mot1,
self.MOT1_PWM, derivative_fix)
self.ultrason = Ultrasonic(self.TRIGGER_PIN, self.ECHO_PIN)
for pin in ('X18', 'X19', 'X20', 'X21'):
self._switches.append(
pyb.Pin(pin, pyb.Pin.IN, pull=pyb.Pin.PULL_UP))
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 main(argv):
for sig in (SIGABRT, SIGILL, SIGINT, SIGSEGV, SIGTERM):
signal(sig, clean)
# --- IP of esp
esp_ip = "129.69.209.119" #placeholder
broker_port = 1883
#Read other measure interval from parameter data
paramArray = json.loads(argv[0])
for param in paramArray:
if not ('name' in param and 'value' in param):
continue
elif param["name"] == "esp_ip":
esp_ip = param["value"]
configFileName = "connections.txt"
topics = []
brokerIps = []
configExists = False
configFile = os.path.join(os.getcwd(), configFileName)
while (not configExists):
configExists = os.path.exists(configFile)
time.sleep(1)
# BEGIN parsing file
fileObject = open(configFile)
fileLines = fileObject.readlines()
fileObject.close()
for line in fileLines:
pars = line.split('=')
topic = pars[0].strip('\n').strip()
ip = pars[1].strip('\n').strip()
topics.append(topic)
brokerIps.append(ip)
# END parsing file
hostname = brokerIps[0]
topic_pub = topics[0]
topic_splitted = topic_pub.split('/')
component = topic_splitted[0]
component_id = topic_splitted[1]
# --- Create ESP12e instance
global esp
esp = ESP12e_Push(esp_ip, hostname, broker_port, topic_pub)
esp.start()
# --- Create Ultrasonic sensor instance
ultrasonic = Ultrasonic(hostname, broker_port, topic)
while ultrasonic.getLastValue() == None:
ultrasonic.updateLastValue()
time.sleep(1)
while True:
time.sleep(1)
def __init__(self):
super().__init__("Distance Error")
self.setPollingPeriod(1)
self._meter = Ultrasonic(DistanceErrorApp.GPIO_TRIGGER,
DistanceErrorApp.GPIO_ECHO)
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 sensorTest():
ZumoButton().wait_for_press()
sensor = Ultrasonic()
count = 0
while count < 5:
sensor.update()
print(sensor.get_value())
count += 1
def distance():
us = Ultrasonic()
button = ZumoButton()
while True:
button.wait_for_press()
us.update()
reading = us.get_value()
print(reading)
time.sleep(0.6)
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 get_distance_value(self):
"""
Shortcut for getting distance
:return: Distance
"""
ultra = Ultrasonic()
ultra.update()
value = ultra.get_value()
return value
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 explorer(dist=10):
m = Motors()
u = Ultrasonic()
while u.update() > dist:
m.forward(0.2, 0.2)
m.backward(.1, 0.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 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 calculateFront(self):
u_sensor = Ultrasonic()
u_sensor.update()
distance = u_sensor.get_value()
if distance < 5:
self.pri_value = 1000
else:
self.pri_value = 0
return self.pri_value
def run():
global ultrasonic1, ultrasonic2, ultrasonic3, motors
ultrasonic1 = Ultrasonic(TRIG1, ECHO1)
ultrasonic2 = Ultrasonic(TRIG2, ECHO2)
ultrasonic3 = Ultrasonic(TRIG3, ECHO3)
motors = Motor(MOTOR1A, MOTOR1B, MOTOR2A, MOTOR2B)
rospy.init_node('controller', anonymous=True)
distance_publisher = rospy.Publisher('distance', Distance, queue_size=10)
rospy.Subscriber('direction', Direction, steer)
rospy.on_shutdown(exit_gracefully)
rate = rospy.Rate(5)
while not rospy.is_shutdown():
distance1, distance2, distance3 = scan_distance()
distance_msg = Distance()
distance_msg.distance1 = distance1
distance_msg.distance2 = distance2
distance_msg.distance3 = distance3
rospy.loginfo(distance_msg)
distance_publisher.publish(distance_msg)
rate.sleep()
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 main():
GPIO_TRIGGER = 66 #P8.7
GPIO_ECHO = 67 #P8.8
PWM_PORT = 6 #P8.13
LPF_A = 0.5
MIN_DIST = 50.0
MAX_DIST = 150.0
DIST_RANGE = MAX_DIST - MIN_DIST
MIN_FREQ = 220
MAX_FREQ = 880
FREQ_RANGE = MAX_FREQ - MIN_FREQ
try:
print("Press Ctrl+C to finish")
meter = Ultrasonic(GPIO_TRIGGER, GPIO_ECHO)
buzz = Buzzer(6)
dist = meter.read()
time.sleep(0.5)
while True:
distRead = meter.read()
if distRead != Ultrasonic.OUT_OF_RANGE:
dist += (distRead - dist) * LPF_A
print("~ {0:.3f} cm".format(dist))
if dist > MIN_DIST and dist < MAX_DIST:
freq = (((dist - MIN_DIST) / DIST_RANGE) *
FREQ_RANGE) + MIN_FREQ
buzz.playNote(freq, 0.5)
else:
time.sleep(0.5)
else:
print("Out of range!")
time.sleep(0.5)
except KeyboardInterrupt:
print("\nCtrl+C pressed.")
finally:
print("Bye!")
meter.cleanup()
buzz.cleanup()
def loop():
global lcd
lcd = LCD()
us = Ultrasonic(GPIO_TRIGGER, GPIO_ECHO)
while True:
distance = us.distance()
if distance:
lcd.message('distance:\n ')
print('distance:%0.2fcm' % distance)
#lcd.clear()
lcd.message('distance:\n%0.2fcm' % distance)
else:
print('Error')
time.sleep(0.2)
def my():
wp.wiringPiSetupGpio()
wp.wiringPiSetup()
uls = Ultrasonic(20, 21)
tesla = Tesla_motor(23, 18, 25, 24)
tesla.allStop()
while 1:
distance = uls.distance()
print(distance)
if distance > 30:
tesla.forwardDrive()
#sleep(5)
else:
tesla.allStop()
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 explorer(dist=10):
ZumoButton().wait_for_press()
m = Motors()
u = Ultrasonic()
print(u.sensor_get_value())
while u.sensor_get_value() > dist:
m.forward(.2, 0.2)
print(u.value)
m.backward(.1, .5)
m.left(.5,3)
m.right(.5, 3.5)
sleep(2)
while u.sensor_get_value() < dist*5:
m.backward(.2, 0.2)
m.left(.75, 5)
def get_sensor_values(self, sensor_class):
"""
Helper function to get values afflicted with a sensor
:param sensor_class: Class of the sensor
:return: Values or None
"""
ultra = Ultrasonic()
ultra.update()
sensor = self.get_sensor(sensor_class)
if sensor is not None:
return self.sensors[sensor]
else:
return None
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 main():
M = Motors()
sensor1 = IRProximitySensor()
sensor2 = Ultrasonic()
sensor3 = Camera()
sensorsCamera = list()
sensorsCamera.append(sensor2)
sensorsCamera.append(sensor3)
sensorsDistance = list()
sensorsDistance.append(sensor2)
sensorsSides = list()
sensorsSides.append(sensor1)
sensobCamera = sensob(sensorsCamera, 3)
sensobDistance = sensob(sensorsDistance, 2)
sensobSides = sensob(sensorsSides, 1)
sensobs = list()
sensobs.append(sensobCamera)
sensobs.append(sensobDistance)
sensobs.append(sensobSides)
behaviors = []
behavior1 = Behavior(sensobSides, 1)
behavior2 = Behavior(sensobDistance, 2)
behavior3 = Behavior(sensobCamera, 3)
active_behaviors = list()
active_behaviors.append(behavior1)
active_behaviors.append(behavior2)
behaviors.append(behavior1)
behaviors.append(behavior2)
behaviors.append(behavior3)
imotobo = Imotob("Straight", "Straight", M)
#motobs = list()
#motobs.append(imotobo)
arbitrator = Arbitrator(behaviors)
bbcon = BBCON(behaviors, active_behaviors, sensobs, imotobo, arbitrator)
ZumoButton().wait_for_press()
i = 0
while (i < 8):
bbcon.run_one_timestep()
i += 1
