RoboGame2020/worker.py

'''
经过重构后的船新代码
使用GPIO.PWM来生成PWM波。
可以指定固定角度或时间，也可以手动控制。
'''
from time import sleep
import RPi.GPIO as GPIO

'''
端口的定义
'''
CAROUSEL_ENA = 19
CAROUSEL_DIR = 26
CAROUSEL_PUL = 21
ELEVATOR_DIR = 16
ELEVATOR_PUL = 20
CONVEYOR_ENA = 12
CONVEYOR_DIR = 6
CONVEYOR_PUL = 13
WATCHDOG_DIR = 9
WATCHDOG_PUL = 5
HORIZONAL_ENA = 14
HORIZONAL_DIR = 18
HORIZONAL_PUL = 15
VERTICAL_ENA = 11
VERTICAL_DIR = 7
VERTICAL_PUL = 1
BAG_PUSHER_ENA = 23
BAG_PUSHER_DIR = 22
BAG_PUSHER_PUL = 24
SCISSORS_PUSHER_ENA = 0
SCISSORS_PUSHER_DIR = 3
SCISSORS_PUSHER_PUL = 8
SPIGOT = 17
SCISSORS = 25


class Motor():
    '''
    小板控制的电机，只有DIR和PUL引脚，用于控制方向和速度。
    '''

    def __init__(self, DIR, PUL, step_angle, subdivision, degree_per_second):
        '''
        初始化，指定引脚，步进角和细分。
        '''
        self.__DIR = DIR
        self.__PUL = PUL
        self.__degree_per_cycle = step_angle / subdivision
        self.__degree_per_second = degree_per_second
        self.__half_cycle = self.__degree_per_cycle / self.__degree_per_second / 2
        self.__frequency = degree_per_second / self.__degree_per_cycle
        GPIO.setup(DIR, GPIO.OUT)
        GPIO.setup(PUL, GPIO.OUT)
        self.__pwm = GPIO.PWM(PUL, self.__frequency)

    def __set_direction(self, output_DIR):
        '''
        设置转动的角度
        '''
        GPIO.output(self.__DIR, output_DIR)

    def __pwm_start(self):
        '''
        开始产生PWM波
        '''
        self.__pwm.ChangeFrequency(self.__frequency)
        self.__pwm.start(50)

    def stop(self):
        '''
        停止产生PWM波
        '''
        self.__pwm.stop()

    def start(self, direction):
        '''
        开始转动
        '''
        self.__set_direction(direction)
        self.__pwm_start()

    def __turn_time(self, second):
        '''
        转动指定时间
        '''
        self.__pwm_start()
        sleep(second)
        self.stop()

    def __turn_degree(self, degree):
        '''
        转动指定角度 
        '''
        cycle = int(degree / self.__degree_per_cycle)
        for _ in range(cycle):
            GPIO.output(self.__PUL, GPIO.LOW)
            sleep(self.__half_cycle)
            GPIO.output(self.__PUL, GPIO.HIGH)
            sleep(self.__half_cycle)

    def forward_time(self, second):
        '''
        设置时间，正转
        '''
        self.__set_direction(GPIO.HIGH)
        self.__turn_time(second)

    def backward_time(self, second):
        '''
        设置时间，反转
        '''
        self.__set_direction(GPIO.LOW)
        self.__turn_time(second)

    def forward_degree(self, degree):
        '''
        设置角度，正转
        '''
        self.__set_direction(GPIO.HIGH)
        self.__turn_degree(degree)

    def backward_degree(self, degree):
        '''
        设置角度，反转
        '''
        self.__set_direction(GPIO.LOW)
        self.__turn_degree(degree)


class ENAMotor(Motor):
    '''
    带有ENA信号的步进电机
    '''

    def __init__(self, ENA, DIR, PUL, step_angle, subdivision, degree_per_second):
        super(ENAMotor, self).__init__(DIR, PUL, step_angle,
                                       subdivision, degree_per_second)
        self.__ena = ENA
        GPIO.setup(ENA, GPIO.OUT)

    def enable(self):
        '''
        启用电机
        '''
        GPIO.output(self.__ena, GPIO.HIGH)

    def disable(self):
        '''
        停用电机
        '''
        GPIO.output(self.__ena, GPIO.LOW)

    def start(self, direction):
        '''
        开始转动
        '''
        self.enable()
        super(ENAMotor, self).start(direction)

    def stop(self):
        '''
        停止转动
        '''
        super(ENAMotor, self).stop()
        self.disable()

    def forward_degree(self, degree):
        '''
        设置角度，正转
        '''
        self.enable()
        super(ENAMotor, self).forward_degree(degree)
        self.disable()

    def backward_degree(self, degree):
        '''
        设置角度，反转
        '''
        self.enable()
        super(ENAMotor, self).backward_degree(degree)
        self.disable()

    def forward_time(self, second):
        '''
        设置时间，正转
        '''
        self.enable()
        super(ENAMotor, self).forward_time(second)
        self.disable()

    def backward_time(self, second):
        '''
        设置时间，反转
        '''
        self.enable()
        super(ENAMotor, self).backward_time(second)
        self.disable()


class Servo:
    '''
    舵机
    '''

    def __init__(self, port):
        '''初始化'''
        self.port = port
        GPIO.setup(port, GPIO.OUT)
        self.__pwm = GPIO.PWM(self.port, 50)

    def degree(self, degree):
        '''转到某个角度'''
        dc = (degree / 180 * 2 + 0.5) * 5
        self.__pwm.ChangeFrequency(50)
        self.__pwm.start(dc)
        sleep(1)
        self.__pwm.stop()


class Relay:
    '''
    继电器
    '''

    def __init__(self, port):
        self.port = port
        GPIO.setup(port, GPIO.OUT)

    def enable(self):
        GPIO.output(self.port, GPIO.HIGH)

    def disable(self):
        GPIO.output(self.port, GPIO.LOW)


GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
carousel = ENAMotor(CAROUSEL_ENA, CAROUSEL_DIR, CAROUSEL_PUL, 1.8, 32, 30)
elevator = Motor(ELEVATOR_DIR, ELEVATOR_PUL, 18, 16, 1800)
conveyor = ENAMotor(CONVEYOR_ENA, CONVEYOR_DIR, CONVEYOR_PUL, 1.8, 32, 30)
watchdog = Motor(WATCHDOG_DIR, WATCHDOG_PUL, 18, 16, 3600)
recycleh = ENAMotor(
    HORIZONAL_ENA, HORIZONAL_DIR, HORIZONAL_PUL, 1.8, 32, 360)
recyclev = ENAMotor(
    VERTICAL_ENA, VERTICAL_DIR, VERTICAL_PUL, 1.8, 32, 360)
bag_pusher = ENAMotor(BAG_PUSHER_ENA, BAG_PUSHER_DIR,
                      BAG_PUSHER_PUL, 1.8, 32, 360)
scissors_pusher = ENAMotor(
    SCISSORS_PUSHER_ENA, SCISSORS_PUSHER_DIR, SCISSORS_PUSHER_PUL, 1.8, 32, 360)
spigot = Relay(SPIGOT)
spigot.disable()
scissors = Servo(SCISSORS)