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_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