def __init__(self):
self.actuadorOP= ActuadorOpenPilot()
self.sensorMagnetometro = SensorMagnetometro(DriverMagnetometro())
self.sensorGiroscopio=SensorGiroscopio(DriverGiroscopio())
self.sensorBateria= SensorBateria()
self.alcanceUltrasonido=4000
self.sensorUltrasonido = SensorUltrasonido(DriverUltrasonido(), self.alcanceUltrasonido)
#self.sensorGPS=SensorGPS(DriverGPS())
# convierto la info del GPS a centimetros porque viene en metros
self.altitudSuelo=self.sensorGPS.getLastInfo().getData()['altitud']*100
# para yaw, pitch, roll
self.velocidadMaxGiroOP=50
self.velocidadCeroGiroOP=50
# para Throtle
self.velocidadEstable=64
self.velocidad10cm=30
self.velocidad5cm=20
self.velocidadMaxMotores=100
self.distanciaMinimaDelSuelo=1 # 30 centimetros
# angulo para avanzar
self.anguloAvance=5
# definicion de modos de vuelo
self.modosDeOperacion={'estabilizado':0, 'acrobatico':1} # esto hay que verificar
class ControladorDronMulticoptero(ControladorDronVolador):
def __init__(self):
self.actuadorOP= ActuadorOpenPilot()
self.sensorMagnetometro = SensorMagnetometro(DriverMagnetometro())
self.sensorGiroscopio=SensorGiroscopio(DriverGiroscopio())
self.sensorBateria= SensorBateria()
self.alcanceUltrasonido=4000
self.sensorUltrasonido = SensorUltrasonido(DriverUltrasonido(), self.alcanceUltrasonido)
#self.sensorGPS=SensorGPS(DriverGPS())
# convierto la info del GPS a centimetros porque viene en metros
self.altitudSuelo=self.sensorGPS.getLastInfo().getData()['altitud']*100
# para yaw, pitch, roll
self.velocidadMaxGiroOP=50
self.velocidadCeroGiroOP=50
# para Throtle
self.velocidadEstable=64
self.velocidad10cm=30
self.velocidad5cm=20
self.velocidadMaxMotores=100
self.distanciaMinimaDelSuelo=1 # 30 centimetros
# angulo para avanzar
self.anguloAvance=5
# definicion de modos de vuelo
self.modosDeOperacion={'estabilizado':0, 'acrobatico':1} # esto hay que verificar
# encender equivale a decir armar motores en OP
def encender(self):
self.actuadorOP.encender()
def setAltitudSuelo(self):
self.altitudSuelo=self.sensorGPS.getLastInfo().getData()['altitud']
def apagar(self):
self.actuadorOP.apagar()
# giro lateral de la cabeza desde la pocision donde esta
# velocidad es un entero de 1 al 50, no puede ser 0
# si grados es negativo ira a la izquierda
def yaw(self, grados, velocidad):
anguloInicial=self.sensorMagnetometro.getAnguloCabezaDron()
gradosAlcanzados=0
direccion=1
if (grados<0):
direccion=-1
grados=abs(grados)
self.actuadorOP.setYaw(self.velocidadCeroGiroOP + velocidad*direccion)
while (gradosAlcanzados<grados & velocidad>=1):
gradosAlcanzados=abs(anguloInicial-self.sensorMagnetometro.getAnguloCabezaDron())
time.sleep(.300)
self.actuadorOP.setYaw(self.velocidadCeroGiroOP)
# giro lateral de la cabeza a la izquierda desde la posicion donde esta
def yaw_izquierda(self, grados, velocidad):
self.yaw(-grados,velocidad)
# giro lateral de la cabeza a la derecha desde la pocision donde esta
def yaw_derecha(self, grados, velocidad):
self.yaw(grados,velocidad)
# elevar el dron: distancia estara en centimetros y es la distancia que debe subir desde donde esta
# velocidad para elevarse debe ser mayor a velocidadEstable y menor a velocidadMaxMotores
# velocidad se sumara a la velocidad estable - hasta velocidadMaxMotores
def up(self, distancia, velocidad):
distanciaInicial=self.getDistancaSuelo()
distanciaAlcanzada=distanciaInicial
distanciaFinal=distanciaInicial+distancia
# el throtle es la velocidad de los motores
velocidad=self.velocidadEstable+velocidad
if(velocidad>self.velocidadMaxMotores):
velocidad=self.velocidadMaxMotores
self.actuadorOP.setThrotle(velocidad)
while (distanciaAlcanzada<distanciaFinal & velocidad>self.velocidadEstable):
distanciaAlcanzada=self.getDistancaSuelo()
time.sleep(.300)
self.actuadorOP.setThrotle(self.velocidadEstable)
# bajar el dron hasta "distancia" del piso. Estara en centimetros
# velocidad tendria que ser menor a la estable si es mayor la dejo en velocidad estable, e.d. no baja
def down(self, distancia, velocidad):
if (velocidad>self.velocidadEstable):
velocidad= self.velocidadEstable
distanciaInicial=self.getDistanciaSuelo()
distanciaAlcanzada=distanciaInicial
distanciaFinal=distancia
if (distanciaFinal<30):
distanciaFinal=30
self.actuadorOP.setThrotle(velocidad)
while (distanciaAlcanzada>distanciaFinal & velocidad<self.velocidadEstable):
distanciaAlcanzada=self.getDistanciaSuelo()
time.sleep(.300)
self.actuadorOP.setThrotle(self.velocidadEstable)
# me devuelve la distancia del dron al suelo, si esta fuera del alcance del ultrasonido devuelve
# distancia GPS. para ello se debera setear 1ro distancia altitud suelo
def getDistanciaSuelo(self):
distanciaSuelo=self.sensorUltrasonido.getLastInfo()['altura']
if (distanciaSuelo>=self.alcanceUltrasonido):
distanciaSuelo=self.sensorGPS.getLastInfo()['altitud']*100-self.altitudSuelo
return distanciaSuelo
def aterrizar1(self):
self.nivelarDron()
self.down(30,self.velocidadEstable/2)
self.down(10,self.velocidad10cm)
self.down(5,self.velocidad5cm)
self.apagar()
def aterrizar2(self):
distanciaInicial=self.getDistanciaSuelo()
distanciaAlcanzada=distanciaInicial
velocidad= self.velocidadEstable * 0.8
self.actuadorOP.setThrotle(velocidad)
while (distanciaAlcanzada>40 ):
distanciaAlcanzada=self.getDistanciaSuelo()
time.sleep(.300)
velocidad= self.velocidadEstable / 2
self.actuadorOP.setThrotle(velocidad)
while (distanciaAlcanzada>10 ):
distanciaAlcanzada=self.getDistanciaSuelo()
time.sleep(.300)
velocidad=self.velocidad10cm
self.actuadorOP.setThrotle(velocidad)
while (distanciaAlcanzada>5 ):
distanciaAlcanzada=self.getDistanciaSuelo()
time.sleep(.300)
self.actuadorOP.setThrotle(self.velocidad5cm)
time.sleep(.300)
self.actuadorOP.setThrotle(self.velocidad5cm/2)
self.apagar()
# cabeceo - elevacion de la cabeza la sube/baja a los "grados" indicados en "y" del giroscopio
# a la velocidad(0-50) indicada
def pitch_arriba(self, grados, velocidad):
grados=-abs(grados) # porque el "angulo y" hacia arriba es negativo
y= self.sensorGiroscopio.getLastInfo().getData()['y']
if (velocidad>50):
velocidad=50
direccion=1 #subir
if (y<grados):
direccion=-1 #bajar
self.actuadorOP.setPitch(self.velocidadCeroGiroOP+velocidad*direccion)
while ( y<grados & velocidad>0 & direccion==-1 ): #bajar
y= self.sensorGiroscopio.getLastInfo().getData()['y']
time.sleep(.300)
while ( y>grados & velocidad>0 & direccion==1 ): #subir
y= self.sensorGiroscopio.getLastInfo().getData()['y']
time.sleep(.300)
self.actuadorOP.setPitch(self.velocidadCeroGiroOP)
# cabeceo - bajar la cabeza, baja los "grados" en y de giroscopio a la velocidad (1-50) indicada
def pitch_abajo(self, grados, velocidad):
grados=abs(grados) # porque el "angulo y" hacia abajo es positivo
y= self.sensorGiroscopio.getLastInfo().getData()['y']
if (velocidad>50):
velocidad=50
direccion=1 #subir
if (y<grados):
direccion=-1 #bajar
self.actuadorOP.setPitch(self.velocidadCeroGiroOP+velocidad*direccion)
while ( y<grados & velocidad>0 & direccion==-1 ): #bajar
y= self.sensorGiroscopio.getLastInfo().getData()['y']
time.sleep(.300)
while ( y>grados & velocidad>0 & direccion==1 ): #subir
y= self.sensorGiroscopio.getLastInfo().getData()['y']
time.sleep(.300)
self.actuadorOP.setPitch(self.velocidadCeroGiroOP)
# giro lateral de costado a la derecha: x giroscopio positivo
# deja el dron en "grados" a la derecha a la "velocidad" indicada
def roll_derecha(self, grados, velocidad):
grados=abs(grados) # porque el "angulo x" a la derecha es positivo
x= self.sensorGiroscopio.getLastInfo().getData()['x']
if (velocidad>50):
velocidad=50
direccion=1 #irA derecha
if (x>grados):
direccion=-1 #irA izquierda
self.actuadorOP.setPitch(self.velocidadCeroGiroOP+velocidad*direccion)
while ( x<grados & velocidad>0 & direccion==1 ): # irA a la derecha
x= self.sensorGiroscopio.getLastInfo().getData()['x']
time.sleep(.300)
while ( x>grados & velocidad>0 & direccion==-1 ): # irA a la izquierda
x= self.sensorGiroscopio.getLastInfo().getData()['x']
time.sleep(.300)
self.actuadorOP.setPitch(self.velocidadCeroGiroOP)
# giro lateral de costado a la izquierda
# deja el dron en "grados" a la izquierda a la "velocidad" indicada
def roll_izquierda(self, grados, velocidad ):
grados=-abs(grados) # porque el "angulo x" a la izquierda es negativo
x= self.sensorGiroscopio.getLastInfo().getData()['x']
if (velocidad>50):
velocidad=50
direccion=1 #irA derecha
if (x>grados):
direccion=-1 #irA izquierda
self.actuadorOP.setPitch(self.velocidadCeroGiroOP+velocidad*direccion)
while ( x<grados & velocidad>0 & direccion==1 ): # irA a la derecha
x= self.sensorGiroscopio.getLastInfo().getData()['x']
time.sleep(.300)
while ( x>grados & velocidad>0 & direccion==-1 ): # irA a la izquierda
x= self.sensorGiroscopio.getLastInfo().getData()['x']
time.sleep(.300)
self.actuadorOP.setPitch(self.velocidadCeroGiroOP)
# devuelve los angulos x, y z en un diccionario
def getAngulosCabeza(self):
return self.sensorGiroscopio.getLastInfo().getData()
# devuelve x y z (longitud, latitud y altura al suelo en cm) como una tupla
def getCoordenadas(self):
xyz=self.sensorGPS.getCoordenadas()
x=xyz['latitud']
y=xyz['longitud']
z=self.getDistancaSuelo()
return {'x':x,'y':y,'z':z}
# avanza el dron en direccion a al acabeza
def irAdelante(self, velocidad):
velocidad=self.velocidadEstable+abs(velocidad)
if (velocidad>self.velocidadMaxMotores):
velocidad=self.velocidadMaxMotores
self.nivelarDron()
self.pitch_abajo(self.anguloAvance,5)
self.actuadorOP.setThrotle(velocidad)
# avanza el dron en direccion contraria a la cabeza
def irAtras(self, velocidad):
velocidad=self.velocidadEstable+abs(velocidad)
if (velocidad>self.velocidadMaxMotores):
velocidad=self.velocidadMaxMotores
self.nivelarDron()
self.pitch_arriba(self.anguloAvance,5)
self.actuadorOP.setThrotle(velocidad)
# avanza el dron hacia la derecha de su cabeza
def irDerecha(self,velocidad):
velocidad=self.velocidadEstable+abs(velocidad)
if (velocidad>self.velocidadMaxMotores):
velocidad=self.velocidadMaxMotores
self.nivelarDron()
self.roll_derecha(self.anguloAvance,5)
self.actuadorOP.setThrotle(velocidad)
# avanza el dron hacia la izquierda de su cabeza
def irIzquierdad(self,velocidad):
velocidad=self.velocidadEstable+abs(velocidad)
if (velocidad>self.velocidadMaxMotores):
velocidad=self.velocidadMaxMotores
self.nivelarDron()
self.roll_izquierda(self.anguloAvance,5)
self.actuadorOP.setThrotle(velocidad)
# pone el "x" e "y" del giroscopio en 0
def nivelarDron(self):
self.pitch_abajo(0,5)
self.roll_derecha(0,5)
# estabilizado - acrobatico y tiene 6 modos mas
def setModo(self,modo):
self.actuadorOP.setModoVuelo(modo)
def mantenerCoordenadas(self):
self.nivelarDron()
self.actuadorOP.setThrotle(self.velocidadEstable)
# devuelve los valores de modos de vuelo en un diccionario:{'estabilizado', 'acrobatico'}
def getModosDeOperacion(self):
return self.modosDeOperacion