Skip to content

Instantly share code, notes, and snippets.

@Hart87 Hart87/FlightCon.py
Last active Sep 21, 2019

Embed
What would you like to do?
A functioning Quadcopter flight controller using and Accelerometer for inertial monitoring, PWM, and MQTT. This script accepts MQTT messages from a slider in an iOS app to adjust motor throttle. UIButtons in iOS app fire off flight control maneuvers when it’s pressed and return to level throttle when the button is unpressed
import paho.mqtt.client as mqtt
from time import sleep
import pigpio
import Adafruit_ADXL345
import multiprocessing
import datetime
pi = pigpio.pi('localhost', 8888)
# Create an ADXL345 instance.
accel = Adafruit_ADXL345.ADXL345()
pi.set_servo_pulsewidth(26, 0) #aft starboard motor - MOTOR 1
pi.set_servo_pulsewidth(20, 0) #forward port motor - MOTOR 4
pi.set_servo_pulsewidth(19, 0) #forward starboard motor - MOTOR 2
pi.set_servo_pulsewidth(21, 0) #aft port motor - MOTOR 3
print ("ENGAGING ACCELEROMETER")
x, y, z = accel.read()
print ( x )
print ( y )
###### INTERTIAL MONITORING UNIT ################
#Level X and Y
def gyro_X_plus(x_axe):
while x_axe > 0:
# motor 1 & 2 down
m1pw = pi.get_servo_pulsewidth(26)
m2pw = pi.get_servo_pulsewidth(19)
m1d = m1pw - 1
m2d = m2pw - 1
pi.set_servo_pulsewidth(26, m1d)
pi.set_servo_pulsewidth(19, m2d)
xi, yi, zi = accel.read()
x_axe = xi
print ( x_axe )
print ("Gyro X positive decremented")
print ( datetime.datetime.now())
return
def gyro_Y_plus(y_axe):
print("gyro y troubleshoot")
while y_axe > 0:
# motor 4 & 2 Down
m4pw = pi.get_servo_pulsewidth(20)
m2pw = pi.get_servo_pulsewidth(19)
m4d = m4pw - 1
m2d = m2pw - 1
pi.set_servo_pulsewidth(20, m4d)
pi.set_servo_pulsewidth(19, m2d)
xi, yi, zi = accel.read()
y_axe = yi
print ( y_axe )
print ("Gyro Y positive decremented")
print ( datetime.datetime.now())
return
def gyro_X_minus(x_axe):
while x_axe < 0:
# motor 3 & 4 down
m3pw = pi.get_servo_pulsewidth(21)
m4pw = pi.get_servo_pulsewidth(20)
m3d = m3pw - 1
m4d = m4pw - 1
pi.set_servo_pulsewidth(21, m3d)
pi.set_servo_pulsewidth(20, m4d)
xi, yi, zi = accel.read()
x_axe = xi
print ( x_axe )
print ("Gyro X negative decremented")
print ( datetime.datetime.now())
return
def gyro_Y_minus(y_axe):
while y_axe < 0:
# motor 3 & 1 Down
m3pw = pi.get_servo_pulsewidth(21)
m1pw = pi.get_servo_pulsewidth(26)
m3d = m3pw - 1
m1d = m1pw - 1
pi.set_servo_pulsewidth(21, m3d)
pi.set_servo_pulsewidth(26, m1d)
xi, yi, zi = accel.read()
y_axe = yi
print ( y_axe )
print ("Gyro Y negative decremented")
print ( datetime.datetime.now())
return
def IMU():
print ( "G Y R O S C O P E" )
#read X & Y axis and store variables
x, y, z = accel.read()
x_axis = x
y_axis = y
absolute_x = abs(x_axis)
absolute_y = abs(y_axis)
print (" Absolute axis ..." )
print ( absolute_x , x_axis )
print ( absolute_y , y_axis )
#Turn down motors methods
#Determine if it is or isnt level, and then which axis is worse. level that first
if absolute_x > 4 or absolute_y > 4 :
if absolute_x >= absolute_y : #IF X HAS MORE TILT THAN Y
#DOES X HAVE POS OR NEG TILT?
if x_axis > 0:
gyro_X_plus(x_axis)
else :
gyro_X_minus(x_axis)
#level the other side that had less tilt....
if y_axis > 0:
gyro_Y_plus(y_axis)
else :
gyro_Y_minus(y_axis)
else : # IF Y HAS MORE TILT THAN X
if y_axis > 0: #DOES Y HAVE POS OR NEG TILT?
gyro_Y_plus(y_axis)
else :
gyro_Y_minus(y_axis)
#level the other side that had less tilt.....
if x_axis > 0:
gyro_X_plus(x_axis)
else :
gyro_X_minus(x_axis)
else :
print ("ALL LEVEL")
print ( datetime.datetime.now())
###### THREAD FOR IMU
def runIMU():
x = 0
while x < 3:
IMU
x +=1
#########################################################3
## THRUST
#########################################################
def ChangeTheCycle( x ):
y = int(x)
pi.set_servo_pulsewidth(26, y)
pi.set_servo_pulsewidth(20, y)
pi.set_servo_pulsewidth(21, y)
pi.set_servo_pulsewidth(19, y)
runIMU()
return
print('turning on')
for x in range(1000,1050,1):
pi.set_servo_pulsewidth(26, x)
pi.set_servo_pulsewidth(20, x)
pi.set_servo_pulsewidth(21, x)
pi.set_servo_pulsewidth(19, x)
sleep(0.10) #1ms delay
print('turning off')
for x in range(1050,1000,-1):
pi.set_servo_pulsewidth(26, x)
pi.set_servo_pulsewidth(20, x)
pi.set_servo_pulsewidth(21, x)
pi.set_servo_pulsewidth(19, x)
sleep(0.10)
######## FLIGHT MANUEVERS #################
#Yaw Left
def YawLeft():
#Increase motor 1 & 4
flight_control = True
m1pw = pi.get_servo_pulsewidth(26)
m4pw = pi.get_servo_pulsewidth(20)
setm1 = m1pw + 20
setm4 = m4pw + 20
pi.set_servo_pulsewidth(26, setm1)
pi.set_servo_pulsewidth(20, setm4)
return
def DepressYL():
flight_control = False
m1pulse = pi.get_servo_pulsewidth(26)
m4pulse = pi.get_servo_pulsewidth(20)
setm1 = m1pulse - 20
setm4 = m4pulse - 20
pi.set_servo_pulsewidth(26, setm1)
pi.set_servo_pulsewidth(20, setm4)
sleep(1)
runIMU()
return
#Yaw Right
def YawRight():
#Increase motor 2 & 3
flight_control = True
m2pw = pi.get_servo_pulsewidth(19)
m3pw = pi.get_servo_pulsewidth(21)
setm2 = m2pw + 20
setm3 = m3pw + 20
pi.set_servo_pulsewidth(19, setm1)
pi.set_servo_pulsewidth(21, setm4)
return
def DepressYR():
flight_control = False
m2pulse = pi.get_servo_pulsewidth(19)
m3pulse = pi.get_servo_pulsewidth(21)
setm2 = m2pulse - 20
setm3 = m3pulse - 20
pi.set_servo_pulsewidth(19, setm2)
pi.set_servo_pulsewidth(21, setm3)
sleep(1)
runIMU()
return
#Pitch Forward
def PitchForward():
flight_control = True
#Increase motor 1 & 3
m1pw = pi.get_servo_pulsewidth(26)
m3pw = pi.get_servo_pulsewidth(21)
setm1 = m1pw + 20
setm3 = m3pw + 20
pi.set_servo_pulsewidth(26, setm1)
pi.set_servo_pulsewidth(21, setm3)
return
def DepressFwd():
flight_control = False
m1pw = pi.get_servo_pulsewidth(26)
m3pw = pi.get_servo_pulsewidth(21)
setm1 = m1pw - 20
setm3 = m3pw - 20
pi.set_servo_pulsewidth(26, setm1)
pi.set_servo_pulsewidth(21, setm3)
sleep(1)
runIMU()
return
#Pitch Backward
def PitchBackward():
flight_control = True
#Increase motor 2 & 4
m2pw = pi.get_servo_pulsewidth(19)
m4pw = pi.get_servo_pulsewidth(20)
setm2 = m2pw + 20
setm4 = m4pw + 20
pi.set_servo_pulsewidth(19, setm2)
pi.set_servo_pulsewidth(20, setm4)
return
def DepressBwd():
flight_control = False
m2pw = pi.get_servo_pulsewidth(19)
m4pw = pi.get_servo_pulsewidth(20)
setm2 = m2pw - 20
setm4 = m4pw - 20
pi.set_servo_pulsewidth(19, setm2)
pi.set_servo_pulsewidth(20, setm4)
sleep(1)
runIMU()
return
#Roll Left
def RollLeft():
flight_control = True
#Increase motor 1 & 2
m2pw = pi.get_servo_pulsewidth(19)
m1pw = pi.get_servo_pulsewidth(26)
setm2 = m1pw + 20
setm1 = m1pw + 20
pi.set_servo_pulsewidth(19, setm2)
pi.set_servo_pulsewidth(26, setm1)
return
def DepressRL():
flight_control = False
m2pw = pi.get_servo_pulsewidth(19)
m1pw = pi.get_servo_pulsewidth(26)
setm2 = m1pw - 20
setm1 = m1pw - 20
pi.set_servo_pulsewidth(19, setm2)
pi.set_servo_pulsewidth(26, setm1)
sleep(1)
runIMU()
return
#Roll Right
def RollRight():
flight_control = True
#Increase motor 3 & 4
m3pw = pi.get_servo_pulsewidth(21)
m4pw = pi.get_servo_pulsewidth(20)
setm3 = m3pw + 20
setm4 = m4pw + 20
pi.set_servo_pulsewidth(21, setm3)
pi.set_servo_pulsewidth(20, setm4)
return
def DepressRR():
flight_control = False
m3pw = pi.get_servo_pulsewidth(21)
m4pw = pi.get_servo_pulsewidth(20)
setm3 = m3pw - 20
setm4 = m4pw - 20
pi.set_servo_pulsewidth(21, setm3)
pi.set_servo_pulsewidth(20, setm4)
sleep(1)
runIMU()
return
###########################################
#Create an MQTT client and attach our routines to it.
client = mqtt.Client()
print("mqtt")
#MQTT
def on_connect(client, userdata, flags, rc):
print("Connected to broker")
client.subscribe("quadcopter/throttle/input")
client.subscribe("quadcopter/throttle/yawleft")
client.subscribe("quadcopter/throttle/yawright")
client.subscribe("quadcopter/throttle/pitchforward")
client.subscribe("quadcopter/throttle/pitchbackward")
client.subscribe("quadcopter/throttle/rollleft")
client.subscribe("quadcopter/throttle/rollright")
client.subscribe("quadcopter/depress/rollright")
client.subscribe("quadcopter/depress/rollleft")
client.subscribe("quadcopter/depress/yawleft")
client.subscribe("quadcopter/depress/yawright")
client.subscribe("quadcopter/depress/pitchforward")
client.subscribe("quadcopter/depress/pitchbackward")
#add Depress button methods .......
def on_message(client, userdata, msg):
print(msg.topic+" "+str(msg.payload))
if msg.topic == "quadcopter/throttle/input":
print(msg.payload)
ChangeTheCycle( msg.payload )
elif msg.topic == "quadcopter/throttle/yawleft":
print("yaw left")
YawLeft()
elif msg.topic == "quadcopter/depress/yawleft":
print("depress yaw left")
DepressYL()
elif msg.topic == "quadcopter/throttle/yawright":
print("yaw right")
YawRight()
elif msg.topic == "quadcopter/depress/yawright":
print("Depress yaw right")
DepressYR()
elif msg.topic == "quadcopter/throttle/pitchforward":
print("pitch forward")
PitchForward()
elif msg.topic == "quadcopter/depress/pitchforward":
print("depress pitch forward")
DepressFwd()
elif msg.topic == "quadcopter/throttle/pitchbackward":
elif msg.topic == "quadcopter/depress/pitchbackward":
print("depress pitch backward")
DepressBwd()
elif msg.topic == "quadcopter/throttle/rollleft":
print("roll left")
RollLeft()
elif msg.topic == "quadcopter/depress/rollleft":
print("depress roll left")
DepressRL()
elif msg.topic == "quadcopter/depress/rollright":
print("depress roll right")
DepressRR()
elif msg.topic == "quadcopter/throttle/rollright":
print("roll right")
RollRight()
else:
print("No")
def on_disconnect(client, userdata, flags, rc):
print("disconnected from broker")
client.connect("192.168.86.136", 1883, 60)
client.on_connect = on_connect
client.on_message = on_message
client.on_disconnect = on_disconnect
client.loop_forever()
@Hart87

This comment has been minimized.

Copy link
Owner Author

Hart87 commented Aug 10, 2019

** test leads with multimeter for level PWM output across motors

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.