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)