roboroach._set_np(1)
print("Initialized")
##########################
####### CALIBRATION
##########################
print("Starting calibration... (press <Ctrl-C> to finish)")
ref_pos = []
try:
while True:
pos = polaris_driver.getPositionFromBX("1801")
if not 'miss' in pos:
ref_pos.append(pos)
time.sleep(0.1);
except KeyboardInterrupt:
pass
ref_pos = np.mean(ref_pos[1:], axis=0)
print("\nPosition: ["+str(ref_pos[0])+", "+str(ref_pos[1])+", "+str(ref_pos[2])+"]")
print("Calibrated")
##########################
####### CONTROL
##########################
print("Line follower started! (press <Ctrl-C> to finish)")
POS_THRESH_POS = 1.5
POS_THRESH_NEG = -1.5
GAIN_MAX = 250
GAIN_MIN = 150
roboroach._set_np(1)
print("Initialized")
##########################
####### CALIBRATION
##########################
print("Starting path acquisition... (press <Ctrl-C> to finish)")
ref_pos = np.array([])
try:
while True:
pos = polaris_driver.getPositionFromBX("1801")
if not 'miss' in pos:
ref_pos = np.vstack([ref_pos, pos])
time.sleep(0.1);
except KeyboardInterrupt:
pass
print("Path acquired!")
##########################
####### CONTROL
##########################
print("Path follower started! (press <Ctrl-C> to finish)")
pos_vec = np.array([0, 0, 0])
i = 0
try:
while i < len(pos)-1
ref_vec = [pos[i], pos[i+1]]
