class GiraDer:
def __init__(self):
self.controller = BasicDroneController()
#Subscribers
self.giraDer_sub = rospy.Subscriber('GiraDer', Empty, self.callback)
def callback(self, data):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ - 90
girando = True
while girando:
rotZ = self.controller.getRotZ()
if (rotZ > rotZ_dest):
if self.controller.getYawVelocity() >= 0:
self.controller.SetCommand(0, 0, -1, 0)
else:
girando = False
self.controller.SetCommand(0, 0, 0, 0)
class GiraDer:
def __init__(self):
self.controller = BasicDroneController()
#Subscribers
self.giraDer_sub = rospy.Subscriber('GiraDer',Empty,self.callback)
def callback(self,data):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ - 90
girando = True
while girando:
rotZ = self.controller.getRotZ()
if (rotZ > rotZ_dest):
if self.controller.getYawVelocity() >= 0:
self.controller.SetCommand(0, 0, -1, 0)
else:
girando = False
self.controller.SetCommand(0, 0, 0, 0)
class Ordenes:
def __init__(self):
self.controller = BasicDroneController()
self.girando = False
self.despLatIzq = False
self.despLatDer = False
self.subiendoDer = False
self.subiendoIzq = False
self.bajandoDer = False
self.bajandoIzq = False
self.subiendo = False
self.bajando = False
def getStatus(self):
return self.controller.status
"""
def emergencia(self):
self.controller.SendEmergency()
def despega(self):
self.controller.SendTakeoff()
def aterriza(self):
self.controller.SendLand()
"""
def yawIzq(self):
self.controller.SetCommand(0, 0, 0.25, 0)
def yawDer(self):
self.controller.SetCommand(0, 0, -0.25, 0)
def paraYaw(self):
self.controller.setYaw(0)
def paraCentrar(self):
self.controller.setRoll(0)
self.controller.setZvelocity(0)
def paraPitch(self):
self.controller.setPitch(0)
def estaGirando(self):
return self.girando
def gira(self):
self.controller.setYaw(1)
self.girando = True
def paraGiro(self):
self.controller.setYaw(0)
self.girando = False
def gira90Izq(self):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ + 90
self.controller.writeLog('rotZ_ini ' + str(rotZ))
self.controller.writeLog('rotZ_dest ' + str(rotZ_dest))
self.girando = True
while self.girando:
rotZ = self.controller.getRotZ()
if (rotZ < rotZ_dest):
if self.controller.getYawVelocity() <= 0:
self.controller.SetCommand(0, 0, 1, 0)
else:
self.girando = False
self.controller.writeLog('YA LLEGAMOS')
self.controller.SetCommand(0, 0, 0, 0)
def gira90Der(self):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ - 90
self.controller.writeLog('rotZ_ini ' + str(rotZ))
self.controller.writeLog('rotZ_dest ' + str(rotZ_dest))
self.girando = True
while self.girando:
rotZ = self.controller.getRotZ()
if (rotZ > rotZ_dest):
if self.controller.getYawVelocity() >= 0:
self.controller.SetCommand(0, 0, -1, 0)
else:
self.girando = False
self.controller.writeLog('YA LLEGAMOS')
self.controller.SetCommand(0, 0, 0, 0)
def gira180(self):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ + 160
self.controller.writeLog('rotZ_ini ' + str(rotZ))
self.controller.writeLog('rotZ_dest ' + str(rotZ_dest))
self.girando = True
while self.girando:
rotZ = self.controller.getRotZ()
if (rotZ < rotZ_dest):
if self.controller.getYawVelocity() <= 0:
self.controller.SetCommand(0, 0, 1, 0)
else:
self.girando = False
self.controller.writeLog('YA LLEGAMOS')
self.controller.SetCommand(0, 0, 0, 0)
def corregirYaw(self, angulo):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ + angulo
self.controller.writeLog('rotZ_ini ' + str(rotZ))
self.controller.writeLog('rotZ_dest ' + str(rotZ_dest))
self.girando = True
while self.girando:
rotZ = self.controller.getRotZ()
if angulo < 0:
if (rotZ > rotZ_dest):
self.controller.writeLog('rotZ ' + str(rotZ))
#if self.controller.getYawVelocity() >= 0:
#self.controller.SetCommand(0, 0, -10, 0)
else:
self.girando = False
self.controller.writeLog('YA LLEGAMOS')
#self.controller.SetCommand(0, 0, 0, 0)
else:
if (rotZ < rotZ_dest):
self.controller.writeLog('rotZ ' + str(rotZ))
#if self.controller.getYawVelocity() <= 0:
#self.controller.SetCommand(0, 0, 10, 0)
else:
self.girando = False
self.controller.writeLog('YA LLEGAMOS')
#self.controller.SetCommand(0, 0, 0, 0)
def despLateralDer(self):
self.controller.SetCommand(-0.15, 0, 0, 0)
self.despLateral = True
def despLateralIzq(self):
self.controller.SetCommand(0.15, 0, 0, 0)
self.despLateral = True
def paraDespLateral(self):
self.controller.SetCommand(0, 0, 0, 0)
self.despLateral = False
"""
def sube(self, alt):
alt_ini = self.controller.getNavdata().altd
alt_fin = alt_ini + alt
subiendo = True
while subiendo:
alt_act = self.controller.getNavdata().altd
if alt_act < alt_fin:
if self.controller.getLinearZ() == 0:
self.controller.SetCommand(0, 0, 0, 1)
else:
subiendo = False
self.controller.writeLog('YA ESTAMOS ARRIBA')
self.controller.SetCommand(0, 0, 0, 0)
def baja(self, alt):
alt_ini = self.controller.getNavdata().altd
alt_fin = alt_ini - alt
bajando = True
while bajando:
alt_act = self.controller.getNavdata().altd
if alt_act > alt_fin:
if self.controller.getLinearZ() == 0:
self.controller.SetCommand(0, 0, 0, -1)
else:
bajando = False
self.controller.writeLog('YA ESTAMOS ABAJO')
self.controller.SetCommand(0, 0, 0, 0)
"""
def sube(self):
self.controller.SetCommand(0, 0, 0, 0.15)
self.subiendo = True
def baja(self):
self.controller.SetCommand(0, 0, 0, -0.15)
self.bajando = True
def subeIzq(self):
self.controller.SetCommand(0.15, 0, 0, 0.15)
self.subiendoIzq = True
def subeDer(self):
self.controller.SetCommand(-0.15, 0, 0, 0.15)
self.subiendoDer = True
def bajaIzq(self):
self.controller.SetCommand(0.15, 0, 0, -0.15)
self.bajandoIzq = True
def bajaDer(self):
self.controller.SetCommand(-0.15, 0, 0, -0.15)
self.bajandoDer = True
def avanza(self, vel):
self.controller.SetCommand(0, vel, 0, 0)
def retrocede(self, vel):
self.controller.SetCommand(0, (-1)*vel, 0, 0)
def para(self):
self.controller.SetCommand(0, 0, 0, 0)
self.girando = False
self.despLatIzq = False
self.despLatDer = False
self.subiendoDer = False
self.subiendoIzq = False
self.bajandoDer = False
self.bajandoIzq = False
self.subiendo = False
self.bajando = False
