def run(self):
global goal_pos, servo_command, init_get, functionMode
while self.__running.isSet():
self.__flag.wait()
if functionMode != 6:
if servo_command == 'lookleft':
servo.lookleft(servo_speed)
elif servo_command == 'lookright':
servo.lookright(servo_speed)
elif servo_command == 'up':
servo.up(servo_speed)
elif servo_command == 'down':
servo.down(servo_speed)
else:
pass
if functionMode == 4:
servo.ahead()
findline.run()
if not functionMode:
move.motorStop()
elif functionMode == 5:
autoDect(50)
if not functionMode:
move.motorStop()
elif functionMode == 6:
if MPU_connection:
accelerometer_data = sensor.get_accel_data()
X_get = accelerometer_data['x']
if not init_get:
goal_pos = X_get
init_get = 1
if servo_command == 'up':
servo.up(servo_speed)
time.sleep(0.2)
accelerometer_data = sensor.get_accel_data()
X_get = accelerometer_data['x']
goal_pos = X_get
elif servo_command == 'down':
servo.down(servo_speed)
time.sleep(0.2)
accelerometer_data = sensor.get_accel_data()
X_get = accelerometer_data['x']
goal_pos = X_get
if abs(X_get - goal_pos) > tor_pos:
if X_get > goal_pos:
servo.down(int(mpu_speed * abs(X_get - goal_pos)))
elif X_get < goal_pos:
servo.up(int(mpu_speed * abs(X_get - goal_pos)))
time.sleep(0.03)
continue
else:
functionMode = 0
try:
self.pause()
except:
pass
time.sleep(0.03)
def findline_thread(): #Line tracking mode
while 1:
while FindLineMode:
findline.run()
time.sleep(0.2)
def findline_thread():
'''Line tracking mode'''
while 1:
while findline_mode:
findline.run()
time.sleep(0.2)
def run(self):
global goal_pos, servo_command, init_get, functionMode
while self.__running.isSet():
self.__flag.wait() # 为True时立即返回, 为False时阻塞直到内部的标识位为True后返回
if functionMode != 6:
if servo_command == 'lookleft':
servo.lookleft(servo_speed)
elif servo_command == 'lookright':
servo.lookright(servo_speed)
elif servo_command == 'up':
servo.up(servo_speed)
elif servo_command == 'down':
servo.down(servo_speed)
elif servo_command == 'lookup':
servo.lookup(servo_speed)
elif servo_command == 'lookdown':
servo.lookdown(servo_speed)
elif servo_command == 'grab':
servo.grab(servo_speed)
elif servo_command == 'loose':
servo.loose(servo_speed)
else:
pass
if functionMode == 4:
servo.ahead()
findline.run()
if not functionMode:
move.motorStop()
elif functionMode == 5:
servo.ahead()
dis_get = ultra.checkdist()
if dis_get < 0.15:
move.motorStop()
move.move(100, 'backward', 'no', 1)
move.motorStop()
move.move(100, 'no', 'left', 1)
time.sleep(0.2)
move.motorStop()
else:
move.move(100, 'forward', 'no', 1)
if not functionMode:
move.motorStop()
elif functionMode == 6:
if MPU_connection:
accelerometer_data = sensor.get_accel_data()
Y_get = accelerometer_data['y']
if not init_get:
goal_pos = Y_get
init_get = 1
if servo_command == 'up':
servo.up(servo_speed)
time.sleep(0.2)
accelerometer_data = sensor.get_accel_data()
Y_get = accelerometer_data['y']
goal_pos = Y_get
elif servo_command == 'down':
servo.down(servo_speed)
time.sleep(0.2)
accelerometer_data = sensor.get_accel_data()
Y_get = accelerometer_data['y']
goal_pos = Y_get
if abs(Y_get - goal_pos) > tor_pos:
if Y_get < goal_pos:
servo.down(int(mpu_speed * abs(Y_get - goal_pos)))
elif Y_get > goal_pos:
servo.up(int(mpu_speed * abs(Y_get - goal_pos)))
time.sleep(0.03)
continue
else:
functionMode = 0
try:
self.pause()
except:
pass
time.sleep(0.07)