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