def interrupt(car, joy, proxy): car[0] = 0 car[1] = 0 car[2] = 0 tcnp.move_to_coordinate(car) tcng.relay_off() joy.close() print('Proxy.reset(), response: {}'.format(proxy.reset()))
def lidar_evade_velocity(car, command, step=30): d = int(command[1:5]) a = int(command[6:10]) #if 180 > a >= 0: # a = a -30 #if 360 >= a > 180: # a = a + 30 dx = int(round(step * math.sin(math.radians(a)), 0)) dy = int(round(step * math.cos(math.radians(a)), 0)) car[0] = -dx car[1] = -dy tcnp.move_to_coordinate(car) #car = move_to_coordinate(car) time.sleep(0.1) print('dx {} dy {} a {} d {}'.format(dx, dy, a, d)) return car
def joy_stick_control_2(car, a, b, x1, y1, z1, start=0, threshold=20, change_mode=1): #Xbox connect with Raspberry car = tcn.acc_or_not(car, a, b) # determine the incresement or decresement of step if abs(x1) < threshold: x1 = 0 if abs(y1) < threshold: y1 = 0 if abs(z1) < threshold: z1 = 0 xyz = [ abs(x1 * car[6] / 100), abs(y1 * car[6] / 100), abs(z1 * car[6] / 100) ] car[0] = int(car[0] + x1 * car[6] / 100) car[1] = int(car[1] + y1 * car[6] / 100) car[2] = int(car[2] + z1 * car[6] / 100) car = tcn.move_to_coordinate(car) tcn.record_position(car) time.sleep(0.006) return car
def joy_stick_control_v(car, a, b, x1, y1, z1, threshold=20): #Xbox connect with Raspberry car = tcn.acc_or_not(car, a, b, 130) if abs(x1) < threshold: x1 = 0 if abs(y1) < threshold: y1 = 0 if abs(z1) < threshold: z1 = 0 car[0] = int(x1 * car[6] / 100) car[1] = int(y1 * car[6] / 100) car[2] = int(z1 * car[6] / 100) #print(" {} {} {}".format(car[0],car[1],car[2])) car = tcn.move_to_coordinate(car) time.sleep(0.006) return car
def set_target_position_lidar(car, sock, x, y, z, max_speed=50): command = tcns.listen_udp(sock) posx = 0 posy = 0 posz = 0 lastx = x lasty = y lastz = z x = x - car[0] y = y - car[1] z = z - car[2] print('xx {} yy {} zz {}'.format(x, y, z)) dis = (x**2 + y**2)**0.5 # Encoder angle = tcnp.vector_angle(x, y) loop_int = int(dis / max_speed) loop_float = dis / max_speed dis_left = int(max_speed * (loop_float - loop_int)) for i in range(loop_int): if command != None: car = lidar_evade_velocity(car, command) car = set_target_position_lidar(car, sock, lastx + car[0], lasty + car[1], z, max_speed) print('evading') return 0 else: if abs(x) > 0: car[0] = int(round(max_speed * math.cos(angle), 0)) if abs(y) > 0: car[1] = int(round(max_speed * math.sin(angle), 0)) if abs(z) > 0: if z > 0: car[2] = max_speed if z < 0: car[2] = -max_speed #print("x {} y {} z {}".format(posx,posy,posz)) tcnp.move_to_coordinate(car) time.sleep(0.006) car[0] = car[1] = car[2] = 0 tcnp.move_to_coordinate(car) car[0] = lastx car[1] = lasty car[2] = lastz return car
def set_target_velocity(car, x, y, z, max_speed=50): posx = 0 posy = 0 posz = 0 lastx = x lasty = y lastz = z x = x - car[0] y = y - car[1] z = z - car[2] print('xx {} yy {} zz {}'.format(x, y, z)) dis = (x**2 + y**2)**0.5 # Encoder angle = tcnp.vector_angle(x, y) loop_int = int(dis / max_speed) loop_float = dis / max_speed dis_left = int(max_speed * (loop_float - loop_int)) for i in range(loop_int): if abs(x) > 0: car[0] = int(round(max_speed * math.cos(angle), 0)) if abs(y) > 0: car[1] = int(round(max_speed * math.sin(angle), 0)) if abs(z) > 0: if z > 0: car[2] = max_speed if z < 0: car[2] = -max_speed #print("x {} y {} z {}".format(posx,posy,posz)) tcnp.move_to_coordinate(car) time.sleep(0.006) car[0] = car[1] = car[2] = 0 tcnp.move_to_coordinate(car) car[0] = lastx car[1] = lasty car[2] = lastz return car
def the_error(proxy): try: joy = xbox.Joystick() except IOError as e: print(e) print('Try reconnecting in 1 second') joy = xbox.Joystick() car = tcnp.init('/dev/ttyUSB1') mainflag = True recorded_position = [] msgb = "" print('Please locate 2 position') while mainflag: try: innerflag = True pose_resp = proxy.get_pose() tcnx.xbox_velocity_vision(joy, car) msgb = show_info(proxy, msgb) # The behavier when push button 'x' if joy.X(): print('Add new way point') car[0] = 0 # Make car stop car[1] = 0 car[2] = 0 tcnp.move_to_coordinate(car) time.sleep(0.5) recorded_position.append([ pose_resp[2], pose_resp[3] ]) # Remove the last element of recorded_position print(recorded_position) time.sleep(0.1) # The behavier when push button 'y' if joy.Y(): print('Delet last generated way point') car[0] = 0 # Make car stop car[1] = 0 car[2] = 0 tcnp.move_to_coordinate(car) time.sleep(0.5) recorded_position.pop( len(recorded_position) - 1) # Remove the last element of recorded_position print(recorded_position) time.sleep(0.1) if joy.Start(): while innerflag: for i in range(len(recorded_position)): position = recorded_position[i] print('Target X : {} Y : {} '.format( position[0], position[1])) forflag = vision_to_target_position( car, joy, position[0], position[1] ) # Move to target according to the camera time.sleep(1) if forflag == False: innerflag = False recorded_position = [] break if forflag != False: for j in range(len(recorded_position)): position = recorded_position[len(recorded_position) - 1 - j] print('Target X : {} Y : {} '.format( position[0], position[1])) forflag = vision_to_target_position( car, joy, position[0], position[1] ) # Move to target according to the camera time.sleep(1) if forflag == False: innerflag = False recorded_position = [] break if joy.Back(): innerflag = False print('Back to setting mode') time.sleep(1) recorded_position = [] print('Please set position') if joy.Back(): mainflag = False print('Back to command mode') tcng.relay_off() print('Proxy.set_reset(), response: {}'.format( proxy.set_reset())) except KeyboardInterrupt: car[0] = 0 car[1] = 0 car[2] = 0 mainflag = False tcnp.move_to_coordinate(car) tcng.relay_off() print('Proxy.set_reset(), response: {}'.format(proxy.set_reset())) break except Exception as e: mainflag = False print(e) tcng.relay_off() print('Proxy.set_reset(), response: {}'.format(proxy.set_reset()))
def vision_to_target_position(car, joy, tx, ty, angle=0, step=15, turn_step=2, error_range=15, deacc_range=150, msgb=""): runflag = True i = 0 j = 0 while runflag: pose_resp = proxy.get_pose() status_resp = proxy.get_status() x = pose_resp[2] y = pose_resp[3] angle = pose_resp[4] dx = tx - x dy = ty - y vxy = move_to_coordinate_speed(dx, dy, angle, step, error_range, deacc_range) #if angle > 0: # car[2] = turn_step #if angle < 0: # car[2] = - turn_step show_mesflag = True str2 = "" #while status_resp[0] != 4 : # switch to xbox control while pose_resp[5] != 100: if show_mesflag == True: print( 'The device cannot identify its current location \n Switch to XBOX_control mode' ) show_mesflag = False tcnx.xbox_velocity_vision(joy, car) status_resp = proxy.get_status() pose_resp = proxy.get_pose() str1 = 'con: ' + format( pose_resp[5]) + ' status: ' + format(status_resp) if str1 != str2: print(str1) str2 = str1 if joy.Back(): runflag = False print('Cancel operation') time.sleep(1) print("Back to 'tr' mode ") return False # Auto mode car[0] = vxy[0] car[1] = vxy[1] tcnp.move_to_coordinate(car) msg1 = "status:" + format(status_resp[0]) + ", " msg2 = "mapid:" + format(pose_resp[1]) + ", " msg3 = "(x,y):(" + format(pose_resp[2]) + "," + format( pose_resp[3]) + "), " msg4 = "thida: " + format(pose_resp[4]) + ", " msg5 = "conf: " + format(pose_resp[5]) msg6 = " ############################### " if status_resp[0] == 5: msga = msg1 + msg2 + msg3 + msg4 + msg5 + msg6 else: msga = msg1 + msg2 + msg3 + msg4 + msg5 + ' dx ' + str( dx) + ' dy ' + str(dy) + ' vx ' + str(vxy[0]) + ' vy ' + str( vxy[1]) if msgb != msga: print(msga) msgb = msga # else: # print(msga) if vxy[0] == 0 and vxy[1] == 0: print('Target reached') break if joy.Back(): print('Cancel operation') car[0] = 0 car[1] = 0 car[2] = 0 tcnp.move_to_coordinate(car) time.sleep(1) print("Back to setting mode ") return False
def set_target(proxy): try: joy = xbox.Joystick() except IOError as ioe: print(ioe) print('reconnect to xbox in 1 second') time.sleep(1) joy = xbox.Joystick() car = tcnp.init('/dev/ttyUSB1') msgb = "" recorded_position = [] try: while True: forflag = True msgb = show_info( proxy, msgb ) # Show camera information . show_ifo return msgb . We put msgb back to show info to make it show new data only ( Old data won't be printed) tcnx.xbox_velocity_vision(joy, car) # Turn on XBOX control mode pose_resp = proxy.get_pose() # The behavier when push button 'x' if joy.X(): print('Add new way point') car[0] = 0 # Make car stop car[1] = 0 car[2] = 0 tcnp.move_to_coordinate(car) time.sleep(0.5) recorded_position.append([ pose_resp[2], pose_resp[3] ]) # Remove the last element of recorded_position print(recorded_position) time.sleep(0.1) # The behavier when push button 'y' if joy.Y(): print('Delet last generated way point') car[0] = 0 # Make car stop car[1] = 0 car[2] = 0 tcnp.move_to_coordinate(car) time.sleep(0.5) recorded_position.pop( len(recorded_position) - 1) # Remove the last element of recorded_position print(recorded_position) time.sleep(0.1) # The behavier when push button 'start' if joy.Start(): if len(recorded_position ) != 0: # If no position was recorded , then pass for i in range(len(recorded_position)): position = recorded_position[i] print('Target X : {} Y : {} '.format( position[0], position[1])) forflag = vision_to_target_position( car, joy, position[0], position[1] ) # Move to target according to the camera time.sleep(1) if forflag == False: break recorded_position = [ ] # Wipe out recording file after execution else: print('no recorded position') # The behavier when push button 'Back' if joy.Back(): joy.close() # Kill xbox process tcng.relay_off() # Make GPIO pin 4 off print('Closing tr mode') print('Proxy.set_reset(), response: {}'.format( proxy.set_reset())) break except KeyboardInterrupt: tcng.relay_off() joy.close() print('Proxy.set_reset(), response: {}'.format(proxy.set_reset())) except Exception as e: tcng.relay_off() print(e) joy.close() print('Proxy.set_reset(), response: {}'.format(proxy.set_reset()))
def scenario2(proxy): joy = xbox.Joystick() car = tcnp.init('/dev/ttyUSB1') mainflag = True while mainflag: try: runflag = True runflagx = True runflagy = True while runflagx: try: tx = int(raw_input('Target X')) runflagx = False except KeyboardInterrupt: runflagy = False runflag = False mainflag = False tcng.relay_off() print('Proxy.set_reset(), response: {}'.format( proxy.set_reset())) break except ValueError: print('You must enter numbers') except Exception as e: print(e) tcng.relay_off() while runflagy: try: ty = int(raw_input('Target Y')) runflagy = False except KeyboardInterrupt: runflag = False mainflag = False tcng.relay_off() print('Proxy.set_reset(), response: {}'.format( proxy.set_reset())) break except ValueError: print('You must enter numbers') except Exception as e: print(e) tcng.relay_off() vision_to_target_position(car, joy, tx, ty) except KeyboardInterrupt: car[0] = 0 car[1] = 0 car[2] = 0 mainflag = False tcnp.move_to_coordinate(car) tcng.relay_off() print('Proxy.set_reset(), response: {}'.format(proxy.set_reset())) break except Exception as e: mainflag = False print(e) tcng.relay_off() print('Proxy.set_reset(), response: {}'.format(proxy.set_reset()))