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])