def initialize_experiment_values():
def get_kinematic_values(data):
line = data.decode('UTF-8')
toks = line.split(',')
if ((len(toks) < 24) | (toks[0] != "FT")):
('Bad read')
continue
posx = float(toks[15])*3
posy = -float(toks[16])*3
net_vel = float(toks[19])*3
heading = float(toks[17])
heading = -heading
motorHeadingMap = heading % (2*np.pi)
propHead = motorHeadingMap/(2*np.pi)
target = int(propHead*800)
correctedTarget, mult = convert_angle_for_arduino(target, previousAngle, mult)
previousAngle, motorSendVal = correctedTarget, correctedTarget
motorLastTime = time.time()
return motorSendVal, motorLastTime
def close_and_clean(RPI_SOCK,RPI_HOST, RPI_PORT):
SENDSTRING = '<'+ '{},{},{},{},{},{},{}'.format(0, 'a', 0.0, 0.0, 0.0, 0.0, 0.0) +'>\n'
RPI_SOCK.sendto(str.encode('{}'.format(SENDSTRING)), (RPI_HOST, RPI_PORT))
RPI_SOCK.close()
sys.exit()
def run_fictrac_client(LOCAL_HOST, LOCAL_PORT, RPI_HOST, RPI_PORT, odor_on_time, odor_off_time, n_cycles):
RPI_SOCK = establish_pi_connection(RPI_HOST, RPI_HOST)
logger = initialize_logger()
with socket(AF_INET, SOCK_STREAM) as sock:
time.sleep(0.02)
sock.connect((LOCAL_HOST, LOCAL_PORT))
mfc1_sp, mfc2_sp, mfc3_sp = 0.0, 0.0, 0.0
led1_sp, led2_sp = 0.0, 0.0
motorSendVal, lastMotorSendVal = 800000, 800000
expStartTime = time.time()
LEDLastTime, odorLastTime, motorLastTime = expStartTime,expStartTime,expStartTime
previousAngle = 800000
mult = 1000
odorOn=False
odorOff=False
odorActivationCount = 0
try:
i = 0
while True:
data = sock.recv(1024)
if not data:
break
if (time.time() - motorLastTime) > (1/60000):
led1_sp, led2_sp = 0.0, 0.0
mfc1_sp = 0.0#empty
motorSendVal, motorLastTime = get_kinematic_values(data)
if odorOn==False and odorOff==False:
odorOn==True
lastOdorStartTime = time.time()
if odorOn==True and (time.time()-lastOdorStart)<odor_on_time:
pass
if odorOn==True and (time.time()-lastOdorStart)>=odor_on_time:
odorOn=False
odorOff=True
lastOdorStopTime = time.time()
odorActivationCount+=1
if odorOff==True and (time.time()-lastOdorStopTime)<odor_off_time:
pass
if odorOff==True and (time.time()-lastOdorStopTime)>=odor_off_time:
odorOn=True
odorOff=False
lastOdorStartTime = time.time()
if odorOn:
mfc2_sp = (float(config.FLOW_RATE) / 666.0)*(float(config.PERCENT_AIR)/100.0) #air
mfc3_sp = (float(config.FLOW_RATE) / 666.0)*(float(config.PERCENT_ACV)/100.0) #acv
if odorOff:
mfc2_sp = 0.0 #air
mfc3_sp = 0.0 #acv
SENDSTRING = '<'+ '{},{},{},{},{},{},{}'.format(1,motorSendVal, mfc1_sp, mfc2_sp, mfc3_sp, led1_sp, led2_sp) +'>\n'
RPI_SOCK.sendto(str.encode('{}'.format(SENDSTRING)), ("192.168.137.10", 5000))
logger.info('{} -- {}, {}, {}, {}, {}, {}, {}, {}, {}'.format((datetime.now()).strftime("%m/%d/%Y-%H:%M:%S.%f"),heading, posx,posy, motorSendVal, mfc1_sp, mfc2_sp, mfc3_sp, led1_sp, led2_sp))
if odorActivationCount > n_cycles:
close_and_clean(RPI_SOCK,RPI_HOST, RPI_PORT)
except KeyboardInterrupt:
close_and_clean(RPI_SOCK,RPI_HOST, RPI_PORT)
if __name__=='__main__':
odor_on_time, odor_off_time, n_cycles = parse_arguments()
config.check_cfg()
LOCAL_HOST, LOCAL_PORT = config.LOCAL_HOST, config.LOCAL_PORT
RPI_HOST, RPI_PORT = config.RPI_HOST, config.RPI_PORT
run_fictrac_client(LOCAL_HOST, LOCAL_PORT, RPI_HOST, RPI_PORT, odor_on_time, odor_off_time, n_cycles)
SENDSTRING = '<' + '{},{},{},{},{},{},{}'.format(
1, motorSendVal, mfc1_sp, mfc2_sp, mfc3_sp,
led1_sp, led2_sp) + '>\n'
RPI_SOCK.sendto(str.encode('{}'.format(SENDSTRING)),
("192.168.137.10", 5000))
now = datetime.now()
dts = now.strftime("%m/%d/%Y-%H:%M:%S.%f")
logger.info(
"{} -- {},{},{},{},{},{},{},{},{},{},{},{},{},{}".
format(dts, correctedTarget, mfc1_sp, mfc2_sp,
mfc3_sp, led1_sp, led2_sp, posx, posy,
ft_heading, ft_frame, ft_error, ft_roll,
ft_pitch, ft_yaw))
except KeyboardInterrupt:
SENDSTRING = '<' + '{},{},{},{},{},{},{}'.format(
0, 'a', mfc1_sp, mfc2_sp, mfc3_sp, led1_sp, led2_sp) + '>\n'
RPI_SOCK.sendto(str.encode('{}'.format(SENDSTRING)),
(RPI_HOST, RPI_PORT))
RPI_SOCK.close()
if __name__ == '__main__':
config.check_cfg()
LOCAL_HOST, LOCAL_PORT = config.LOCAL_HOST, config.LOCAL_PORT
RPI_HOST, RPI_PORT = config.RPI_HOST, config.RPI_PORT
run_fictrac_client(LOCAL_HOST, LOCAL_PORT, RPI_HOST, RPI_PORT)
#