def run(self):
while self.__running.isSet():
self.__flag.wait() # 为True时立即返回, 为False时阻塞直到内部的标识位为True后返回
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
time.sleep(0.07)
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)
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)
elif servo_command == 'handup':
servo.handUp(servo_speed)
elif servo_command == 'handdown':
servo.handDown(servo_speed)
else:
pass
if functionMode == 4:
if MPU_connection:
try:
accelerometer_data = sensor.get_accel_data()
Y_get = accelerometer_data['y']
X_get = accelerometer_data['x']
tcpCliSock.send(
('OSD %f %f' %
(round(X_get, 2), round(Y_get, 2))).encode())
#print('OSD %f %f'%(round(Y_get,2),round(X_get,2)))
time.sleep(0.1)
except:
print('MPU_6050 I/O ERROR')
pass
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(1)
move.motorStop()
else:
move.move(100, 'forward', 'no', 1)
if not functionMode:
move.motorStop()
elif functionMode == 6:
if MPU_connection:
xGet = sensor.get_accel_data()
print(xGet)
xGet = xGet['x']
xOut = kalman_filter_X.kalman(xGet)
pwm.set_pwm(4, 0, servo.pwm3_pos + pwmGenOut(xOut * 9))
# pwm.set_pwm(2, 0, self.steadyGoal+pwmGenOut(xGet*10))
time.sleep(0.05)
continue
else:
functionMode = 0
servo_move.pause()
time.sleep(0.07)