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)
Ejemplo n.º 2
0
Archivo: server.py Proyecto: wxy620/gwr
    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)
Ejemplo n.º 3
0
    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)