class FlightController:
def __init__(self):
self.im_width = 640
self.im_height = 480
self.board = MultiWii("COM4")
def compute_action(self, x1, y1, x2, y2, scale_factor=0.25):
# define the possible turning and moving action
self.board.getData(MultiWii.RAW_IMU)
turning, moving, vertical = self.board.rawIMU
area = (x2 - x1) * (y2 - y1)
center = [(x2 + x1) / 2, (y2 + y1) / 2]
# obtain a x center between 0.0 and 1.0
normalized_center = center[0] / self.im_width
# obtain a y center between 0.0 and 1.0
normalized_center.append(center[1] / self.im_height)
if normalized_center[0] > 0.6:
turning += scale_factor
elif normalized_center[0] < 0.4:
turning -= scale_factor
if normalized_center[1] > 0.6:
vertical += scale_factor
elif normalized_center[1] < 0.4:
vertical -= scale_factor
# if the area is too big move backwards
if area > 100:
moving += scale_factor
elif area < 80:
moving -= scale_factor
return [turning, moving, vertical]
def get_observation(self, velocities): #imu [ax, ay, az]
self.board.getData(MultiWii.RAW_IMU)
imu = self.board.rawIMU
return imu[:3] - velocities
def send_action(self, motors):
self.board.sendCMD(16, MultiWii.SET_MOTORS, motors)
while True:
data, _ = cmd_socket.recvfrom(bufflen)
data = data.split(':')
if data == ['']:
continue
#print str(data)
orden_s = [int(data[0]), int(data[1]), int(data[2]), int(data[3])]
print orden_s
if orden_s[3] == -2:
print 'disarm command received'
break
if (orden_s != orden):
orden = orden_s
#imu=uav.sendCMDreceiveATT(8,MultiWii.SET_RAW_RC,orden)
uav.sendCMD(8, MultiWii.SET_RAW_RC, orden)
##message =str(uav.getData(MultiWii.ATTITUDE))
##mon_socket.sendto(message,address)
#if imu!=None:
#imu_socket.sendto(imu,dir)
##print orden
if not data: break
cmd_socket.close()
uav.disarm()
except KeyboardInterrupt:
cmd_socket.close()
uav.disarm()
except Exception as e:
print str(e)
cmd_socket.close()
rotate_offset)
if abs(
controller.a_joystick_left_y
) >= 0.1 and throttle >= 1000 and throttle <= 2000:
throttle = throttle + int(
controller.a_joystick_left_y * (-10))
elif throttle > 2000:
throttle = 2000
elif throttle < 1000:
throttle = 1000
if cv2.waitKey(1) & 0xFF == ord('q'):
break
data = [roll, pitch, rotate, throttle]
butterfly.sendCMD(8, MultiWii.SET_RAW_RC, data)
textPrint.print(screen,
"Console Messege: {}".format(console_messege))
textPrint.print(screen, " {}".format(" "))
textPrint.print(screen, "Roll: {}".format(roll))
textPrint.print(screen, "Pitch: {}".format(pitch))
textPrint.print(screen, "Rotate: {}".format(rotate))
textPrint.print(screen, "Throttle: {}".format(throttle))
textPrint.print(screen, " {}".format(" "))
textPrint.print(screen, "Sensitivity: {}".format(sensitivity))
board.arm()
print("Drone is armed now!")
elif (key.decode() == 'd'): #disarm
board.disarm()
print("Disarmed.")
elif (key.decode() == 'r'): #reset
pitch = 1500
roll = 1500
rotate = 1500
throttle = 1000
CNN = -1
elif (key.decode() == 'u'): #left
#pitch = 1400
roll = 1300
data = [roll, pitch, rotate, throttle]
board.sendCMD(8, MultiWii.SET_RAW_RC, data)
print(data)
time.sleep(0.1)
#pitch = 1500
roll = 1500
elif (key.decode() == 'o'): #right
#pitch = 1600
roll = 1700
data = [roll, pitch, rotate, throttle]
board.sendCMD(8, MultiWii.SET_RAW_RC, data)
print(data)
time.sleep(0.1)
#pitch = 1500
roll = 1500
elif (key.decode() == 'j'): #spin left
rotate = 1300
input("press any key to continue")
try:
f = open("decision.txt", mode='w')
f.write(str(init + 0.1))
print("detected")
except Exception as e:
print(e)
finally:
f.close()
try:
drone.open("/dev/rfcomm0")
time.sleep(2)
drone.arm()
time.sleep(0.5)
drone.sendCMD(8, MultiWii.SET_RAW_RC, [1500, 1500, 1500, 1000], '4h')
print("arm")
except Exception as error:
print(error)
# pyplot.ion()
# fig = pyplot.figure()
# imu_fig = fig.add_subplot(121)
# pyplot.xticks([1, 2], ['roll', 'pitch'])
# rect1 = imu_fig.bar([1, 2], [-90, 90])
#
# imu_fig = fig.add_subplot(122)
# pyplot.xticks([1, 2, 3, 4], ['roll', 'pitch', 'yaw', 'throttle'])
# rect2 = imu_fig.bar([1, 2, 3, 4], [1000, 2000, 1000, 1000])
start = time.time()
class pi_drone_driver():
def __init__(self):
self.board = MultiWii("/dev/ttyACM0")
rospy.init_node("pi_drone", anonymous=True)
self.imu_pub = rospy.Publisher("/pi_drone/imu", Imu, queue_size=1)
self.attitude_pub = rospy.Publisher("/pi_drone/rpy", Vector3, queue_size=1)
rospy.Subscriber("/pi_drone/RC_in", sendRC, self.send_rc_callback)
rospy.Subscriber("/pi_drone/arm", Bool, self.arm_callback)
self.imu_msg = Imu()
self.rpy_msg = Vector3()
self.rc_channels = [1500, 1500, 1500, 1000, 1000, 1000, 1000, 1000]
self.arm = False
self.prev_imu_time = 0
self.prev_attitude_time = 0
while not rospy.is_shutdown():
self.attitude_publisher(30)
self.imu_publisher(50)
def deg2rad(self, deg):
rad = (3.14159/180.0) * deg
return rad
def attitude_publisher(self, rate):
if ((time.time() - self.prev_attitude_time) >= (1.0 / rate)):
self.prev_attitude_time = time.time()
self.board.getData(MultiWii.ATTITUDE)
self.rpy_msg.x = self.board.attitude['angx']
self.rpy_msg.y = self.board.attitude['angy']
self.rpy_msg.z = self.board.attitude['heading']
self.attitude_pub.publish(self.rpy_msg)
def imu_publisher(self, rate):
if ((time.time() - self.prev_imu_time) >= (1.0 / rate)):
self.prev_imu_time = time.time()
self.board.getData(MultiWii.RAW_IMU)
ax = self.board.rawIMU['ax']
ay = self.board.rawIMU['ay']
az = self.board.rawIMU['az']
gx = self.board.rawIMU['gx']
gy = self.board.rawIMU['gy']
gz = self.board.rawIMU['gz']
self.imu_msg.header.stamp = rospy.Time.now()
self.imu_msg.header.frame_id = "imu_link"
self.imu_msg.orientation_covariance = [-1.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
self.imu_msg.angular_velocity_covariance = [-1.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
self.imu_msg.linear_acceleration_covariance = [-1.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
self.imu_msg.linear_acceleration.x = 9.81 * ax/512
self.imu_msg.linear_acceleration.y = 9.81 * ay/512
self.imu_msg.linear_acceleration.z = -9.81 * az/512
self.imu_msg.angular_velocity.x = self.deg2rad(gx/4.096)
self.imu_msg.angular_velocity.y = self.deg2rad(gy/4.096)
self.imu_msg.angular_velocity.z = self.deg2rad(gz/4.096)
#rospy.loginfo(imu_msg)
self.imu_pub.publish(self.imu_msg)
def send_rc_callback(self, rc_data):
self.rc_channels[0] = rc_data.channels[0]
self.rc_channels[1] = rc_data.channels[1]
self.rc_channels[2] = rc_data.channels[2]
self.rc_channels[3] = rc_data.channels[3]
if self.arm:
self.rc_channels[4] = 2000
else:
self.rc_channels[4] = 1000
#print(self.rc_channels)
self.board.sendCMD(16,MultiWii.SET_RAW_RC, self.rc_channels)
def arm_callback(self, msg):
if msg.data:
self.arm = True
else:
self.arm = False
# Importar modulos esenciales para el manejo del sensor y el motor import RPi.GPIO as GPIO import time from pymultiwii import MultiWii # Constante del macro que define el cambio en un parametro de motores SET_MOTOR = 214 # Activacion del puerto serial al cual esta conectado el ESC del motor serialPort = "/dev/ttyUSB0" board = MultiWii(serialPort) #Inicializacion del motor power = 1080 Throttle = [power, 0, 0, 0] board.sendCMD(8, SET_MOTOR, Throttle) # Pin GPIO donde esta conectado el activador (entrada) del # sensor HC-SR04. TRIG = 11 # Pin GPIO donde esta conectado el eco (salida) del sensor # HC-SR04. ECHO = 13 # Distancia segmentada del cilindro - 27 cm hasta el Sensor tamCilindro = 27 # Indicar que se usa la numeracion de pines en la Raspberry GPIO.setmode(GPIO.BOARD)