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()))