1# Copyright 2016 The Android Open Source Project 2# 3# Licensed under the Apache License, Version 2.0 (the "License"); 4# you may not use this file except in compliance with the License. 5# You may obtain a copy of the License at 6# 7# http://www.apache.org/licenses/LICENSE-2.0 8# 9# Unless required by applicable law or agreed to in writing, software 10# distributed under the License is distributed on an "AS IS" BASIS, 11# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12# See the License for the specific language governing permissions and 13# limitations under the License. 14 15import codecs 16import logging 17import struct 18import sys 19import time 20import hardware as hw 21import pyudev 22import serial 23import serial.tools.list_ports 24 25ARDUINO_ANGLE_MAX = 180.0 # degrees 26ARDUINO_ANGLES = [0]*5 + range(0, 90, 3) + [90]*5 + range(90, -1, -3) 27ARDUINO_BAUDRATE = 9600 28ARDUINO_CMD_LENGTH = 3 29ARDUINO_CMD_TIME = 2.0 * ARDUINO_CMD_LENGTH / ARDUINO_BAUDRATE # round trip 30ARDUINO_DEFAULT_CH = '1' 31ARDUINO_MOVE_TIME = 0.06 - ARDUINO_CMD_TIME # seconds 32ARDUINO_START_BYTE = 255 33ARDUINO_START_NUM_TRYS = 3 34ARDUINO_TEST_CMD = [b'\x01', b'\x02', b'\x03'] 35ARDUINO_VALID_CH = ['1', '2', '3', '4', '5', '6'] 36CANAKIT_BAUDRATE = 115200 37CANAKIT_COM_SLEEP = 0.05 38CANAKIT_DATA_DELIMITER = '\r\n' 39CANAKIT_DEFAULT_CH = '1' 40CANAKIT_DEVICE = 'relay' 41CANAKIT_PID = 'fc73' 42CANAKIT_SET_CMD = 'REL' 43CANAKIT_SLEEP_TIME = 2 # seconds 44CANAKIT_VALID_CMD = ['ON', 'OFF'] 45CANAKIT_VALID_CH = ['1', '2', '3', '4'] 46CANAKIT_VID = '04d8' 47HS755HB_ANGLE_MAX = 202.0 # degrees 48NUM_ROTATIONS = 10 49SERIAL_SEND_TIMEOUT = 0.02 50 51 52def get_cmd_line_args(): 53 """Get command line arguments. 54 55 Args: 56 None, but gets sys.argv() 57 Returns: 58 rotate_cntl: str; 'arduino' or 'canakit' 59 rotate_ch: dict; arduino -> {'ch': str} 60 canakit --> {'vid': str, 'pid': str, 'ch': str} 61 num_rotations: int; number of rotations 62 """ 63 num_rotations = NUM_ROTATIONS 64 rotate_cntl = 'canakit' 65 rotate_ch = {} 66 for s in sys.argv[1:]: 67 if s[:8] == 'rotator=': 68 if len(s) > 8: 69 rotator_ids = s[8:].split(':') 70 if len(rotator_ids) == 1: 71 # 'rotator=default' 72 if rotator_ids[0] == 'default': 73 print ('Using default values %s:%s:%s for VID:PID:CH ' 74 'of rotator' % (CANAKIT_VID, CANAKIT_PID, 75 CANAKIT_DEFAULT_CH)) 76 vid = '0x' + CANAKIT_VID 77 pid = '0x' + CANAKIT_PID 78 ch = CANAKIT_DEFAULT_CH 79 rotate_ch = {'vid': vid, 'pid': pid, 'ch': ch} 80 # 'rotator=$ch' 81 elif rotator_ids[0] in CANAKIT_VALID_CH: 82 print ('Using default values %s:%s for VID:PID ' 83 'of rotator' % (CANAKIT_VID, CANAKIT_PID)) 84 vid = '0x' + CANAKIT_VID 85 pid = '0x' + CANAKIT_PID 86 ch = rotator_ids[0] 87 rotate_ch = {'vid': vid, 'pid': pid, 'ch': ch} 88 # 'rotator=arduino' 89 elif rotator_ids[0] == 'arduino': 90 rotate_cntl = 'arduino' 91 rotate_ch = {'ch': ARDUINO_DEFAULT_CH} 92 # 'rotator=arduino:$ch' 93 elif len(rotator_ids) == 2: 94 rotate_cntl = 'arduino' 95 ch = rotator_ids[1] 96 rotate_ch = {'ch': ch} 97 if ch not in ARDUINO_VALID_CH: 98 print 'Invalid arduino ch: %s' % ch 99 print 'Valid channels:', ARDUINO_VALID_CH 100 sys.exit() 101 # 'rotator=$vid:$pid:$ch' 102 elif len(rotator_ids) == 3: 103 vid = '0x' + rotator_ids[0] 104 pid = '0x' + rotator_ids[1] 105 rotate_ch = {'vid': vid, 'pid': pid, 'ch': rotator_ids[2]} 106 else: 107 err_string = 'Rotator ID (if entered) must be of form: ' 108 err_string += 'rotator=default or rotator=CH or ' 109 err_string += 'rotator=VID:PID:CH or ' 110 err_string += 'rotator=arduino or rotator=arduino:CH' 111 print err_string 112 sys.exit() 113 if (rotate_cntl == 'canakit' and 114 rotate_ch['ch'] not in CANAKIT_VALID_CH): 115 print 'Invalid canakit ch: %s' % rotate_ch['ch'] 116 print 'Valid channels:', CANAKIT_VALID_CH 117 sys.exit() 118 119 if s[:14] == 'num_rotations=': 120 num_rotations = int(s[14:]) 121 122 return rotate_cntl, rotate_ch, num_rotations 123 124 125def serial_port_def(name): 126 """Determine the serial port and open. 127 128 Args: 129 name: str; device to locate (ie. 'Arduino') 130 Returns: 131 serial port object 132 """ 133 devices = pyudev.Context() 134 for device in devices.list_devices(subsystem='tty', ID_BUS='usb'): 135 if name in device['ID_VENDOR']: 136 arduino_port = device['DEVNAME'] 137 break 138 139 return serial.Serial(arduino_port, ARDUINO_BAUDRATE, timeout=1) 140 141 142def arduino_read_cmd(port): 143 """Read back Arduino command from serial port.""" 144 cmd = [] 145 for _ in range(ARDUINO_CMD_LENGTH): 146 cmd.append(port.read()) 147 return cmd 148 149 150def arduino_send_cmd(port, cmd): 151 """Send command to serial port.""" 152 for i in range(ARDUINO_CMD_LENGTH): 153 port.write(cmd[i]) 154 155 156def arduino_loopback_cmd(port, cmd): 157 """Send command to serial port.""" 158 arduino_send_cmd(port, cmd) 159 time.sleep(ARDUINO_CMD_TIME) 160 return arduino_read_cmd(port) 161 162 163def establish_serial_comm(port): 164 """Establish connection with serial port.""" 165 print 'Establishing communication with %s' % port.name 166 trys = 1 167 hex_test = convert_to_hex(ARDUINO_TEST_CMD) 168 logging.info(' test tx: %s %s %s', hex_test[0], hex_test[1], hex_test[2]) 169 while trys <= ARDUINO_START_NUM_TRYS: 170 cmd_read = arduino_loopback_cmd(port, ARDUINO_TEST_CMD) 171 hex_read = convert_to_hex(cmd_read) 172 logging.info(' test rx: %s %s %s', 173 hex_read[0], hex_read[1], hex_read[2]) 174 if cmd_read != ARDUINO_TEST_CMD: 175 trys += 1 176 else: 177 logging.info(' Arduino comm established after %d try(s)', trys) 178 break 179 180 181def convert_to_hex(cmd): 182 # compatible with both python 2 and python 3 183 return [('%0.2x' % int(codecs.encode(x, 'hex_codec'), 16) if x else '--') 184 for x in cmd] 185 186 187def arduino_rotate_servo_to_angle(ch, angle, serial_port, delay=0): 188 """Rotate servo to the specified angle. 189 190 Args: 191 ch: str; servo to rotate in ARDUINO_VALID_CH 192 angle: int; servo angle to move to 193 serial_port: object; serial port 194 delay: int; time in seconds 195 """ 196 197 err_msg = 'Angle must be between 0 and %d.' % (ARDUINO_ANGLE_MAX) 198 if angle < 0: 199 print err_msg 200 angle = 0 201 elif angle > ARDUINO_ANGLE_MAX: 202 print err_msg 203 angle = ARDUINO_ANGLE_MAX 204 cmd = [struct.pack('B', i) for i in [ARDUINO_START_BYTE, int(ch), angle]] 205 arduino_send_cmd(serial_port, cmd) 206 time.sleep(delay) 207 208 209def arduino_rotate_servo(ch, serial_port): 210 """Rotate servo between 0 --> 90 --> 0. 211 212 Args: 213 ch: str; servo to rotate 214 serial_port: object; serial port 215 """ 216 for angle in ARDUINO_ANGLES: 217 angle_norm = int(round(angle*ARDUINO_ANGLE_MAX/HS755HB_ANGLE_MAX, 0)) 218 arduino_rotate_servo_to_angle( 219 ch, angle_norm, serial_port, ARDUINO_MOVE_TIME) 220 221 222def canakit_cmd_send(vid, pid, cmd_str): 223 """Wrapper for sending serial command. 224 225 Args: 226 vid: str; vendor ID 227 pid: str; product ID 228 cmd_str: str; value to send to device. 229 """ 230 hw_list = hw.Device(CANAKIT_DEVICE, vid, pid, '1', '0') 231 relay_port = hw_list.get_tty_path('relay') 232 relay_ser = serial.Serial(relay_port, CANAKIT_BAUDRATE, 233 timeout=SERIAL_SEND_TIMEOUT, 234 parity=serial.PARITY_EVEN, 235 stopbits=serial.STOPBITS_ONE, 236 bytesize=serial.EIGHTBITS) 237 try: 238 relay_ser.write(CANAKIT_DATA_DELIMITER) 239 time.sleep(CANAKIT_COM_SLEEP) # This is critical for relay. 240 relay_ser.write(cmd_str) 241 relay_ser.close() 242 except ValueError: 243 print 'Port %s:%s is not open' % (vid, pid) 244 sys.exit() 245 246 247def set_relay_channel_state(vid, pid, ch, relay_state): 248 """Set relay channel and state. 249 250 Args: 251 vid: str; vendor ID 252 pid: str; product ID 253 ch: str; channel number of relay to set. '1', '2', '3', or '4' 254 relay_state: str; either 'ON' or 'OFF' 255 Returns: 256 None 257 """ 258 if ch in CANAKIT_VALID_CH and relay_state in CANAKIT_VALID_CMD: 259 canakit_cmd_send( 260 vid, pid, CANAKIT_SET_CMD + ch + '.' + relay_state + '\r\n') 261 else: 262 print 'Invalid channel or command, no command sent.' 263 264 265def main(): 266 """Main function. 267 268 expected rotator string is vid:pid:ch or arduino:ch. 269 Canakit vid:pid can be found through lsusb on the host. 270 ch is hard wired and must be determined from the controller. 271 """ 272 # set up logging for debug info 273 logging.basicConfig(level=logging.INFO) 274 275 # get cmd line args 276 rotate_cntl, rotate_ch, num_rotations = get_cmd_line_args() 277 ch = rotate_ch['ch'] 278 print 'Controller: %s, ch: %s' % (rotate_cntl, ch) 279 280 # initialize port and cmd strings 281 if rotate_cntl == 'arduino': 282 # initialize Arduino port 283 serial_port = serial_port_def('Arduino') 284 285 # send test cmd to Arduino until cmd returns properly 286 establish_serial_comm(serial_port) 287 288 # initialize servo at origin 289 print 'Moving servo to origin' 290 arduino_rotate_servo_to_angle(ch, 0, serial_port, 1) 291 else: 292 vid = rotate_ch['vid'] 293 pid = rotate_ch['pid'] 294 295 # rotate phone 296 print 'Rotating phone %dx' % num_rotations 297 for _ in xrange(num_rotations): 298 if rotate_cntl == 'arduino': 299 arduino_rotate_servo(ch, serial_port) 300 else: 301 set_relay_channel_state(vid, pid, ch, 'ON') 302 time.sleep(CANAKIT_SLEEP_TIME) 303 set_relay_channel_state(vid, pid, ch, 'OFF') 304 time.sleep(CANAKIT_SLEEP_TIME) 305 print 'Finished rotations' 306 307 308if __name__ == '__main__': 309 main() 310