def control():
global vehicle, rcCMD
global rollPID, pitchPID, heightPID, yawPID
global desiredPos, currentPos, velocities
global desiredRoll, desiredPitch, desiredThrottle
global rPIDvalue, pPIDvalue, yPIDvalue
global f_yaw, f_pitch, f_roll, f_desx, f_desy
global vel_x, vel_y, vel_z
global sl_currentPos, sl_xPIDvalue, sl_yPIDvalue, slx_posPID, sly_posPID
# Start simulation messages
print "\n\nStarting simulation!!\n"
print "Simulation size -> %d" % (data.shape[0])
print "Estimated completion time -> %0.2f minutes" % (
(data.shape[0] * update_rate) / 60)
time.sleep(3)
# Start Neural network
network = 'modules/networks/sl_rtrl_step_control.csv'
print "Starting \'%s\' neural network..." % (network)
net = prn.loadNN(network)
# Array of inputs to network
memory = 100
inputs = np.zeros((10, memory))
print "Neural network active!"
time.sleep(1)
#while True:
# if udp.active:
# print "UDP server is active..."
# break
# else:
# print "Waiting for UDP server to be active..."
# time.sleep(0.5)
try:
if logging:
st = datetime.datetime.fromtimestamp(
time.time()).strftime('%m_%d_%H-%M-%S') + ".csv"
f = open("logs/ground-test-" + st, "w")
logger = csv.writer(f)
# V -> vehicle | P -> pilot (joystick) | D -> desired position
# M -> motion capture | C -> commanded controls | sl -> Second marker | Mode
logger.writerow(('timestamp','Vroll','Vpitch','Vyaw','Proll','Ppitch','Pyaw','Pthrottle', \
'x','y','z','Dx','Dy','Dz','Mroll','Mpitch','Myaw','Mode','Croll','Cpitch','Cyaw','Cthrottle', \
'slx','sly','slz','slr','slp','sly', \
'vel_x', 'vel_fx', 'vel_y', 'vel_fy', 'vel_z', 'vel_fz', \
'NNslX','NNslY','NNslZ' ))
# Index for scrolling the data csv file
data_index = 0
while True:
# Variable to time the loop
current = time.time()
elapsed = 0
# Update joystick commands from UDP communication, order (roll, pitch, yaw, throttle)
#for channel in range(0, 4):
#rcCMD[channel] = int(udp.message[channel])
rcCMD[0] = int(data['Proll'][data_index])
rcCMD[1] = int(data['Ppitch'][data_index])
rcCMD[2] = int(data['Pyaw'][data_index])
rcCMD[3] = int(data['Pthrottle'][data_index])
# Coordinate map from Optitrack in the MAST Lab: X, Y, Z. NED: If going up, Z is negative.
######### WALL ########
#Door | x+ |
# | |
# | y+ |
#---------------------|
# y- | |
# | |
# x-| |
#######################
# Update current position of the vehicle
currentPos['x'] = data['x'][data_index] #udp.message[5]
currentPos['y'] = data['y'][data_index] #udp.message[6]
currentPos['z'] = data['z'][data_index] #-udp.message[7]
# Update position of the slung load
sl_currentPos['x'] = data['slx'][data_index] #udp.message[8]
sl_currentPos['y'] = data['slx'][data_index] #udp.message[9]
# Get velocities of the vehicle
velocities['x'], velocities['fx'] = vel_x.get_velocity(
currentPos['x'])
velocities['y'], velocities['fy'] = vel_y.get_velocity(
currentPos['y'])
velocities['z'], velocities['fz'] = vel_z.get_velocity(
currentPos['z'])
# Update vehicle Attitude
#vehicle.getData(MultiWii.ATTITUDE)
# Slung load PID calculation, the relative position of the vehicle vs the slung load
sl_xPIDvalue = slx_posPID.update(sl_currentPos['x'] -
currentPos['x'])
sl_yPIDvalue = sly_posPID.update(sl_currentPos['y'] -
currentPos['y'])
# Desired position changed using slung load movements
if int(data['Mode'][data_index]) == 1: #udp.message[4] == 1:
desiredPos['x'] = 0.0
desiredPos['y'] = 0.0
if int(data['Mode'][data_index]) == 2: #udp.message[4] == 2:
desiredPos['x'] = limit(f_desx.update(sl_xPIDvalue), -1.0, 1.0)
desiredPos['y'] = limit(f_desy.update(sl_yPIDvalue), -1.0, 1.0)
# Filter new values before using them
heading = f_yaw.update(data['Myaw'][data_index]) #udp.message[12])
# PID updating, Roll is for Y and Pitch for X, Z is negative
rPIDvalue = rollPID.update(desiredPos['y'] - currentPos['y'])
pPIDvalue = pitchPID.update(desiredPos['x'] - currentPos['x'])
hPIDvalue = heightPID.update(desiredPos['z'] - currentPos['z'])
yPIDvalue = yawPID.update(0.0 - heading)
# Heading must be in radians, MultiWii heading comes in degrees, optitrack in radians
sinYaw = sin(heading)
cosYaw = cos(heading)
# Conversion from desired accelerations to desired angle commands
desiredRoll = toPWM(
degrees((rPIDvalue * cosYaw + pPIDvalue * sinYaw) * (1 / g)),
1)
desiredPitch = toPWM(
degrees((pPIDvalue * cosYaw - rPIDvalue * sinYaw) * (1 / g)),
1)
desiredThrottle = ((hPIDvalue + g) * vehicle_weight) / (
cos(f_pitch.update(radians(data['Vroll'][data_index]))) *
cos(f_roll.update(radians(data['Vpitch'][data_index]))))
if int(data['Mode'][data_index]) == 2:
desiredThrottle = round((desiredThrottle / kt_sl) + u0, 0)
else:
desiredThrottle = round((desiredThrottle / kt) + u0, 0)
desiredYaw = round(1500 - (yPIDvalue * ky), 0)
# Limit commands for safety
if int(data['Mode'][data_index]) == 1:
rcCMD[0] = limit(desiredRoll, 1200, 1800)
rcCMD[1] = limit(desiredPitch, 1200, 1800)
rcCMD[2] = limit(desiredYaw, 1000, 2000)
rcCMD[3] = limit(desiredThrottle, 1000, 2000)
slx_posPID.resetIntegrator()
sly_posPID.resetIntegrator()
mode = 'Auto'
elif int(data['Mode'][data_index]) == 2:
#rcCMD[0] = limit(desiredRoll,1200,1800)
#rcCMD[1] = limit(desiredPitch,1200,1800)
#rcCMD[2] = limit(desiredYaw,1000,2000)
#rcCMD[3] = limit(desiredThrottle,1000,2000)
mode = 'SlungLoad'
else:
# Prevent integrators/derivators to increase if they are not in use
rollPID.resetIntegrator()
pitchPID.resetIntegrator()
heightPID.resetIntegrator()
yawPID.resetIntegrator()
slx_posPID.resetIntegrator()
sly_posPID.resetIntegrator()
mode = 'Manual'
rcCMD = [limit(n, 1000, 2000) for n in rcCMD]
# Neural network update
# Order of the inputs -> vehicle roll, vehicle pitch, vehicle yaw, x, y, z, roll, pitch, yaw, throttle
inputs = np.delete(inputs, 0,
1) # Delete the oldest element on the array
# Add new column of data to the front of the stack
inputs = np.column_stack((inputs, \
[data['Vroll'][data_index], data['Vpitch'][data_index], data['Vyaw'][data_index], \
currentPos['x'], currentPos['y'], currentPos['z'], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3] ]))
# Get output of neural network
outputs = prn.NNOut(inputs, net)
# Send commands to vehicle
#vehicle.sendCMD(8,MultiWii.SET_RAW_RC,rcCMD)
udp.message[0] = data['Proll'][data_index]
udp.message[1] = data['Ppitch'][data_index]
udp.message[2] = data['Pyaw'][data_index]
udp.message[3] = data['Pthrottle'][data_index]
udp.message[4] = data['Mode'][data_index]
udp.message[11] = data['Mroll'][data_index]
udp.message[13] = data['Mpitch'][data_index]
udp.message[12] = data['Myaw'][data_index]
udp.message[8] = data['slx'][data_index]
udp.message[9] = data['sly'][data_index]
udp.message[10] = data['slz'][data_index]
udp.message[14] = data['slr'][data_index]
udp.message[15] = data['slp'][data_index]
udp.message[16] = data['sly'][data_index]
row = (time.time(), \
data['Vroll'][data_index], data['Vpitch'][data_index], data['Vyaw'][data_index], \
udp.message[0], udp.message[1], udp.message[2], udp.message[3], \
currentPos['x'], currentPos['y'], currentPos['z'], desiredPos['x'], desiredPos['y'], desiredPos['z'], \
udp.message[11], udp.message[13], udp.message[12], \
udp.message[4], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
udp.message[8], udp.message[9], udp.message[10], udp.message[14],udp.message[15], udp.message[16], \
velocities['x'], velocities['fx'], velocities['y'], velocities['fy'], velocities['z'], velocities['fz'], \
# The output we care about is the latest....
outputs[0,memory-1], outputs[1,memory-1], outputs[2,memory-1] )
if logging:
logger.writerow(row)
if mode == 'Manual':
print "[%d] Mode: %s | X: %0.3f | Y: %0.3f | Z: %0.3f | Heading: %0.3f" % (
data_index, mode, currentPos['x'], currentPos['y'],
currentPos['z'], heading)
if mode == 'Auto':
print "[%d] Mode: %s | X: %0.3f | Y: %0.3f | Z: %0.3f" % (
data_index, mode, currentPos['x'], currentPos['y'],
currentPos['z'])
elif mode == 'SlungLoad':
print "[%d] Mode: %s | SL_X: %0.3f | SL_Y: %0.3f" % (
data_index, mode, sl_currentPos['x'], sl_currentPos['y'])
# Wait until the update_rate is completed
while elapsed < update_rate:
elapsed = time.time() - current
# Update data index and kill the thread when its done
data_index = data_index + 1
if data_index == data.shape[0]:
print "\n\nSimulation terminated!\n"
break
except Exception, error:
print "Error in control thread: " + str(error)
def control():
global vehicle, rcCMD
global rollPID, pitchPID, heightPID, yawPID
global desiredPos, currentPos, velocities
global desiredRoll, desiredPitch, desiredThrottle
global rPIDvalue, pPIDvalue, yPIDvalue
global f_yaw, f_pitch, f_roll, f_desx, f_desy
global vel_x, vel_y, vel_z
global sl_currentPos, sl_xPIDvalue, sl_yPIDvalue, slx_posPID, sly_posPID
# Start Neural network
print "Starting neural network..."
#net = prn.loadNN('modules/sl_rtrl_best.csv')
net = prn.loadNN('modules/networks/sl_rtrl_step_control.csv')
# Array of inputs to network
inputs = np.zeros((10,1))
print "Neural network active!"
while True:
if udp.active:
print "UDP server is active..."
break
else:
print "Waiting for UDP server to be active..."
time.sleep(0.5)
try:
if logging:
st = datetime.datetime.fromtimestamp(time.time()).strftime('%m_%d_%H-%M-%S')+".csv"
f = open("logs/mw-sl-neural-"+st, "w")
logger = csv.writer(f)
# V -> vehicle | P -> pilot (joystick) | D -> desired position
# M -> motion capture | C -> commanded controls | sl -> Second marker | Mode
logger.writerow(('timestamp','Vroll','Vpitch','Vyaw','Proll','Ppitch','Pyaw','Pthrottle', \
'x','y','z','Dx','Dy','Dz','Mroll','Mpitch','Myaw','Mode','Croll','Cpitch','Cyaw','Cthrottle', \
'slx','sly','slz','slr','slp','sly', \
'vel_x', 'vel_fx', 'vel_y', 'vel_fy', 'vel_z', 'vel_fz', \
'NNslX','NNslY' ))
while True:
# Variable to time the loop
current = time.time()
elapsed = 0
# Update joystick commands from UDP communication, order (roll, pitch, yaw, throttle)
for channel in range(0, 4):
rcCMD[channel] = int(udp.message[channel])
# Coordinate map from Optitrack in the MAST Lab: X, Y, Z. NED: If going up, Z is negative.
######### WALL ########
#Door | x+ |
# | |
# | y+ |
#---------------------|
# y- | |
# | |
# x-| |
#######################
# Update current position of the vehicle
currentPos['x'] = udp.message[5]
currentPos['y'] = udp.message[6]
currentPos['z'] = -udp.message[7]
# Get velocities of the vehicle
velocities['x'],velocities['fx'] = vel_x.get_velocity(currentPos['x'])
velocities['y'],velocities['fy'] = vel_y.get_velocity(currentPos['y'])
velocities['z'],velocities['fz'] = vel_z.get_velocity(currentPos['z'])
# Update vehicle Attitude
vehicle.getData(MultiWii.ATTITUDE)
# Neural network update
# Order of the inputs -> vehicle roll, vehicle pitch, vehicle yaw, x, y, z, roll, pitch, yaw, throttle
np.put(inputs, [0,1,2,3,4,5,6,7,8,9], \
[vehicle.attitude['angx'], vehicle.attitude['angy'], vehicle.attitude['heading'], \
currentPos['x'], currentPos['y'], currentPos['z'], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3] ])
# Get output of neural network
outputs = prn.NNOut(inputs,net)
# Update position of the slung load
#sl_currentPos['x'] = udp.message[8]
#sl_currentPos['y'] = udp.message[9]
sl_currentPos['x'] = outputs[0,0]
sl_currentPos['y'] = outputs[1,0]
# Slung load PID calculation, the relative position of the vehicle vs the slung load
sl_xPIDvalue = slx_posPID.update(sl_currentPos['x'] - currentPos['x'])
sl_yPIDvalue = sly_posPID.update(sl_currentPos['y'] - currentPos['y'])
# Desired position changed using slung load movements
if udp.message[4] == 1:
desiredPos['x'] = 0.0
desiredPos['y'] = 0.0
wp_index = 0
wp_step = 0
if udp.message[4] == 2:
# Aggressive step response
#desiredPos['x'] = 1.0
#desiredPos['y'] = 1.0
# Slung load control staying at 0,0 (normal with filter)
#desiredPos['x'] = limit(f_desx.update(sl_xPIDvalue), -1.0, 1.0)
#desiredPos['y'] = limit(f_desy.update(sl_yPIDvalue), -1.0, 1.0)
# Slung load control staying at 0,0 (normal with no filter)
#desiredPos['x'] = limit(sl_xPIDvalue, -1.0, 1.0)
#desiredPos['y'] = limit(sl_yPIDvalue, -1.0, 1.0)
# Slung load control aggressive step response
#desiredPos['x'] = 1.0 + limit(f_desx.update(sl_xPIDvalue), -2.0, 2.0)
#desiredPos['y'] = 1.0 + limit(f_desy.update(sl_yPIDvalue), -2.0, 2.0)
# Slung load control aggressive step response (no filter)
#desiredPos['x'] = 1.0 + limit(sl_xPIDvalue, -2.0, 2.0)
#desiredPos['y'] = 1.0 + limit(sl_yPIDvalue, -2.0, 2.0)
# Way-point control
if wp_step >= wp_time:
wp_step = 0
wp_index += 1 # to iterate on the WP list
if wp_index > len(wp)-1:
wp_index = 0
else:
desiredPos['x'] = wp[wp_index][0]
desiredPos['y'] = wp[wp_index][1]
wp_step += 1 # increase the time step to let time pass
# Filter new values before using them
heading = f_yaw.update(udp.message[12])
# PID updating, Roll is for Y and Pitch for X, Z is negative
rPIDvalue = rollPID.update( desiredPos['y'] - currentPos['y'])
pPIDvalue = pitchPID.update( desiredPos['x'] - currentPos['x'])
hPIDvalue = heightPID.update(desiredPos['z'] - currentPos['z'])
yPIDvalue = yawPID.update(0.0 - heading)
# Heading must be in radians, MultiWii heading comes in degrees, optitrack in radians
sinYaw = sin(heading)
cosYaw = cos(heading)
# Conversion from desired accelerations to desired angle commands
desiredRoll = toPWM(degrees( (rPIDvalue * cosYaw + pPIDvalue * sinYaw) * (1 / g) ),1)
desiredPitch = toPWM(degrees( (pPIDvalue * cosYaw - rPIDvalue * sinYaw) * (1 / g) ),1)
desiredThrottle = ((hPIDvalue + g) * vehicle_weight) / (cos(f_pitch.update(radians(vehicle.attitude['angx'])))*cos(f_roll.update(radians(vehicle.attitude['angy']))))
#if udp.message[4] == 2:
desiredThrottle = round((desiredThrottle / kt_sl) + u0, 0)
#else:
# desiredThrottle = round((desiredThrottle / kt) + u0, 0)
desiredYaw = round(1500 - (yPIDvalue * ky), 0)
# Limit commands for safety
if udp.message[4] == 1:
rcCMD[0] = limit(desiredRoll,1000,2000)
rcCMD[1] = limit(desiredPitch,1000,2000)
rcCMD[2] = limit(desiredYaw,1000,2000)
rcCMD[3] = limit(desiredThrottle,1000,2000)
slx_posPID.resetIntegrator()
sly_posPID.resetIntegrator()
mode = 'Auto'
elif udp.message[4] == 2:
rcCMD[0] = limit(desiredRoll,1000,2000)
rcCMD[1] = limit(desiredPitch,1000,2000)
rcCMD[2] = limit(desiredYaw,1000,2000)
rcCMD[3] = limit(desiredThrottle,1000,2000)
mode = 'SlungLoad'
else:
# Prevent integrators/derivators to increase if they are not in use
rollPID.resetIntegrator()
pitchPID.resetIntegrator()
heightPID.resetIntegrator()
yawPID.resetIntegrator()
slx_posPID.resetIntegrator()
sly_posPID.resetIntegrator()
mode = 'Manual'
rcCMD = [limit(n,1000,2000) for n in rcCMD]
# Send commands to vehicle
vehicle.sendCMD(8,MultiWii.SET_RAW_RC,rcCMD)
row = (time.time(), \
vehicle.attitude['angx'], vehicle.attitude['angy'], vehicle.attitude['heading'], \
udp.message[0], udp.message[1], udp.message[2], udp.message[3], \
currentPos['x'], currentPos['y'], currentPos['z'], desiredPos['x'], desiredPos['y'], desiredPos['z'], \
udp.message[11], udp.message[13], udp.message[12], \
udp.message[4], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
udp.message[8], udp.message[9], udp.message[10], udp.message[14],udp.message[15], udp.message[16], \
velocities['x'], velocities['fx'], velocities['y'], velocities['fy'], velocities['z'], velocities['fz'], \
outputs[0,0], outputs[1,0] )
if logging:
logger.writerow(row)
if mode == 'Manual':
print "Mode: %s | X: %0.3f | Y: %0.3f | Z: %0.3f | Heading: %0.3f" % (mode, currentPos['x'], currentPos['y'], currentPos['z'], heading)
if mode == 'Auto':
print "Mode: %s | X: %0.3f | Y: %0.3f | Z: %0.3f" % (mode, currentPos['x'], currentPos['y'], currentPos['z'])
elif mode == 'SlungLoad':
print "Mode: %s | WP: %d | SL_X: %0.3f | SL_Y: %0.3f" % (mode, wp_index, sl_currentPos['x'], sl_currentPos['y'])
# Wait until the update_rate is completed
while elapsed < update_rate:
elapsed = time.time() - current
except Exception,error:
print "Error in control thread: "+str(error)
def control():
global vehicle, rcCMD
global rollPID, pitchPID, heightPID, yawPID
global desiredPos, currentPos, velocities
global desiredRoll, desiredPitch, desiredThrottle
global rPIDvalue, pPIDvalue, yPIDvalue
global f_yaw, f_pitch, f_roll, f_desx, f_desy
global vel_x, vel_y, vel_z
# Start Neural network
print("Starting neural network...")
net = prn.loadNN('modules/networkNOV.csv')
# Array of inputs to network
inputs = np.zeros((10, 1))
print("Neural network active!")
while True:
if udp.active:
print("UDP server is active...")
break
else:
print("Waiting for UDP server to be active...")
time.sleep(0.5)
try:
if logging:
st = datetime.datetime.fromtimestamp(
time.time()).strftime('%m_%d_%H-%M-%S') + ".csv"
f = open("logs/mw-neural-" + st, "w")
logger = csv.writer(f)
# V -> vehicle | P -> pilot (joystick) | D -> desired position
# M -> motion capture | C -> commanded controls | sl -> Second marker | Mode
logger.writerow(('timestamp','Vroll','Vpitch','Vyaw','Proll','Ppitch','Pyaw','Pthrottle', \
'x','y','z','Dx','Dy','Dz','Mroll','Mpitch','Myaw','Mode','Croll','Cpitch','Cyaw','Cthrottle', \
'slx','sly','slz','slr','slp','sly', \
'vel_x', 'vel_fx', 'vel_y', 'vel_fy', 'vel_z', 'vel_fz', \
'NNroll','NNpitch','NNyaw','NNx','NNy','NNz' ))
while True:
# Variable to time the loop
current = time.time()
elapsed = 0
# Update joystick commands from UDP communication, order (roll, pitch, yaw, throttle)
for channel in range(0, 4):
rcCMD[channel] = int(udp.message[channel])
# Coordinate map from Optitrack in the MAST Lab: X, Y, Z. NED: If going up, Z is negative.
######### WALL ########
#Door | x+ |
# | |
# | y+ |
#---------------------|
# y- | |
# | |
# x-| |
#######################
# Update current position of the vehicle
currentPos['x'] = udp.message[5]
currentPos['y'] = udp.message[6]
currentPos['z'] = -udp.message[7]
# Get velocities of the vehicle
velocities['x'], velocities['fx'] = vel_x.get_velocity(
currentPos['x'])
velocities['y'], velocities['fy'] = vel_y.get_velocity(
currentPos['y'])
velocities['z'], velocities['fz'] = vel_z.get_velocity(
currentPos['z'])
# Update vehicle Attitude
vehicle.getData(MultiWii.ATTITUDE)
# Desired position changed using joystick movements
if udp.message[4] == 1:
desiredPos['x'] = radius
desiredPos['y'] = 0.0
trajectory_step = 0.0
if udp.message[4] == 2:
if trajectory == 'circle':
desiredPos['x'], desiredPos['y'] = circle_trajectory(
radius, w, trajectory_step)
trajectory_step += update_rate
elif trajectory == 'infinity':
desiredPos['x'], desiredPos['y'] = infinity_trajectory(
a, b, w, trajectory_step)
trajectory_step += update_rate
# Filter new values before using them
heading = f_yaw.update(udp.message[12])
# PID updating, Roll is for Y and Pitch for X, Z is negative
rPIDvalue = rollPID.update(desiredPos['y'] - currentPos['y'])
pPIDvalue = pitchPID.update(desiredPos['x'] - currentPos['x'])
hPIDvalue = heightPID.update(desiredPos['z'] - currentPos['z'])
yPIDvalue = yawPID.update(0.0 - heading)
# Heading must be in radians, MultiWii heading comes in degrees, optitrack in radians
sinYaw = sin(heading)
cosYaw = cos(heading)
# Conversion from desired accelerations to desired angle commands
desiredRoll = toPWM(
degrees((rPIDvalue * cosYaw + pPIDvalue * sinYaw) * (1 / g)),
1, 50)
desiredPitch = toPWM(
degrees((pPIDvalue * cosYaw - rPIDvalue * sinYaw) * (1 / g)),
1, 50)
desiredThrottle = ((hPIDvalue + g) * vehicle_weight) / (
cos(f_pitch.update(radians(vehicle.attitude['angx']))) *
cos(f_roll.update(radians(vehicle.attitude['angy']))))
desiredThrottle = round((desiredThrottle / kt) + u0, 0)
desiredYaw = round(1500 - (yPIDvalue * ky), 0)
# Limit commands for safety
if udp.message[4] == 1:
rcCMD[0] = limit(desiredRoll, 1200, 1800)
rcCMD[1] = limit(desiredPitch, 1200, 1800)
rcCMD[2] = limit(desiredYaw, 1000, 2000)
rcCMD[3] = limit(desiredThrottle, 1000, 2000)
mode = 'Hold'
elif udp.message[4] == 2:
rcCMD[0] = limit(desiredRoll, 1200, 1800)
rcCMD[1] = limit(desiredPitch, 1200, 1800)
rcCMD[2] = limit(desiredYaw, 1000, 2000)
rcCMD[3] = limit(desiredThrottle, 1000, 2000)
mode = 'Trajectory'
else:
# Prevent integrators to increase if they are not in use
rollPID.resetIntegrator()
pitchPID.resetIntegrator()
heightPID.resetIntegrator()
yawPID.resetIntegrator()
mode = 'Manual'
rcCMD = [limit(n, 1000, 2000) for n in rcCMD]
# Neural network update
# Order of the inputs -> roll, pitch, yaw, throttle, vehicle roll, vehicle pitch, motion capture yaw, x, y, z
np.put(inputs, [0,1,2,3,4,5,6,7,8,9], \
[rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
vehicle.attitude['angx'], vehicle.attitude['angy'], udp.message[9], \
currentPos['x'], currentPos['y'], currentPos['z'] ])
# Get output of neural network
outputs = prn.NNOut(inputs, net)
# Send commands to vehicle
vehicle.sendCMD(8, MultiWii.SET_RAW_RC, rcCMD)
row = (time.time(), \
vehicle.attitude['angx'], vehicle.attitude['angy'], vehicle.attitude['heading'], \
udp.message[0], udp.message[1], udp.message[2], udp.message[3], \
currentPos['x'], currentPos['y'], currentPos['z'], desiredPos['x'], desiredPos['y'], desiredPos['z'], \
udp.message[11], udp.message[13], udp.message[12], \
udp.message[4], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
udp.message[8], udp.message[9], udp.message[10], udp.message[14],udp.message[15], udp.message[16], \
velocities['x'], velocities['fx'], velocities['y'], velocities['fy'], velocities['z'], velocities['fz'], \
outputs[0,0], outputs[1,0], outputs[2,0], outputs[3,0], outputs[4,0], outputs[5,0] )
if logging:
logger.writerow(row)
if mode == 'Hold' or 'Manual':
print(
"Mode: %s | X: %0.2f | Y: %0.2f | Z: %0.2f" %
(mode, currentPos['x'], currentPos['y'], currentPos['z']))
elif mode == 'Trajectory':
print("Mode: %s | Des_X: %0.2f | Des_Y: %0.2f" %
(mode, desiredPos['x'], desiredPos['y']))
# Wait until the update_rate is completed
while elapsed < update_rate:
elapsed = time.time() - current
except Exception as error:
print("Error in control thread: " + str(error))
def control():
global vehicle, rcCMD
global rollPID, pitchPID, heightPID, yawPID
global desiredPos, currentPos
global desiredRoll, desiredPitch, desiredThrottle
global rPIDvalue, pPIDvalue, yPIDvalue
global f_yaw, f_pitch, f_roll
global ky
# Start Neural network
net = prn.loadNN('modules/network.csv')
# Array of inputs to network
inputs = np.zeros((10,1))
while True:
if udp.active:
print "UDP server is active..."
break
else:
print "Waiting for UDP server to be active..."
time.sleep(0.5)
try:
if logging:
st = datetime.datetime.fromtimestamp(time.time()).strftime('%m_%d_%H-%M-%S')+".csv"
f = open("logs/mw-"+st, "w")
logger = csv.writer(f)
# V -> vehicle | P -> pilot (joystick) | D -> desired position | M -> motion capture | C -> commanded controls | NN -> Neural Network prediction
logger.writerow(('timestamp','Vroll','Vpitch','Vyaw','Proll','Ppitch','Pyaw','Pthrottle', \
'x','y','z','Dx','Dy','Dz','Mroll','Mpitch','Myaw','Mode','Croll','Cpitch','Cyaw','Cthrottle', \
'NNroll','NNpitch','NNyaw','NNx','NNy','NNz'))
while True:
# Variable to time the loop
current = time.time()
elapsed = 0
# Update joystick commands from UDP communication, order (roll, pitch, yaw, throttle)
rcCMD[0] = udp.message[0]
rcCMD[1] = udp.message[1]
rcCMD[2] = udp.message[2]
rcCMD[3] = udp.message[3]
# Coordinate map from Optitrack in the MAST Lab: X, Y, Z. NED: If going up, Z is negative.
######### WALL ########
#Door | x+ |
# | |
# | y+ |
#---------------------|
# y- | |
# | |
# x-| |
#######################
# Update current position of the vehicle
currentPos['x'] = udp.message[5]
currentPos['y'] = udp.message[6]
currentPos['z'] = -udp.message[7]
# Update Attitude
vehicle.getData(MultiWii.ATTITUDE)
# Filter new values before using them
heading = f_yaw.update(udp.message[12])
# PID updating, Roll is for Y and Pitch for X, Z is negative
rPIDvalue = rollPID.update(desiredPos['y'] - currentPos['y'])
pPIDvalue = pitchPID.update(desiredPos['x'] - currentPos['x'])
hPIDvalue = heightPID.update(desiredPos['z'] - currentPos['z'])
yPIDvalue = yawPID.update(0.0 - heading)
# Heading must be in radians, MultiWii heading comes in degrees, optitrack in radians
sinYaw = sin(heading)
cosYaw = cos(heading)
# Conversion from desired accelerations to desired angle commands
desiredRoll = toPWM(degrees( (rPIDvalue * cosYaw + pPIDvalue * sinYaw) * (1 / g) ),1)
desiredPitch = toPWM(degrees( (pPIDvalue * cosYaw - rPIDvalue * sinYaw) * (1 / g) ),1)
desiredThrottle = ((hPIDvalue + g) * vehicle_weight) / (cos(f_pitch.update(radians(vehicle.attitude['angx'])))*cos(f_roll.update(radians(vehicle.attitude['angy']))))
desiredThrottle = (desiredThrottle / kt) + u0
desiredYaw = 1500 - (yPIDvalue * ky)
# Limit commands for safety
if udp.message[4] == 1:
rcCMD[0] = limit(desiredRoll,1000,2000)
rcCMD[1] = limit(desiredPitch,1000,2000)
rcCMD[2] = limit(desiredYaw,1000,2000)
rcCMD[3] = limit(desiredThrottle,1000,2000)
mode = 'Auto'
else:
# Prevent integrators/derivators to increase if they are not in use
rollPID.resetIntegrator()
pitchPID.resetIntegrator()
heightPID.resetIntegrator()
yawPID.resetIntegrator()
mode = 'Manual'
rcCMD = [limit(n,1000,2000) for n in rcCMD]
# Neural network update
# Order of the inputs -> roll, pitch, yaw, throttle, vehicle roll, vehicle pitch, motion capture yaw, x, y, z
np.put(inputs, [0,1,2,3,4,5,6,7,8,9], \
[rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
vehicle.attitude['angx'], vehicle.attitude['angy'], udp.message[9], \
currentPos['x'], currentPos['y'], currentPos['z'] ])
# Get output of neural network
outputs = prn.NNOut(inputs,net)
# Send commands to vehicle
vehicle.sendCMD(8,MultiWii.SET_RAW_RC,rcCMD)
row = (time.time(), \
vehicle.attitude['angx'], vehicle.attitude['angy'], vehicle.attitude['heading'], \
#vehicle.rawIMU['ax'], vehicle.rawIMU['ay'], vehicle.rawIMU['az'], vehicle.rawIMU['gx'], vehicle.rawIMU['gy'], vehicle.rawIMU['gz'], \
#vehicle.rcChannels['roll'], vehicle.rcChannels['pitch'], vehicle.rcChannels['throttle'], vehicle.rcChannels['yaw'], \
udp.message[0], udp.message[1], udp.message[2], udp.message[3], \
currentPos['x'], currentPos['y'], currentPos['z'], desiredPos['x'], desiredPos['y'], desiredPos['z'], \
udp.message[11], udp.message[12], udp.message[13], \
udp.message[7], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
outputs[0,0], outputs[1,0], outputs[2,0], outputs[3,0], outputs[4,0], outputs[5,0] )
if logging:
logger.writerow(row)
print "Mode: %s | Z: %0.3f | X: %0.3f | Y: %0.3f " % (mode, currentPos['z'], currentPos['x'], currentPos['y'])
#print "Mode: %s | heading: %0.3f | desiredYaw: %0.3f" % (mode, heading, desiredYaw)
# Wait until the update_rate is completed
while elapsed < update_rate:
elapsed = time.time() - current
except Exception,error:
print "Error in control thread: "+str(error)
def control():
global vehicle, rcCMD
global rollPID, pitchPID, heightPID, yawPID
global desiredPos, currentPos, velocities
global desiredRoll, desiredPitch, desiredThrottle
global rPIDvalue, pPIDvalue, yPIDvalue
global f_yaw, f_pitch, f_roll, f_desx, f_desy
global vel_x, vel_y, vel_z
global sl_currentPos, sl_xPIDvalue, sl_yPIDvalue, slx_posPID, sly_posPID
# Start Neural network
print "Starting neural network..."
#net = prn.loadNN('modules/sl_rtrl_best.csv')
net = prn.loadNN('modules/networks/sl_rtrl_step_control.csv')
# Array of inputs to network
inputs = np.zeros((10, 1))
print "Neural network active!"
while True:
if udp.active:
print "UDP server is active..."
break
else:
print "Waiting for UDP server to be active..."
time.sleep(0.5)
try:
if logging:
st = datetime.datetime.fromtimestamp(
time.time()).strftime('%m_%d_%H-%M-%S') + ".csv"
f = open("logs/mw-sl-neural-" + st, "w")
logger = csv.writer(f)
# V -> vehicle | P -> pilot (joystick) | D -> desired position
# M -> motion capture | C -> commanded controls | sl -> Second marker | Mode
logger.writerow(('timestamp','Vroll','Vpitch','Vyaw','Proll','Ppitch','Pyaw','Pthrottle', \
'x','y','z','Dx','Dy','Dz','Mroll','Mpitch','Myaw','Mode','Croll','Cpitch','Cyaw','Cthrottle', \
'slx','sly','slz','slr','slp','sly', \
'vel_x', 'vel_fx', 'vel_y', 'vel_fy', 'vel_z', 'vel_fz', \
'NNslX','NNslY' ))
while True:
# Variable to time the loop
current = time.time()
elapsed = 0
# Update joystick commands from UDP communication, order (roll, pitch, yaw, throttle)
for channel in range(0, 4):
rcCMD[channel] = int(udp.message[channel])
# Coordinate map from Optitrack in the MAST Lab: X, Y, Z. NED: If going up, Z is negative.
######### WALL ########
#Door | x+ |
# | |
# | y+ |
#---------------------|
# y- | |
# | |
# x-| |
#######################
# Update current position of the vehicle
currentPos['x'] = udp.message[5]
currentPos['y'] = udp.message[6]
currentPos['z'] = -udp.message[7]
# Get velocities of the vehicle
velocities['x'], velocities['fx'] = vel_x.get_velocity(
currentPos['x'])
velocities['y'], velocities['fy'] = vel_y.get_velocity(
currentPos['y'])
velocities['z'], velocities['fz'] = vel_z.get_velocity(
currentPos['z'])
# Update vehicle Attitude
vehicle.getData(MultiWii.ATTITUDE)
# Neural network update
# Order of the inputs -> vehicle roll, vehicle pitch, vehicle yaw, x, y, z, roll, pitch, yaw, throttle
np.put(inputs, [0,1,2,3,4,5,6,7,8,9], \
[vehicle.attitude['angx'], vehicle.attitude['angy'], vehicle.attitude['heading'], \
currentPos['x'], currentPos['y'], currentPos['z'], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3] ])
# Get output of neural network
outputs = prn.NNOut(inputs, net)
# Update position of the slung load
#sl_currentPos['x'] = udp.message[8]
#sl_currentPos['y'] = udp.message[9]
sl_currentPos['x'] = outputs[0, 0]
sl_currentPos['y'] = outputs[1, 0]
# Slung load PID calculation, the relative position of the vehicle vs the slung load
sl_xPIDvalue = slx_posPID.update(sl_currentPos['x'] -
currentPos['x'])
sl_yPIDvalue = sly_posPID.update(sl_currentPos['y'] -
currentPos['y'])
# Desired position changed using slung load movements
if udp.message[4] == 1:
desiredPos['x'] = 0.0
desiredPos['y'] = 0.0
wp_index = 0
wp_step = 0
if udp.message[4] == 2:
# Aggressive step response
#desiredPos['x'] = 1.0
#desiredPos['y'] = 1.0
# Slung load control staying at 0,0 (normal with filter)
#desiredPos['x'] = limit(f_desx.update(sl_xPIDvalue), -1.0, 1.0)
#desiredPos['y'] = limit(f_desy.update(sl_yPIDvalue), -1.0, 1.0)
# Slung load control staying at 0,0 (normal with no filter)
#desiredPos['x'] = limit(sl_xPIDvalue, -1.0, 1.0)
#desiredPos['y'] = limit(sl_yPIDvalue, -1.0, 1.0)
# Slung load control aggressive step response
#desiredPos['x'] = 1.0 + limit(f_desx.update(sl_xPIDvalue), -2.0, 2.0)
#desiredPos['y'] = 1.0 + limit(f_desy.update(sl_yPIDvalue), -2.0, 2.0)
# Slung load control aggressive step response (no filter)
#desiredPos['x'] = 1.0 + limit(sl_xPIDvalue, -2.0, 2.0)
#desiredPos['y'] = 1.0 + limit(sl_yPIDvalue, -2.0, 2.0)
# Way-point control
if wp_step >= wp_time:
wp_step = 0
wp_index += 1 # to iterate on the WP list
if wp_index > len(wp) - 1:
wp_index = 0
else:
desiredPos['x'] = wp[wp_index][0]
desiredPos['y'] = wp[wp_index][1]
wp_step += 1 # increase the time step to let time pass
# Filter new values before using them
heading = f_yaw.update(udp.message[12])
# PID updating, Roll is for Y and Pitch for X, Z is negative
rPIDvalue = rollPID.update(desiredPos['y'] - currentPos['y'])
pPIDvalue = pitchPID.update(desiredPos['x'] - currentPos['x'])
hPIDvalue = heightPID.update(desiredPos['z'] - currentPos['z'])
yPIDvalue = yawPID.update(0.0 - heading)
# Heading must be in radians, MultiWii heading comes in degrees, optitrack in radians
sinYaw = sin(heading)
cosYaw = cos(heading)
# Conversion from desired accelerations to desired angle commands
desiredRoll = toPWM(
degrees((rPIDvalue * cosYaw + pPIDvalue * sinYaw) * (1 / g)),
1)
desiredPitch = toPWM(
degrees((pPIDvalue * cosYaw - rPIDvalue * sinYaw) * (1 / g)),
1)
desiredThrottle = ((hPIDvalue + g) * vehicle_weight) / (
cos(f_pitch.update(radians(vehicle.attitude['angx']))) *
cos(f_roll.update(radians(vehicle.attitude['angy']))))
#if udp.message[4] == 2:
desiredThrottle = round((desiredThrottle / kt_sl) + u0, 0)
#else:
# desiredThrottle = round((desiredThrottle / kt) + u0, 0)
desiredYaw = round(1500 - (yPIDvalue * ky), 0)
# Limit commands for safety
if udp.message[4] == 1:
rcCMD[0] = limit(desiredRoll, 1000, 2000)
rcCMD[1] = limit(desiredPitch, 1000, 2000)
rcCMD[2] = limit(desiredYaw, 1000, 2000)
rcCMD[3] = limit(desiredThrottle, 1000, 2000)
slx_posPID.resetIntegrator()
sly_posPID.resetIntegrator()
mode = 'Auto'
elif udp.message[4] == 2:
rcCMD[0] = limit(desiredRoll, 1000, 2000)
rcCMD[1] = limit(desiredPitch, 1000, 2000)
rcCMD[2] = limit(desiredYaw, 1000, 2000)
rcCMD[3] = limit(desiredThrottle, 1000, 2000)
mode = 'SlungLoad'
else:
# Prevent integrators/derivators to increase if they are not in use
rollPID.resetIntegrator()
pitchPID.resetIntegrator()
heightPID.resetIntegrator()
yawPID.resetIntegrator()
slx_posPID.resetIntegrator()
sly_posPID.resetIntegrator()
mode = 'Manual'
rcCMD = [limit(n, 1000, 2000) for n in rcCMD]
# Send commands to vehicle
vehicle.sendCMD(8, MultiWii.SET_RAW_RC, rcCMD)
row = (time.time(), \
vehicle.attitude['angx'], vehicle.attitude['angy'], vehicle.attitude['heading'], \
udp.message[0], udp.message[1], udp.message[2], udp.message[3], \
currentPos['x'], currentPos['y'], currentPos['z'], desiredPos['x'], desiredPos['y'], desiredPos['z'], \
udp.message[11], udp.message[13], udp.message[12], \
udp.message[4], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
udp.message[8], udp.message[9], udp.message[10], udp.message[14],udp.message[15], udp.message[16], \
velocities['x'], velocities['fx'], velocities['y'], velocities['fy'], velocities['z'], velocities['fz'], \
outputs[0,0], outputs[1,0] )
if logging:
logger.writerow(row)
if mode == 'Manual':
print "Mode: %s | X: %0.3f | Y: %0.3f | Z: %0.3f | Heading: %0.3f" % (
mode, currentPos['x'], currentPos['y'], currentPos['z'],
heading)
if mode == 'Auto':
print "Mode: %s | X: %0.3f | Y: %0.3f | Z: %0.3f" % (
mode, currentPos['x'], currentPos['y'], currentPos['z'])
elif mode == 'SlungLoad':
print "Mode: %s | WP: %d | SL_X: %0.3f | SL_Y: %0.3f" % (
mode, wp_index, sl_currentPos['x'], sl_currentPos['y'])
# Wait until the update_rate is completed
while elapsed < update_rate:
elapsed = time.time() - current
except Exception, error:
print "Error in control thread: " + str(error)
def control():
global vehicle, rcCMD
global rollPID, pitchPID, heightPID, yawPID
global desiredPos, currentPos, velocities
global desiredRoll, desiredPitch, desiredThrottle
global rPIDvalue, pPIDvalue, yPIDvalue
global f_yaw, f_pitch, f_roll, f_desx, f_desy
global vel_x, vel_y, vel_z
# Start Neural network
print "Starting neural network..."
net = prn.loadNN('modules/networkNOV.csv')
# Array of inputs to network
inputs = np.zeros((10,1))
print "Neural network active!"
while True:
if udp.active:
print "UDP server is active..."
break
else:
print "Waiting for UDP server to be active..."
time.sleep(0.5)
try:
if logging:
st = datetime.datetime.fromtimestamp(time.time()).strftime('%m_%d_%H-%M-%S')+".csv"
f = open("logs/mw-neural-"+st, "w")
logger = csv.writer(f)
# V -> vehicle | P -> pilot (joystick) | D -> desired position
# M -> motion capture | C -> commanded controls | sl -> Second marker | Mode
logger.writerow(('timestamp','Vroll','Vpitch','Vyaw','Proll','Ppitch','Pyaw','Pthrottle', \
'x','y','z','Dx','Dy','Dz','Mroll','Mpitch','Myaw','Mode','Croll','Cpitch','Cyaw','Cthrottle', \
'slx','sly','slz','slr','slp','sly', \
'vel_x', 'vel_fx', 'vel_y', 'vel_fy', 'vel_z', 'vel_fz', \
'NNroll','NNpitch','NNyaw','NNx','NNy','NNz' ))
while True:
# Variable to time the loop
current = time.time()
elapsed = 0
# Update joystick commands from UDP communication, order (roll, pitch, yaw, throttle)
for channel in range(0, 4):
rcCMD[channel] = int(udp.message[channel])
# Coordinate map from Optitrack in the MAST Lab: X, Y, Z. NED: If going up, Z is negative.
######### WALL ########
#Door | x+ |
# | |
# | y+ |
#---------------------|
# y- | |
# | |
# x-| |
#######################
# Update current position of the vehicle
currentPos['x'] = udp.message[5]
currentPos['y'] = udp.message[6]
currentPos['z'] = -udp.message[7]
# Get velocities of the vehicle
velocities['x'],velocities['fx'] = vel_x.get_velocity(currentPos['x'])
velocities['y'],velocities['fy'] = vel_y.get_velocity(currentPos['y'])
velocities['z'],velocities['fz'] = vel_z.get_velocity(currentPos['z'])
# Update vehicle Attitude
vehicle.getData(MultiWii.ATTITUDE)
# Desired position changed using joystick movements
if udp.message[4] == 1:
desiredPos['x'] = radius
desiredPos['y'] = 0.0
trajectory_step = 0.0
if udp.message[4] == 2:
if trajectory == 'circle':
desiredPos['x'], desiredPos['y'] = circle_trajectory(radius, w, trajectory_step)
trajectory_step += update_rate
elif trajectory == 'infinity':
desiredPos['x'], desiredPos['y'] = infinity_trajectory(a, b, w, trajectory_step)
trajectory_step += update_rate
# Filter new values before using them
heading = f_yaw.update(udp.message[12])
# PID updating, Roll is for Y and Pitch for X, Z is negative
rPIDvalue = rollPID.update(desiredPos['y'] - currentPos['y'])
pPIDvalue = pitchPID.update(desiredPos['x'] - currentPos['x'])
hPIDvalue = heightPID.update(desiredPos['z'] - currentPos['z'])
yPIDvalue = yawPID.update(0.0 - heading)
# Heading must be in radians, MultiWii heading comes in degrees, optitrack in radians
sinYaw = sin(heading)
cosYaw = cos(heading)
# Conversion from desired accelerations to desired angle commands
desiredRoll = toPWM(degrees( (rPIDvalue * cosYaw + pPIDvalue * sinYaw) * (1 / g) ), 1, 50)
desiredPitch = toPWM(degrees( (pPIDvalue * cosYaw - rPIDvalue * sinYaw) * (1 / g) ), 1, 50)
desiredThrottle = ((hPIDvalue + g) * vehicle_weight) / (cos(f_pitch.update(radians(vehicle.attitude['angx'])))*cos(f_roll.update(radians(vehicle.attitude['angy']))))
desiredThrottle = round((desiredThrottle / kt) + u0 ,0)
desiredYaw = round(1500 - (yPIDvalue * ky) ,0)
# Limit commands for safety
if udp.message[4] == 1:
rcCMD[0] = limit(desiredRoll,1200,1800)
rcCMD[1] = limit(desiredPitch,1200,1800)
rcCMD[2] = limit(desiredYaw,1000,2000)
rcCMD[3] = limit(desiredThrottle,1000,2000)
mode = 'Hold'
elif udp.message[4] == 2:
rcCMD[0] = limit(desiredRoll,1200,1800)
rcCMD[1] = limit(desiredPitch,1200,1800)
rcCMD[2] = limit(desiredYaw,1000,2000)
rcCMD[3] = limit(desiredThrottle,1000,2000)
mode = 'Trajectory'
else:
# Prevent integrators to increase if they are not in use
rollPID.resetIntegrator()
pitchPID.resetIntegrator()
heightPID.resetIntegrator()
yawPID.resetIntegrator()
mode = 'Manual'
rcCMD = [limit(n,1000,2000) for n in rcCMD]
# Neural network update
# Order of the inputs -> roll, pitch, yaw, throttle, vehicle roll, vehicle pitch, motion capture yaw, x, y, z
np.put(inputs, [0,1,2,3,4,5,6,7,8,9], \
[rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
vehicle.attitude['angx'], vehicle.attitude['angy'], udp.message[9], \
currentPos['x'], currentPos['y'], currentPos['z'] ])
# Get output of neural network
outputs = prn.NNOut(inputs,net)
# Send commands to vehicle
vehicle.sendCMD(8,MultiWii.SET_RAW_RC,rcCMD)
row = (time.time(), \
vehicle.attitude['angx'], vehicle.attitude['angy'], vehicle.attitude['heading'], \
udp.message[0], udp.message[1], udp.message[2], udp.message[3], \
currentPos['x'], currentPos['y'], currentPos['z'], desiredPos['x'], desiredPos['y'], desiredPos['z'], \
udp.message[11], udp.message[13], udp.message[12], \
udp.message[4], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
udp.message[8], udp.message[9], udp.message[10], udp.message[14],udp.message[15], udp.message[16], \
velocities['x'], velocities['fx'], velocities['y'], velocities['fy'], velocities['z'], velocities['fz'], \
outputs[0,0], outputs[1,0], outputs[2,0], outputs[3,0], outputs[4,0], outputs[5,0] )
if logging:
logger.writerow(row)
if mode == 'Hold' or 'Manual':
print "Mode: %s | X: %0.2f | Y: %0.2f | Z: %0.2f" % (mode, currentPos['x'], currentPos['y'], currentPos['z'])
elif mode == 'Trajectory':
print "Mode: %s | Des_X: %0.2f | Des_Y: %0.2f" % (mode, desiredPos['x'], desiredPos['y'])
# Wait until the update_rate is completed
while elapsed < update_rate:
elapsed = time.time() - current
except Exception,error:
print "Error in control thread: "+str(error)
def control():
global vehicle, rcCMD
global rollPID, pitchPID, heightPID, yawPID
global desiredPos, currentPos, velocities
global desiredRoll, desiredPitch, desiredThrottle
global rPIDvalue, pPIDvalue, yPIDvalue
global f_yaw, f_pitch, f_roll, f_desx, f_desy
global vel_x, vel_y, vel_z
global sl_currentPos, sl_xPIDvalue, sl_yPIDvalue, slx_posPID, sly_posPID
# Start simulation messages
print "\n\nStarting simulation!!\n"
print "Simulation size -> %d" % (data.shape[0])
print "Estimated completion time -> %0.2f minutes" % ((data.shape[0]*update_rate)/60)
time.sleep(3)
# Start Neural network
network = 'modules/networks/sl_rtrl_step_control.csv'
print "Starting \'%s\' neural network..." % (network)
net = prn.loadNN(network)
# Array of inputs to network
memory = 100
inputs = np.zeros((10,memory))
print "Neural network active!"
time.sleep(1)
#while True:
# if udp.active:
# print "UDP server is active..."
# break
# else:
# print "Waiting for UDP server to be active..."
# time.sleep(0.5)
try:
if logging:
st = datetime.datetime.fromtimestamp(time.time()).strftime('%m_%d_%H-%M-%S')+".csv"
f = open("logs/ground-test-"+st, "w")
logger = csv.writer(f)
# V -> vehicle | P -> pilot (joystick) | D -> desired position
# M -> motion capture | C -> commanded controls | sl -> Second marker | Mode
logger.writerow(('timestamp','Vroll','Vpitch','Vyaw','Proll','Ppitch','Pyaw','Pthrottle', \
'x','y','z','Dx','Dy','Dz','Mroll','Mpitch','Myaw','Mode','Croll','Cpitch','Cyaw','Cthrottle', \
'slx','sly','slz','slr','slp','sly', \
'vel_x', 'vel_fx', 'vel_y', 'vel_fy', 'vel_z', 'vel_fz', \
'NNslX','NNslY','NNslZ' ))
# Index for scrolling the data csv file
data_index = 0
while True:
# Variable to time the loop
current = time.time()
elapsed = 0
# Update joystick commands from UDP communication, order (roll, pitch, yaw, throttle)
#for channel in range(0, 4):
#rcCMD[channel] = int(udp.message[channel])
rcCMD[0] = int(data['Proll'][data_index])
rcCMD[1] = int(data['Ppitch'][data_index])
rcCMD[2] = int(data['Pyaw'][data_index])
rcCMD[3] = int(data['Pthrottle'][data_index])
# Coordinate map from Optitrack in the MAST Lab: X, Y, Z. NED: If going up, Z is negative.
######### WALL ########
#Door | x+ |
# | |
# | y+ |
#---------------------|
# y- | |
# | |
# x-| |
#######################
# Update current position of the vehicle
currentPos['x'] = data['x'][data_index]#udp.message[5]
currentPos['y'] = data['y'][data_index]#udp.message[6]
currentPos['z'] = data['z'][data_index]#-udp.message[7]
# Update position of the slung load
sl_currentPos['x'] = data['slx'][data_index]#udp.message[8]
sl_currentPos['y'] = data['slx'][data_index]#udp.message[9]
# Get velocities of the vehicle
velocities['x'],velocities['fx'] = vel_x.get_velocity(currentPos['x'])
velocities['y'],velocities['fy'] = vel_y.get_velocity(currentPos['y'])
velocities['z'],velocities['fz'] = vel_z.get_velocity(currentPos['z'])
# Update vehicle Attitude
#vehicle.getData(MultiWii.ATTITUDE)
# Slung load PID calculation, the relative position of the vehicle vs the slung load
sl_xPIDvalue = slx_posPID.update(sl_currentPos['x'] - currentPos['x'])
sl_yPIDvalue = sly_posPID.update(sl_currentPos['y'] - currentPos['y'])
# Desired position changed using slung load movements
if int(data['Mode'][data_index]) == 1:#udp.message[4] == 1:
desiredPos['x'] = 0.0
desiredPos['y'] = 0.0
if int(data['Mode'][data_index]) == 2:#udp.message[4] == 2:
desiredPos['x'] = limit(f_desx.update(sl_xPIDvalue), -1.0, 1.0)
desiredPos['y'] = limit(f_desy.update(sl_yPIDvalue), -1.0, 1.0)
# Filter new values before using them
heading = f_yaw.update(data['Myaw'][data_index])#udp.message[12])
# PID updating, Roll is for Y and Pitch for X, Z is negative
rPIDvalue = rollPID.update( desiredPos['y'] - currentPos['y'])
pPIDvalue = pitchPID.update( desiredPos['x'] - currentPos['x'])
hPIDvalue = heightPID.update(desiredPos['z'] - currentPos['z'])
yPIDvalue = yawPID.update(0.0 - heading)
# Heading must be in radians, MultiWii heading comes in degrees, optitrack in radians
sinYaw = sin(heading)
cosYaw = cos(heading)
# Conversion from desired accelerations to desired angle commands
desiredRoll = toPWM(degrees( (rPIDvalue * cosYaw + pPIDvalue * sinYaw) * (1 / g) ),1)
desiredPitch = toPWM(degrees( (pPIDvalue * cosYaw - rPIDvalue * sinYaw) * (1 / g) ),1)
desiredThrottle = ((hPIDvalue + g) * vehicle_weight) / (cos(f_pitch.update(radians(data['Vroll'][data_index])))*cos(f_roll.update(radians(data['Vpitch'][data_index]))))
if int(data['Mode'][data_index]) == 2:
desiredThrottle = round((desiredThrottle / kt_sl) + u0, 0)
else:
desiredThrottle = round((desiredThrottle / kt) + u0, 0)
desiredYaw = round(1500 - (yPIDvalue * ky), 0)
# Limit commands for safety
if int(data['Mode'][data_index]) == 1:
rcCMD[0] = limit(desiredRoll,1200,1800)
rcCMD[1] = limit(desiredPitch,1200,1800)
rcCMD[2] = limit(desiredYaw,1000,2000)
rcCMD[3] = limit(desiredThrottle,1000,2000)
slx_posPID.resetIntegrator()
sly_posPID.resetIntegrator()
mode = 'Auto'
elif int(data['Mode'][data_index]) == 2:
#rcCMD[0] = limit(desiredRoll,1200,1800)
#rcCMD[1] = limit(desiredPitch,1200,1800)
#rcCMD[2] = limit(desiredYaw,1000,2000)
#rcCMD[3] = limit(desiredThrottle,1000,2000)
mode = 'SlungLoad'
else:
# Prevent integrators/derivators to increase if they are not in use
rollPID.resetIntegrator()
pitchPID.resetIntegrator()
heightPID.resetIntegrator()
yawPID.resetIntegrator()
slx_posPID.resetIntegrator()
sly_posPID.resetIntegrator()
mode = 'Manual'
rcCMD = [limit(n,1000,2000) for n in rcCMD]
# Neural network update
# Order of the inputs -> vehicle roll, vehicle pitch, vehicle yaw, x, y, z, roll, pitch, yaw, throttle
inputs = np.delete(inputs,0,1) # Delete the oldest element on the array
# Add new column of data to the front of the stack
inputs = np.column_stack((inputs, \
[data['Vroll'][data_index], data['Vpitch'][data_index], data['Vyaw'][data_index], \
currentPos['x'], currentPos['y'], currentPos['z'], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3] ]))
# Get output of neural network
outputs = prn.NNOut(inputs,net)
# Send commands to vehicle
#vehicle.sendCMD(8,MultiWii.SET_RAW_RC,rcCMD)
udp.message[0] = data['Proll'][data_index]
udp.message[1] = data['Ppitch'][data_index]
udp.message[2] = data['Pyaw'][data_index]
udp.message[3] = data['Pthrottle'][data_index]
udp.message[4] = data['Mode'][data_index]
udp.message[11] = data['Mroll'][data_index]
udp.message[13] = data['Mpitch'][data_index]
udp.message[12] = data['Myaw'][data_index]
udp.message[8] = data['slx'][data_index]
udp.message[9] = data['sly'][data_index]
udp.message[10] = data['slz'][data_index]
udp.message[14] = data['slr'][data_index]
udp.message[15] = data['slp'][data_index]
udp.message[16] = data['sly'][data_index]
row = (time.time(), \
data['Vroll'][data_index], data['Vpitch'][data_index], data['Vyaw'][data_index], \
udp.message[0], udp.message[1], udp.message[2], udp.message[3], \
currentPos['x'], currentPos['y'], currentPos['z'], desiredPos['x'], desiredPos['y'], desiredPos['z'], \
udp.message[11], udp.message[13], udp.message[12], \
udp.message[4], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
udp.message[8], udp.message[9], udp.message[10], udp.message[14],udp.message[15], udp.message[16], \
velocities['x'], velocities['fx'], velocities['y'], velocities['fy'], velocities['z'], velocities['fz'], \
# The output we care about is the latest....
outputs[0,memory-1], outputs[1,memory-1], outputs[2,memory-1] )
if logging:
logger.writerow(row)
if mode == 'Manual':
print "[%d] Mode: %s | X: %0.3f | Y: %0.3f | Z: %0.3f | Heading: %0.3f" % (data_index, mode, currentPos['x'], currentPos['y'], currentPos['z'], heading)
if mode == 'Auto':
print "[%d] Mode: %s | X: %0.3f | Y: %0.3f | Z: %0.3f" % (data_index, mode, currentPos['x'], currentPos['y'], currentPos['z'])
elif mode == 'SlungLoad':
print "[%d] Mode: %s | SL_X: %0.3f | SL_Y: %0.3f" % (data_index, mode, sl_currentPos['x'], sl_currentPos['y'])
# Wait until the update_rate is completed
while elapsed < update_rate:
elapsed = time.time() - current
# Update data index and kill the thread when its done
data_index = data_index + 1
if data_index == data.shape[0]:
print "\n\nSimulation terminated!\n"
break
except Exception,error:
print "Error in control thread: "+str(error)
def control():
global vehicle, rcCMD
global rollPID, pitchPID, heightPID, yawPID
global desiredPos, currentPos
global desiredRoll, desiredPitch, desiredThrottle
global rPIDvalue, pPIDvalue, yPIDvalue
global f_yaw, f_pitch, f_roll
global ky
# Start Neural network
net = prn.loadNN('modules/network.csv')
# Array of inputs to network
inputs = np.zeros((10, 1))
while True:
if udp.active:
print "UDP server is active..."
break
else:
print "Waiting for UDP server to be active..."
time.sleep(0.5)
try:
if logging:
st = datetime.datetime.fromtimestamp(
time.time()).strftime('%m_%d_%H-%M-%S') + ".csv"
f = open("logs/mw-" + st, "w")
logger = csv.writer(f)
# V -> vehicle | P -> pilot (joystick) | D -> desired position | M -> motion capture | C -> commanded controls | NN -> Neural Network prediction
logger.writerow(('timestamp','Vroll','Vpitch','Vyaw','Proll','Ppitch','Pyaw','Pthrottle', \
'x','y','z','Dx','Dy','Dz','Mroll','Mpitch','Myaw','Mode','Croll','Cpitch','Cyaw','Cthrottle', \
'NNroll','NNpitch','NNyaw','NNx','NNy','NNz'))
while True:
# Variable to time the loop
current = time.time()
elapsed = 0
# Update joystick commands from UDP communication, order (roll, pitch, yaw, throttle)
rcCMD[0] = udp.message[0]
rcCMD[1] = udp.message[1]
rcCMD[2] = udp.message[2]
rcCMD[3] = udp.message[3]
# Coordinate map from Optitrack in the MAST Lab: X, Y, Z. NED: If going up, Z is negative.
######### WALL ########
#Door | x+ |
# | |
# | y+ |
#---------------------|
# y- | |
# | |
# x-| |
#######################
# Update current position of the vehicle
currentPos['x'] = udp.message[5]
currentPos['y'] = udp.message[6]
currentPos['z'] = -udp.message[7]
# Update Attitude
vehicle.getData(MultiWii.ATTITUDE)
# Filter new values before using them
heading = f_yaw.update(udp.message[12])
# PID updating, Roll is for Y and Pitch for X, Z is negative
rPIDvalue = rollPID.update(desiredPos['y'] - currentPos['y'])
pPIDvalue = pitchPID.update(desiredPos['x'] - currentPos['x'])
hPIDvalue = heightPID.update(desiredPos['z'] - currentPos['z'])
yPIDvalue = yawPID.update(0.0 - heading)
# Heading must be in radians, MultiWii heading comes in degrees, optitrack in radians
sinYaw = sin(heading)
cosYaw = cos(heading)
# Conversion from desired accelerations to desired angle commands
desiredRoll = toPWM(
degrees((rPIDvalue * cosYaw + pPIDvalue * sinYaw) * (1 / g)),
1)
desiredPitch = toPWM(
degrees((pPIDvalue * cosYaw - rPIDvalue * sinYaw) * (1 / g)),
1)
desiredThrottle = ((hPIDvalue + g) * vehicle_weight) / (
cos(f_pitch.update(radians(vehicle.attitude['angx']))) *
cos(f_roll.update(radians(vehicle.attitude['angy']))))
desiredThrottle = (desiredThrottle / kt) + u0
desiredYaw = 1500 - (yPIDvalue * ky)
# Limit commands for safety
if udp.message[4] == 1:
rcCMD[0] = limit(desiredRoll, 1000, 2000)
rcCMD[1] = limit(desiredPitch, 1000, 2000)
rcCMD[2] = limit(desiredYaw, 1000, 2000)
rcCMD[3] = limit(desiredThrottle, 1000, 2000)
mode = 'Auto'
else:
# Prevent integrators/derivators to increase if they are not in use
rollPID.resetIntegrator()
pitchPID.resetIntegrator()
heightPID.resetIntegrator()
yawPID.resetIntegrator()
mode = 'Manual'
rcCMD = [limit(n, 1000, 2000) for n in rcCMD]
# Neural network update
# Order of the inputs -> roll, pitch, yaw, throttle, vehicle roll, vehicle pitch, motion capture yaw, x, y, z
np.put(inputs, [0,1,2,3,4,5,6,7,8,9], \
[rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
vehicle.attitude['angx'], vehicle.attitude['angy'], udp.message[9], \
currentPos['x'], currentPos['y'], currentPos['z'] ])
# Get output of neural network
outputs = prn.NNOut(inputs, net)
# Send commands to vehicle
vehicle.sendCMD(8, MultiWii.SET_RAW_RC, rcCMD)
row = (time.time(), \
vehicle.attitude['angx'], vehicle.attitude['angy'], vehicle.attitude['heading'], \
#vehicle.rawIMU['ax'], vehicle.rawIMU['ay'], vehicle.rawIMU['az'], vehicle.rawIMU['gx'], vehicle.rawIMU['gy'], vehicle.rawIMU['gz'], \
#vehicle.rcChannels['roll'], vehicle.rcChannels['pitch'], vehicle.rcChannels['throttle'], vehicle.rcChannels['yaw'], \
udp.message[0], udp.message[1], udp.message[2], udp.message[3], \
currentPos['x'], currentPos['y'], currentPos['z'], desiredPos['x'], desiredPos['y'], desiredPos['z'], \
udp.message[11], udp.message[12], udp.message[13], \
udp.message[7], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
outputs[0,0], outputs[1,0], outputs[2,0], outputs[3,0], outputs[4,0], outputs[5,0] )
if logging:
logger.writerow(row)
print "Mode: %s | Z: %0.3f | X: %0.3f | Y: %0.3f " % (
mode, currentPos['z'], currentPos['x'], currentPos['y'])
#print "Mode: %s | heading: %0.3f | desiredYaw: %0.3f" % (mode, heading, desiredYaw)
# Wait until the update_rate is completed
while elapsed < update_rate:
elapsed = time.time() - current
except Exception, error:
print "Error in control thread: " + str(error)