"""CameraX Arduino box rotator controller. Usage: python rotator.py --ch --dir --debug """ import argparse import logging import time import pyudev import serial ARDUINO_ANGLE_MAX = 180 # degrees ARDUINO_BAUD_RATE = 9600 CMD_TEST = ['\x01', '\x02', '\x03'] # test hex command to send to Arduino CMD_WORD_LENGTH = 3 # bytes HS755HB_ANGLE_MAX = 202.0 # degrees NUM_TRYS = 3 # number of tries to get serial port to communicate properly START_BYTE_SERVO = 255 def get_cmdline_args(): """Get command line arguements.""" parser = argparse.ArgumentParser() parser.add_argument('--ch', help='--ch [1:6]', default=1, required=True, type=int, choices=[1, 2, 3, 4, 5, 6]) parser.add_argument('--dir', help='--dir OFF|ON', required=True, type=str, choices=['ON', 'OFF']) parser.add_argument('--offset', help='--offset #', default=0, type=int) parser.add_argument( '--debug', help='--debug', default=False, action='store_true') args = parser.parse_args() # determine angle for rotation from ON|OFF direction angle = 0 # default OFF if args.dir == 'ON': angle = 90 return args.ch, angle, args.offset, args.debug def serial_port_def(name): """Determine the serial port and open. Args: name: Device to locate (ie. 'Arduino') Returns: Serial port object """ devices = pyudev.Context() for device in devices.list_devices(subsystem='tty', ID_BUS='usb'): if name in device['ID_VENDOR']: arduino_port = device['DEVNAME'] continue logging.info('Using port %s', arduino_port) return serial.Serial(arduino_port, ARDUINO_BAUD_RATE, timeout=1) def read_command(port): """Read back command from serial port.""" cmd = [] for _ in range(CMD_WORD_LENGTH): cmd.append(port.read()) return cmd def send_command(port, cmd): """Send command to serial port.""" for i in range(CMD_WORD_LENGTH): port.write(cmd[i]) time.sleep(2.0 * CMD_WORD_LENGTH / ARDUINO_BAUD_RATE) # round trip return read_command(port) def establish_serial_comm(port): """Establish connection with serial port.""" logging.info('Establishing communication with %s', port.name) attempts = 0 hex_test = convert_to_hex(CMD_TEST) logging.debug(' test tx: %s %s %s', hex_test[0], hex_test[1], hex_test[2]) while attempts < NUM_TRYS: cmd_read = send_command(port, CMD_TEST) hex_read = convert_to_hex(cmd_read) logging.debug(' test rx: %s %s %s', hex_read[0], hex_read[1], hex_read[2]) if cmd_read != CMD_TEST: attempts += 1 else: logging.debug('Arduino comm established after %d attempt(s)', attempts) break def convert_to_hex(cmd): return [x.encode('hex') for x in cmd] def rotate_servo(servo, angle, port): """Rotate servo to the specified angle. Args: servo: int; servo to rotate [1,2,3,4,5,6] angle: int; servo angle to move to port: object; serial port Returns: None """ err_msg = 'Servo angle %.1f must be between 0 and %d.' % ( angle, ARDUINO_ANGLE_MAX) if angle < 0: logging.error('%s', err_msg) angle = 0 elif angle > ARDUINO_ANGLE_MAX: logging.error('%s', err_msg) angle = ARDUINO_ANGLE_MAX cmd = [chr(x) for x in [START_BYTE_SERVO, servo, angle]] cmd_read = send_command(port, cmd) hex_read = convert_to_hex(cmd_read) logging.debug(' rx: %s %s %s', hex_read[0], hex_read[1], hex_read[2]) def main(): # get channel, rotate, offset, debug from command line servo, angle, offset, debug = get_cmdline_args() # set logging level if debug: logging.basicConfig(level=logging.DEBUG) else: logging.basicConfig(level=logging.INFO) # initialize port serial_port = serial_port_def('Arduino') # send test commands to Arduino until command returns properly establish_serial_comm(serial_port) # rotate servo to angle angle_norm = int( round((angle - offset) * ARDUINO_ANGLE_MAX / HS755HB_ANGLE_MAX, 0)) logging.info('Moving servo %d to position %d', servo, angle) rotate_servo(servo, angle_norm, serial_port) if __name__ == '__main__': main()