def __del__(self):
serial.cleanup()
self.rr.close()
print("Finished cleaning up myself.")
def calibrate_droplet_straight(self, direction, fName=None):
alpha = 1. #reflection coefficient
gamma = 2. #expansion coefficient
rho = -0.5 #contraction coefficient
sigma = 0.5 #shrink coefficient
x_0 = np.array([0,0])
x_1 = np.array([100,-100])
x_2 = np.array([-100,100])
spx = np.array([x_0, x_1, x_2])
old_spx = np.zeros((3,2))
if fName is None:
settings_history_fName = 'dmc_data_%s.csv'%("cw" if cw_q else "ccw")
simplex_history_fName = 'dmc_data_%s.csv'%("cw" if cw_q else "ccw")
else:
settings_history_fName = fName + "_%s_hist.csv"%(direction)
simplex_history_fName = fName + "_%s_spx.csv"%(direction)
settings_history_file = open(settings_history_fName,'w')
settings_history_file.write('mot0set, mot1set, mot2set, radius, sign, distance_traveled\n')
simplex_history_file = open(simplex_history_fName,'w')
simplex_history_file.write('spx00, spx01, spx02, rad0, spx10, spx11, spx12, rad1, spx20, spx21, spx22, rad2, spx30, spx31, spx32, rad3\n')
finished = False
def fun(x):
(radius, travel_dir, dist_traveled) =self.test_straight_settings(x, direction, settings_history_file)
return -radius - travel_dir*dist_traveled
try:
while(True):
spx = np.array(sorted(spx, key=fun))
if str(spx) == str(old_spx):
finished = True
old_spx = copy.deepcopy(spx)
func_values = [fun(point) for point in spx]
output_array = np.array(map(lambda spx_point, radius: np.append(spx_point, radius), spx, func_values)).flatten().tolist()
simplex_history_file.write(",".join(map(str, output_array))+"\n")
simplex_history_file.flush()
print("spx: %s"%(str(spx)))
x_o = np.mean(spx[:-1],0).astype(int)
x_r = (x_o + alpha*(x_o - spx[-1])).astype(int)
if (fun(x_r)<fun(spx[-2])) and (fun(x_r)>=fun(spx[0])):
print("Reflecting.")
spx[-1] = x_r
elif fun(x_r)<fun(spx[0]):
x_e = (x_o + gamma*(x_o-spx[-1])).astype(int)
if fun(x_e)<fun(x_r):
print("Expanding.")
spx[-1] = x_e
else:
print("Reflecting.")
spx[-1] = x_r
else:
x_c = (x_o + rho*(x_o-spx[-1])).astype(int)
if fun(x_c)<fun(spx[-1]):
print("Contracting.")
spx[-1] = x_c
else:
for i in range(1,3):
spx[i] = (spx[0] + sigma*(spx[i]-spx[0])).astype(int)
if finished:
set_motors(i, *spx[0])
print("We're done straightening direction %d"%direction)
settings_history_file.close()
simplex_history_file.close()
return cw_q
except KeyboardInterrupt:
print("Calibration interrupted by user.")
settings_history_file.close()
simplex_history_file.close()
serial.cleanup()
def calibrate_droplet_straight(self, direction, fName=None):
alpha = 1. #reflection coefficient
gamma = 2. #expansion coefficient
rho = -0.5 #contraction coefficient
sigma = 0.5 #shrink coefficient
x_0 = np.array([0, 0])
x_1 = np.array([100, -100])
x_2 = np.array([-100, 100])
spx = np.array([x_0, x_1, x_2])
old_spx = np.zeros((3, 2))
if fName is None:
settings_history_fName = 'dmc_data_%s.csv' % ("cw"
if cw_q else "ccw")
simplex_history_fName = 'dmc_data_%s.csv' % ("cw"
if cw_q else "ccw")
else:
settings_history_fName = fName + "_%s_hist.csv" % (direction)
simplex_history_fName = fName + "_%s_spx.csv" % (direction)
settings_history_file = open(settings_history_fName, 'w')
settings_history_file.write(
'mot0set, mot1set, mot2set, radius, sign, distance_traveled\n')
simplex_history_file = open(simplex_history_fName, 'w')
simplex_history_file.write(
'spx00, spx01, spx02, rad0, spx10, spx11, spx12, rad1, spx20, spx21, spx22, rad2, spx30, spx31, spx32, rad3\n'
)
finished = False
def fun(x):
(radius, travel_dir, dist_traveled) = self.test_straight_settings(
x, direction, settings_history_file)
return -radius - travel_dir * dist_traveled
try:
while (True):
spx = np.array(sorted(spx, key=fun))
if str(spx) == str(old_spx):
finished = True
old_spx = copy.deepcopy(spx)
func_values = [fun(point) for point in spx]
output_array = np.array(
map(lambda spx_point, radius: np.append(spx_point, radius),
spx, func_values)).flatten().tolist()
simplex_history_file.write(",".join(map(str, output_array)) +
"\n")
simplex_history_file.flush()
print("spx: %s" % (str(spx)))
x_o = np.mean(spx[:-1], 0).astype(int)
x_r = (x_o + alpha * (x_o - spx[-1])).astype(int)
if (fun(x_r) < fun(spx[-2])) and (fun(x_r) >= fun(spx[0])):
print("Reflecting.")
spx[-1] = x_r
elif fun(x_r) < fun(spx[0]):
x_e = (x_o + gamma * (x_o - spx[-1])).astype(int)
if fun(x_e) < fun(x_r):
print("Expanding.")
spx[-1] = x_e
else:
print("Reflecting.")
spx[-1] = x_r
else:
x_c = (x_o + rho * (x_o - spx[-1])).astype(int)
if fun(x_c) < fun(spx[-1]):
print("Contracting.")
spx[-1] = x_c
else:
for i in range(1, 3):
spx[i] = (spx[0] + sigma *
(spx[i] - spx[0])).astype(int)
if finished:
set_motors(i, *spx[0])
print("We're done straightening direction %d" % direction)
settings_history_file.close()
simplex_history_file.close()
return cw_q
except KeyboardInterrupt:
print("Calibration interrupted by user.")
settings_history_file.close()
simplex_history_file.close()
serial.cleanup()
def __del__(self):
serial.cleanup()
self.rr.close()
print("Finished cleaning up myself.")
#count = lineCount
now = 0
def process_and_transmit_line(line):
byte_strs = [str(line[i])+str(line[i+1]) for i in range(1,len(line)-1,2)]
byte_sum=0
for b in byte_strs:
byte_sum+=int(b,16)
byte_sum-=int(byte_strs[-1],16)
if(format(0xFF&((0xFF^(byte_sum & 0xFF))+1),'02X') != byte_strs[-1]):
print("checksum mismatch?")
print("\t" + format(0xFF&((0xFF^(byte_sum & 0xFF))+1),'02X'))
print("\t" + byte_strs[-1])
dat = ''.join([chr(int(b,16)) for b in byte_strs])
#print(dat)
Serial.write(dat)
try:
with open(hexFilePath,"r") as f:
for line in f:
if((time.time()-now)>programTime):
while not Serial.check_chars():
1+1 #do nothing
now=time.time()
process_and_transmit_line(line)
except(KeyboardInterrupt, SystemExit):
Serial.cleanup()
raise
#print(time.time()-now)
Serial.cleanup()
