for key in fx.keys():
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()
parser.add_argument('-w', '--which', help='nma, fft, peaks...')
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) +