def testUpdatePitch(cls):
_quadcopter = quadcopter.quadcopter(model.model())
updt = _quadcopter.update(0.5, [950, 1000, 1050, 1000])
cls.assertAlmostEqual(_quadcopter.x[0],
0.0,
places=3,
msg=None,
delta=None)
cls.assertAlmostEqual(_quadcopter.x[1],
0.0,
places=3,
msg=None,
delta=None)
cls.assertAlmostEqual(_quadcopter.x[2],
3.5550,
places=3,
msg=None,
delta=None)
cls.assertAlmostEqual(_quadcopter.theta[0],
-7.5000,
places=3,
msg=None,
delta=None)
cls.assertAlmostEqual(_quadcopter.theta[1],
0.0,
places=3,
msg=None,
delta=None)
cls.assertAlmostEqual(_quadcopter.theta[2],
0.0125,
places=3,
msg=None,
delta=None)
def testUpdateYaw(cls):
_quadcopter = quadcopter.quadcopter(model.model())
updt = _quadcopter.update(0.5, [1200, 1000, 1200, 1000])
cls.assertAlmostEqual(_quadcopter.x[0],
0.0,
places=3,
msg=None,
delta=None)
cls.assertAlmostEqual(_quadcopter.x[1],
0.0,
places=3,
msg=None,
delta=None)
cls.assertAlmostEqual(_quadcopter.x[2],
4.8675,
places=3,
msg=None,
delta=None)
cls.assertAlmostEqual(_quadcopter.theta[0],
0.0,
places=3,
msg=None,
delta=None)
cls.assertAlmostEqual(_quadcopter.theta[1],
0.0,
places=3,
msg=None,
delta=None)
cls.assertAlmostEqual(_quadcopter.theta[2],
2.2000,
places=3,
msg=None,
delta=None)
def setUpClass(cls):
cls._quadcopter = quadcopter.quadcopter(model.model())
parser.add_argument("-i", dest="imulog", action="store_true", help="save IMU data log: myQ_sensor.csv")
parser.add_argument("-ip", dest="ip", action="store", help="set ip addres for netscan")
parser.add_argument("-c", dest="calibIMU", action="store_true", help="Calibrate IMU")
parser.add_argument("-n", dest="netscan", action="store_true", help="Start network check")
parser.add_argument("-w", dest="webserver", action="store_true", help="Start webserver|http//:192.68.0.10/myQ.html")
args = parser.parse_args()
# TODO move this options in mode_init
# init logger
logger = setupLogger("myQ", args.debug, "myQ.log")
logger.info("myQ starting...")
logger.info("Fasten your seat belt")
# screen = curses.initscr()
myQ = quadcopter("qpi", pin0=18, pin1=23, pin2=24, pin3=25, simulation=False)
# GPIO: 18 23 24 25
# pin : 12 16 18 22
myQ.imulog = args.imulog
myQ.savelog = args.savelog
myQ.calibIMU = args.calibIMU
myQ.debuglev = args.debug
myQ.netscanOn = args.netscan # TODO when fully tested , set netscan on, by default
myQ.webserverOn = args.webserver
myQ.load("myQ.cfg")
if args.ip is not None:
logger.debug("New IP Address to scan: " + args.ip)
myQ.ip = args.ip
dest='webserver',
action='store_true',
help='Start webserver|http//:192.68.0.10/myQ.html')
args = parser.parse_args()
#TODO move this options in mode_init
#init logger
logger = setupLogger('myQ', args.debug, 'myQ.log')
logger.info('myQ starting...')
logger.info('Fasten your seat belt')
#screen = curses.initscr()
myQ = quadcopter('qpi',
pin0=18,
pin1=23,
pin2=24,
pin3=25,
simulation=False)
#GPIO: 18 23 24 25
#pin : 12 16 18 22
myQ.imulog = args.imulog
myQ.savelog = args.savelog
myQ.calibIMU = args.calibIMU
myQ.debuglev = args.debug
myQ.netscanOn = args.netscan # TODO when fully tested , set netscan on, by default
myQ.webserverOn = args.webserver
myQ.load('myQ.cfg')
if args.ip is not None:
#!/usr/bin/env python2
from quadcopter import quadcopter
my_quadcopter = quadcopter()
direction_map = {'a': 'left', 'd': 'right', 's': 'rear', 'w': 'front'}
def controller():
while True:
print("command: init | take_off | dsn | all #speed | (a|s|d|w) #speed | rollb")
command = raw_input()
if command == 'init':
my_quadcopter.init_hardware()
elif command == 'take_off':
my_quadcopter.take_off()
elif command == "rollb":
my_quadcopter.keep_balance()
elif command[0:3] == 'all':
[co, speed] = command.split(' ')
my_quadcopter.set_all_to(int(speed))
elif command == 'dsn':
my_quadcopter.descend()
elif command[0] in direction_map:
[mo_short, speed] = command.split(' ')
mo = direction_map[command[0]]
my_quadcopter.set_unique_to(mo, int(speed))
else:
print("wrong command!!!!\n")
controller()
"""\ First test of the code. Objectives: 1) Have an arm with a single motor at either end come to a level balance from any starting position. 2) Maybe something else? Not sure yet. This will require... 1) Get the current roll angle (I'll use roll instead of pitch, for no particular reason) 2) Run a PD loop to close the error """ from quadcopter import quadcopter import sys quad = quadcopter() pitch_setpoint = 0.0 roll_setpoint = 0.0 z_accel_setpoint = 1.0 max_z_error = 1.0 # we'll just normalise to 1G maybe. while True: current_roll_error = quad.get_rotational_error(pitch_setpoint, roll_setpoint, z_accel_setpoint)[1] # get the roll error z_accel_error = quad.get_acceleration_error(0.0,0.0,z_accel_setpoint)[2] both_throttles =
def main():
quadcopter = quad.quadcopter(model.model())
# References
altitudeReference = 10
yawReference = 2
pitchReference = 0.0
rollReference = 0.0
# Last values
lastAltitudeError = 0
lastYawError = 0
lastPitchError = 0
lastRollError = 0
controller = fuzzy_control.fuzzyController(ALTITUDE_RANGE, [0, 100])
yawController = fuzzy_control.fuzzyController(YAW_RANGE, [-50, 50])
pitchController = fuzzy_control.fuzzyController(PITCH_RANGE, [-50, 50])
rollController = fuzzy_control.fuzzyController(ROLL_RANGE, [-50, 50])
altitudeHistory = []
yawHistory = []
pitchHistory = []
rollHistory = []
timeHistory = []
for i in range(MAXSTEPS):
# Do noise :)
quadcopter.x[2] += random() / 2
quadcopter.theta[2] += random() / 10
quadcopter.theta[0] += random() / 10
quadcopter.theta[1] += random() / 10
currentAltitude = quadcopter.x[2]
currentYaw = quadcopter.theta[2]
currentPitch = quadcopter.theta[0]
currentRoll = quadcopter.theta[1]
# Update reference
altitudeReference = (10 - 10 * math.exp(-i * 0.005))
# Evaluate input signals
# Altitude Error
altitudeError = currentAltitude - altitudeReference
altitudeErrorDelta = altitudeError - lastAltitudeError
# Yaw error
yawError = currentYaw - yawReference
yawErrorDelta = yawError - lastYawError
# Pitch error
pitchError = currentPitch - pitchReference
pitchErrorDelta = pitchError - lastPitchError
# Roll error
rollError = currentRoll - rollReference
rollErrorDelta = rollError - lastRollError
# Update signals
lastAltitudeError = altitudeError
lastYawError = yawError
lastPitchError = pitchError
lastRollError = rollError
# Control
altitudeControl = 10 * controller.output(
[altitudeError, altitudeErrorDelta])
yawControl = yawController.output([yawError, yawErrorDelta])
pitchControl = pitchController.output([pitchError, pitchErrorDelta])
rollControl = rollController.output([rollError, rollErrorDelta])
motor1 = altitudeControl + yawControl + pitchControl
motor2 = altitudeControl - yawControl + rollControl
motor3 = altitudeControl + yawControl - pitchControl
motor4 = altitudeControl - yawControl - rollControl
print "Altitude=%s / Controller=%s / Error=%s / Derror=%s" % (
currentAltitude, altitudeControl, altitudeError,
altitudeErrorDelta)
print "Yaw=%s / Controller=%s / Error=%s / Derror=%s" % (
currentYaw, yawControl, yawError, yawErrorDelta)
print "Pitch=%s / Controller=%s / Error=%s / Derror=%s" % (
currentPitch, pitchControl, pitchError, pitchErrorDelta)
print "Roll=%s / Controller=%s / Error=%s / Derror=%s" % (
currentRoll, rollControl, rollError, rollErrorDelta)
# Update quadcopter state
quadcopter.update(STEPTIME, [motor1, motor2, motor3, motor4])
# History maker
altitudeHistory.append(currentAltitude)
yawHistory.append(currentYaw)
pitchHistory.append(currentPitch)
rollHistory.append(currentRoll)
timeHistory.append(i * STEPTIME)
# Plot results
plt.plot(timeHistory, altitudeHistory, 'b')
plt.plot(timeHistory, [altitudeReference] * (len(timeHistory)), 'g--')
plt.plot(timeHistory, yawHistory, 'r')
plt.plot(timeHistory, pitchHistory, 'k')
plt.plot(timeHistory, rollHistory, 'g')
plt.show()