x0j-=0.0001
dp5 = (f1-f0)/(x1i-x0i)
d1p5= (f2-f1)/(x2i-x1i)
dx = (x2j-x0j)/2
val = (d1p5-dp5)/dx
return val
def calculate_H(t,rob,ser):
nums = len(ser)-1
H = np.zeros((nums,nums))
for i in range(len(H)):
for j in range(len(H[i])):
H[i][j] = double_diff(rob['x'],ser[str(i)],ser[str(j)],t)
return H
servo_data = create_servo_dict('average_random.csv')
test = fft(servo_data['0'])
N = len(test)
tot = 0
for t in range(len(servo_data['t']))[1:]:
tot+=servo_data['t'][t]-servo_data['t'][t-1]
T = tot/len(servo_data['t'])
xf = np.linspace(0.0,1.0/(2.0*T),N//2)
servoFFT = 2.0/N*np.abs(test[:N//2])
servoPer = []
for i in range(len(xf))[1:-2]:
val = double_diff(servoFFT,xf,xf,i)
hi = xf[i]
if val<0 and 1000<(1/hi)<4000:
#freqs.append(hi)
#plt.axvline(x=hi)
fxd[key] = []
#fx[key] = differentiate(robot_data[key],robot_data['t'])
for i in range(len(fx['t']))[1:]:
if key!='t':
fxd[key].append((fx[key][i]-fx[key][i-1])/(fx['t'][i]-fx['t'][i-1]))
else:
fxd['t'].append((fx['t'][i]+fx['t'][i-1])/2)
return fxd
parser = argparse.ArgumentParser()
parser.add_argument('-c','--camera',help='Camera data file to convert')
parser.add_argument('-s','--servo',help='Servo data file from Arduino')
args = parser.parse_args()
robot_data = createXYangT(args.camera)
servo_data = create_servo_dict(args.servo)
robot_vel = differentiate(robot_data)
servo_vel = differentiate(servo_data)
robot_acc = differentiate(robot_vel)
servo_acc = differentiate(servo_vel)
#print robot_acc
#print servo_acc
ax1 = plt.subplot()
ax2 = ax1.twinx()
ax1.plot(robot_data['t'],robot_data['x'])
#ax2.plot(servo_acc['t'],servo_acc['3'],'r-')
ax2.plot(servo_data['t'],servo_data['3'],'r-')
#ax2.plot(servo_data['t'],servo_data['3'],'r-')
plt.show()
def find_percentile(alist,percent):
args = parser.parse_args()
start = 1500
amp = 140
THE_FORWARDS = {}
names = ['first', 'second', 'third', 'fourth', 'fifth']
fig = plt.figure(figsize=(12, 6))
plt.rc('axes', labelsize=16)
for nam in names:
rob_str = args.which + '/robot_data/*' + nam + '*'
ser_str = args.which + '/servo_data/*' + nam + '*'
rob_str = glob.glob(rob_str)[0]
ser_str = glob.glob(ser_str)[0]
#print rob_str, ser_str
robot_data = createXYangT(rob_str)
servo_data = create_servo_dict(ser_str)
numServos = len(servo_data.keys()) - 1
#servo_new = extrapolate(robot_data,servo_data)
#servo_new = extrapolate(robot_data,servo_data)
#print robot_data
#robot_new = extrapolate(servo_data,robot_data)
robot_diff = {'forwards': [0], 'sideways': [0]}
for t in range(len(robot_data['t']))[1:]:
x0 = robot_data['x'][t - 1]
x1 = robot_data['x'][t]
y0 = robot_data['y'][t - 1]
y1 = robot_data['y'][t]
phi = np.arctan2(y1 - y0, x1 - x0) - robot_data['ang'][t - 1]
hyp = np.sqrt((y1 - y0)**2 + (x1 - x0)**2)
robot_diff['forwards'].append(hyp * np.cos(phi) +
robot_diff['forwards'][t - 1])
