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 init(port='/dev/ttyUSB1'):
ser = serial.Serial(port, 115200)
ser.write([0xFF, 0xFE, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00]) # Return to origin
xpos = 0
ypos = 0
zpos = 0
dir_byte = 0
number_of_command = 0
step = 1
car = [xpos, ypos, zpos, dir_byte, ser, number_of_command, step]
# 0 1 2 3 4 5 6 7 8
#Check is record file exist and clear data if there are some already store in there
try:
if os.stat('last_position_input.txt').st_size != 0:
f1 = open('last_position_input.txt', 'w')
f1.close()
if os.stat('history_position_input.txt').st_size != 0:
f2 = open('history_position_input.txt', 'w')
f2.close()
except FileNotFoundError:
f1 = open('last_position_input.txt', 'w')
f1.close()
f2 = open('history_position_input.txt', 'w')
f2.close()
tcnp.record_position(car)
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 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 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 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 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 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 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 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()))