def run_main(self):
args = self.get_args()
args_ = {}
if args.pin is None:
raise Exception("Servo pin required")
else:
try:
args_['servo_pin'] = int(args.pin)
except:
raise Exception("Valid servo pin required")
self.fpw = FilterPosWheel(**args_)
camera = PicamImpl()
camera.set_save_directory('tests', 'filter_pos')
while not self.exit_threads:
progress("\
n: rotate to next filter\n\
p: rotate to previous filter\n\
a: capture image\n\
e: exit\n\
filter id, >0 : rotate to filter no.\n\
")
try:
c = input()
if c == "n":
self.logger.log_info("Rotate to next filter...")
self.fpw.rotate_to_next()
self.logger.log_info("Filter reached: %d" %
self.fpw.filter_id)
elif c == "p":
self.logger.log_info("Rotate to previous filter...")
self.fpw.rotate_to_prev()
self.logger.log_info("Filter reached: %d" %
self.fpw.filter_id)
elif c == "e":
self.logger.log_info("Exiting...")
break
elif c == "a":
self.logger.log_info("Capturing image...")
camera.capture_single()
else:
try:
fid = int(c)
self.logger.log_info("Rotate to filter %d..." % fid)
self.fpw.rotate_to_filter(fid)
self.logger.log_info("Filter reached: %d" %
self.fpw.filter_id)
except:
self.logger.log_info("Got keyboard input %s" % c)
except Exception as e:
self.logger.log_debug(e)
def set_service_state(service, state):
call = service_file[service]
if int(state) == 0:
msg = 'Stopping ' + service
call = 'sudo systemctl stop ' + call
else:
msg = 'Starting ' + service
call = 'sudo systemctl restart ' + call
utils.progress(msg)
os.system(call)
return '', 204
def __init__(self, **kwargs):
super().__init__(**kwargs)
self.rb = RedisBridge(db=self.rd_cfg.databases['instruments'])
self.keys = self.rd_cfg.generate_flat_keys('instruments')
self.conn = mavutil.mavlink_connection(
self.cfg.redis_to_ap,
autoreconnect = True,
source_system = 1,
source_component = 93,
baud=self.cfg.connection_baudrate,
force_connected=True
)
self.lock = threading.Lock()
default_msg_hz = 30.0
msg_hz = {
'send_vision_position_estimate': 30.0,
'send_obstacle_distance': 15.0
}
self.mavlink_thread = threading.Thread(target=self.mavlink_loop, args=[self.conn])
self.mavlink_thread.start()
self.sched = BackgroundScheduler()
logging.getLogger('apscheduler').setLevel(logging.ERROR)
self.data = {}
for k, v in self.keys.items():
try:
if v in msg_hz.keys():
seconds = 1.0/msg_hz[v]
else:
seconds = 1.0/default_msg_hz
func = getattr(self, v)
self.sched.add_job(self.send_message,
'interval',
seconds=seconds,
args=[func, k],
max_instances=1
)
except:
utils.progress(traceback)
else:
self.data[k] = None
def run_main(self):
args = self.get_args()
args_ = {}
if args.pin is None:
raise Exception("Servo pin required")
else:
args_['servo_pin'] = args.pin
if args.filters is None:
raise Exception("Filter count required")
else:
args_['filter_count'] = args.filters
if args.c_thresh:
args_['c_threshold'] = args.c_thresh
# else:
# args_['c_threshold'] = 1000
if args.c_thresh_var:
args_['c_threshold_var'] = args.c_thresh_var
# else:
# args_['c_threshold'] = 50
fcw = FilterContWheel(**args_)
if args.speed:
speed = args.speed
else:
speed = 0.5
camera = PicamImpl()
camera.set_save_directory('tests', 'filter_cont')
while not self.exit_threads:
progress("====================================\n\
n: rotate to next filter\n\
r: rotate at current set speed\n\
s: stop rotation\n\
w: increase speed (max 1)\n\
q: decrease speed (min -1)\n\
d: check if notch is detected\n\
c: calibrate c threshold\n\
a: capture image\n\
e: exit\n\
filter id, >0 : rotate to filter no.\n\
====================================\n\
")
try:
c = input()
if c == "n":
self.logger.log_info("Rotate to next filter...")
fcw.rotate_to_next(speed)
self.logger.log_info("Filter reached: %d" % fcw.filter_id)
elif c == "r":
self.logger.log_info("Rotate at speed %f" % speed)
fcw.rotate(speed)
elif c == "s":
fcw.halt()
self.logger.log_info("Rotation stopped")
elif c == "w":
if speed < 1.0:
speed += 0.1
self.logger.log_info("Speed set to %f" % speed)
elif c == "q":
if speed > -1.0:
speed -= 0.1
self.logger.log_info("Speed set to %f" % speed)
elif c == "d":
self.logger.log_info("Check notch detected...")
detected = str(fcw.is_notch_detected())
self.logger.log_info("Notch detected: %s" % detected)
elif c == "c":
self.logger.log_info("Calibrating c threshold...")
a = fcw.detect_c_threshold()
self.logger.log_info("C threshold found: %d" %
fcw.c_threshold)
self.logger.log_info("C threshold found: %s" % str(a))
elif c == "a":
self.logger.log_info("Capturing image...")
camera.capture_single()
elif c == "e":
self.logger.log_info("Exiting...")
break
else:
try:
fid = int(c)
except:
self.logger.log_info("Got keyboard input %s" % c)
else:
self.logger.log_info("Rotate to filter %d..." % fid)
fcw.rotate_to_filter(fid)
self.logger.log_info("Filter reached: %d" %
fcw.filter_id)
except Exception as e:
self.logger.log_debug(e)