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.reset(), response: {}'.format(proxy.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.reset(), response: {}'.format(proxy.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.reset(), response: {}'.format(proxy.reset()) ) interrupt(car, joy, proxy) break except Exception as e: mainflag = False print(e) tcng.relay_off() print('Proxy.reset(), response: {}'.format(proxy.reset()))
def main_xbox_velocity_lidar(): joy = xbox.Joystick() car = tcn.init('/dev/ttyUSB1') sock = tcns.init_server_udp() try: g.init() g.relay_on() # Loop until back button is pressed while True: command = tcns.listen_udp(sock) if command != None: tcnv.lidar_evade_velocity(car, command) else: x = 100 * round(joy.leftX(), 2) y = 100 * round(joy.leftY(), 2) z = 100 * round(joy.rightX(), 2) a = int(joy.A()) b = int(joy.B()) #print('{} {} {}'.format(x,y,z)) car = joy_stick_control_v(car, a, b, x, y, -z) except Exception as e: g.relay_off() joy.close() print(e) except KeyboardInterrupt: g.relay_off() joy.close()
def main_xbox_velocity(): joy = xbox.Joystick() # joy represent xbox object car = tcn.init('/dev/ttyUSB1') try: g.init() g.relay_on() # turn on the power of STM32 # Loop until back button is pressed while not joy.Back(): try: x = 100 * round( joy.leftX(), 2 ) # The number from joystick is float , since joy_stick_control_v y = 100 * round( joy.leftY(), 2 ) # require int input , we convert it into integer through round it to 2 decimals z = 100 * round(joy.rightX(), 2) # and multiply by 100 a = int(joy.A()) b = int(joy.B()) #print('{} {} {}'.format(x,y,z)) car = joy_stick_control_v(car, a, b, x, y, -z) except: g.relay_off() except: g.relay_off() joy.close()
def scenario1(proxy): car = tcnp.init() print('car') try: joy = xbox.Joystick() print('joy') joy.connected() print(joy) except Exception as e: print(e) joy = xbox.Joystick() mapping_flag = True msgb = "" while mapping_flag: try: #print('try') tcnx.xbox_velocity_vision(joy, car) #print('xbox') #time.sleep(0.1) pose_resp = proxy.get_pose() status_resp = proxy.get_status() msgb = show_info(proxy, msgb) #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 #if msgb!=msga: # print(msga) # msgb=msga # else: # print(msga) if joy.Back(): mapping_flag = False print(joy.Back()) joy.close() tcng.relay_off() except KeyboardInterrupt: joy.close() tcng.relay_off() #interrupt(car,joy,proxy) break except Exception as e: tcng.relay_off joy.close() print(e)
def main_xbox_position(): joy = xbox.Joystick() car = tcn.init('/dev/ttyUSB1') # Loop until back button is pressed while not joy.Back(): #print(start) x = 100 * round(joy.leftX(), 2) y = 100 * round(joy.leftY(), 2) z = 100 * round(joy.rightX(), 2) a = int(joy.A()) b = int(joy.B()) car = joy_stick_control_2(car, a, b, x, y, -z) joy.close()
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 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()))