import serial
import os
from geometry_msgs.msg import Twist
import rospy
import numpy as np
import matplotlib.pyplot as plt
import time
from PIL import Image
import matplotlib.colors as colors
import logging
import timeit
rospy.init_node('droneControlFinal', anonymous=True)
velocity_publisher = rospy.Publisher('beepldeep', Twist, queue_size=10)
vel_msg = Twist()
#TODO (updated 3/07/2017):
# -The program keeps getting slower as time goes on.
#inputs to the program:
filename_arg = "C1R2Milled_Z50N_Y10N_508.txt"
comport = "/dev/ttyUSB0"
comport2 = "COM6"
#global variables
average = 0
counter = 1
oldtime = time.time()
CapCounter = 0
frameID = 0
ROW_NUM = 6; #match these with the arduino ones
COL_NUM = 6;
Cap_Matrix = np.zeros((ROW_NUM, COL_NUM))
Cap_Calib_Matrix = np.zeros((ROW_NUM, COL_NUM))
graph_Min = 0;
graph_Max = 2.5;
CALIB_RUN = 1
distance = 0;
rangeC = 4
evaluation = 0
logging.basicConfig(filename=filename_arg, level=logging.DEBUG, format='%(relativeCreated)d, %(message)s')
def clipper(inp, max, min):
res = inp
if (inp>max):
res = max
if (inp<min):
res = min
return res
def cap_auton_plotter():
global Cap_Matrix
global Cap_Calib_Matrix
global CapCounter
global CALIB_RUN
global distance
global frameID
CapCounter+=1
if (CALIB_RUN ):
CALIB_RUN = 0
Cap_Calib_Matrix = Cap_Matrix
print "calibrated"
print Cap_Calib_Matrix
Cap_Matrix = np.subtract(Cap_Matrix,Cap_Calib_Matrix)
tempMatrix = Cap_Matrix.reshape((1,ROW_NUM*COL_NUM))
if evaluation ==1:
ser2.write('X')
streamedVal = ser2.readline()
print streamedVal
f = 14
try:
if type(streamedVal) == str:
f = eval(streamedVal)
else:
f= streamedVal
except NameError:
print "Something went wrong with this value Maybe a very high force was applied entering 0N. Message: "
print streamedVal
print f
frameID+=1
tempMatrix = np.append(frameID, tempMatrix)
tempMatrix = np.append(f, tempMatrix)
print frameID
print Cap_Matrix
logging.debug(', '.join(map(str,tempMatrix)))
CapCounter =0
#plt.axis([0, 10, 0, 1])
plt.ion()
counter = 0
ser = serial.Serial(comport, 9600, timeout=1)
if evaluation == 1:
ser2 = serial.Serial(comport2, 9600, timeout=1)
time.sleep(2)
ser.write('2')
Cap_Matrix = np.zeros((ROW_NUM, COL_NUM)) #Stores data in NanoFarads
while 1:
counter+=1
line = ser.readline()
#print line
splitted = line.split(',')
if len(splitted) != COL_NUM+1:
continue
if counter <ROW_NUM*2+1: #skip the first 8 readings-----------------------------------------------------------
continue
row = int(splitted[0])
Cap_Matrix[row-1,:] = np.array(splitted[1:], np.int32)
#print Cap_Matrix
if counter%ROW_NUM == 0: #skip the first 8 readings
if CALIB_RUN:
CALIB_RUN = 0
Cap_Calib_Matrix = Cap_Matrix
print "calibrated"
print Cap_Calib_Matrix
Cap_Matrix = np.subtract(Cap_Matrix,Cap_Calib_Matrix)
print Cap_Matrix[0:4, 0:5]
Cap_Matrix_MOD = Cap_Matrix[0:3,0:4]
a = Cap_Matrix_MOD[0, 0]
b = Cap_Matrix_MOD[0, 1]
c = Cap_Matrix_MOD[0, 2]
d = Cap_Matrix_MOD[0, 3]
e = Cap_Matrix_MOD[1, 0]
f = Cap_Matrix_MOD[1, 1]
g = Cap_Matrix_MOD[1, 2]
h = Cap_Matrix_MOD[1, 3]
i = Cap_Matrix_MOD[2, 0]
j = Cap_Matrix_MOD[2, 1]
k = Cap_Matrix_MOD[2, 2]
l = Cap_Matrix_MOD[2, 3]
r1_avg = (a+b+c+d)/4
r2_avg = (e+f+g+h)/4
r3_avg = (i+j+k+l)/4
r_avg = (r1_avg+r2_avg+r3_avg)/3
pitch=1
if r3_avg>r1_avg and r3_avg>r2_avg:
pitch='backwards'
elif r2_avg>r1_avg and r2_avg>r3_avg:
pitch='neutral'
# elif r3_avg<5 and r2_avg<5 and r1_avg<5:
# pitch='neutral'
else:
pitch='forwards'
c1_avg = (a+e+i)/3
c2_avg = (b+f+j)/3
c3_avg = (c+g+k)/3
c4_avg = (d+h+l)/3
c_avg = (c1_avg+c2_avg+c3_avg+c4_avg)/4
roll_right = (c3_avg+c4_avg)/2
roll_left = (c1_avg+c2_avg)/2
roll_neutral = (c2_avg+c3_avg)/2
roll=1
if roll_right>roll_left and roll_right>roll_neutral:
roll = 'right'
elif roll_neutral>roll_right and roll_neutral>roll_left:
roll = 'neutral'
#elif roll_neutral<5 and roll_right<5 and roll_left<5:
# roll = 'neutral'
else:
roll = 'left'
cmd_roll = 0
cmd_pitch = 0
if roll == 'right':
cmd_roll = 0.4
elif roll == 'left':
cmd_roll = -0.4
else:
cmd_roll = 0
if pitch == 'forwards':
cmd_pitch = 0.4
elif pitch == 'backwards':
cmd_pitch = -0.4
else:
cmd_pitch = 0
vel_msg.linear.x = cmd_pitch
vel_msg.linear.y = cmd_roll
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 0
velocity_publisher.publish(vel_msg)
#print vel_msg.linear.x, ', ',vel_msg.linear.y
print "row 1 average: ",r1_avg,"\nrow 2 average: ",r2_avg,"\nrow 3 average: ",r3_avg,'\nroll right: ', roll_right, '\nneutral: ',roll_neutral, '\nroll left: ',roll_left,'\nroll: ', roll,'\npitch: ',pitch
#print CAP_Matrix_MOD[0:4, 0:5]
Comments