Scribd
Upload
6 views3 pages

Apaga Velas Final

The document contains code for controlling a robot to navigate hallways and rooms to find and extinguish a candle. It uses sensors to follow walls and avoid obstacles, and a camera to locate and center on the candle before using fans to blow it out. It repeats this process until the candle has been extinguished.

Uploaded by

Raul Castellano Costa
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
6 views3 pages

Apaga Velas Final

The document contains code for controlling a robot to navigate hallways and rooms to find and extinguish a candle. It uses sensors to follow walls and avoid obstacles, and a camera to locate and center on the candle before using fans to blow it out. It repeats this process until the candle has been extinguished.

Uploaded by

Raul Castellano Costa
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 3

#!

/usr/bin/python
# -*- coding: utf-8 -*-
from Robot import *

from picamera.array import PiRGBArray


from picamera import PiCamera
import time
import cv2
from candle import candle

import numpy
import random

def rutinaPasillo(robot, hardSp, softSp):


numItIzq = 0
numItDer = 0
while True:
print "Empiezo una iteracion"
(lV, rV) = robot.distance()
izquierda = numpy.interp(lV, Si, Dcm)
derecha = numpy.interp(rV, Sd, Dcm)
if robot.light() < 460:
robot.motors(softSp, softSp)
time.sleep(0.5)
print "soy la luz"
break
if robot.touch() == 0:
if numItDer > 0.02 or numItIzq > 0.02:
robot.motors(softSp,-softSp)
time.sleep(0.5)
numItDer = 0
numItIzq = 0
if izquierda > derecha:
numItIzq = 0
numItDer += t
robot.motors(softSp, hardSp)
elif derecha > izquierda:
numItDer = 0
numItIzq += t
robot.motors(hardSp, softSp)
else:# Choca
print "tocado y hundido"
robot.motors(0, 0)
time.sleep(0.2)
robot.motors(-softSp, -softSp)
time.sleep(0.3)
robot.motors(hardSp, -hardSp)
time.sleep(0.5)

Si = [
227,
250,
266,
300,
345,
445,
670,
740,
]
Sd = [
240,
255,
270,
300,
340,
435,
645,
725,
]

Dcm = [
28,
24,
20,
16,
12,
8,
4,
3,
]
v = [
7,
8,
9,
10,
11,
12,
6,
5,
13,
14,
]

robot = Robot(TOUCH_CONFIG)

camera = PiCamera()
stream = PiRGBArray(camera)
time.sleep(1.5)
# Variables editables
t = 0.000001
nGiros=0
hardSp = 100
softSp = 55
dentro=True
velaApagada = False
try:

while True:
if not robot.error:
rutinaPasillo(robot, hardSp, softSp)
rutinaPasillo(robot, hardSp, softSp)
if not velaApagada: # Entramos en una habitacion
robot.motors(softSp, softSp)
time.sleep(2)
converged = False
nGiros=0
while not converged and nGiros<0.9:
stream.truncate(0)
camera.capture(stream, format='bgr')
image = stream.array
c = candle(image)
if c is None and nGiros < 0.3:
robot.motors(40,-40)
time.sleep(0.1)
nGiros+=0.1
elif c is None and (nGiros >= 0.3 and nGiros <
0.9):
robot.motors(-40,40)
time.sleep(0.1)
nGiros+=0.1
else:
(x, y, w, h) = c
dx = x - 360
motor = dx * 500 / 360
if motor > 50:
motor = 50
if motor < -50:
motor = -50
print 'vueltas vueltas dubidu'
robot.motors(motor, -motor)
time.sleep(0.1)
robot.motors(0, 0)
converged = abs(motor) < 15
w = 0
while w < 160:
stream.truncate(0)
camera.capture(stream, format='bgr')
image = stream.array
c = candle(image)
if c is not None:
(x, y, w, h) = c
print w
robot.motors(softSp, softSp)
time.sleep(0.3)
robot.motors(0, 0)
else:
break
if w>0:
robot.motors(0,70)
time.sleep(0.1)

robot.fan(600)
robot.motors(0, 0)
time.sleep(4)
robot.fan(0)
robot.motors(0, 0)
robot.motors(-softSp, softSp)
time.sleep(2)
velaApagada=True
else:
robot.motors(hardSp, hardSp)
time.sleep(0.5)
robot.motors(0,0)
break
time.sleep(t)
else:
print 'Error!'
except KeyboardInterrupt:
robot.terminate()
camera.close()
robot.terminate()

You might also like