树莓派产生PWM控制伺服电机和步进电机踩坑

树莓派产生PWM控制伺服电机和步进电机踩坑

本人最近在做项目当中，涉及到利用PWM对伺服电机(servo motor)和步进电机(step motor)的控制。分享下所写的代码，以及遇到的问题，希望可以帮到大家。

0 介绍

• PWM信号：网上已经有很多优秀的介绍，如果读者不了解，建议先重点了解一下占空比，频率这两个概念。
• 伺服电机：一般有两种类型，其中一种被称之为continuous，也就是说它和我们俗称的玩具赛车马达一样，可以一直朝一个方向旋转；另一种的活动范围受限，最多只能旋转0°到180°或者270°，通常通过改变PWM信号的占空比(duty cycle)来控制其绝对位置。比较常见的数值是，占空比为2%对应伺服电机0°的位置，占空比为12%对应其最大可达到的位置。
• 步进电机： 不同于伺服电机，步进电机由脉冲信号控制，例如你给步进电机发送一个脉冲，它就会移动一定的角度，这取决于控制分辨率(resolution)等。一般来说，价格昂贵的步进电机，同时会配有对应的驱动版，能够大大简化控制程序的编写，同时能够达到非常高的精度。幸运的是，本人使用的就是比较昂贵的步进电机。

1 伺服电机控制

废话不多说，直接上代码。

class servo_motor(object):
    '''
        Servo motor type: 
            ANNIMOS 20KG Digital Servo High Torque Full Metal Gear Waterproof for RC Model DIY, DS3218MG,Control Angle 270°
            2% duty cycle -> 0 degree
            12% duty cycle -> 270 degree
        Servo motor is only used for testing the calibration algorithm
    '''
    control_channel = 26    # set GPIO 26 for output PWM to servo motor 
    init_freq = 50          # initil frequency = 50Hz
    init_duty_cycle = 0     # set duty cycle as 2% (0 degree)
    cur_position = 0        # range (0 - 270 degree)
    name = 'servo_motor'

    def __init__(self, log):
        self.log = log      # initialize the logging
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.control_channel, GPIO.OUT)
        self.PWM = GPIO.PWM(self.control_channel, self.init_freq)
        self.PWM.start(self.init_duty_cycle)
        self.set_posiotion(200)
        time.sleep(2)
        self.log.info('servo initialization accomplished')

    def __del__(self):
        self.PWM.stop()
        GPIO.cleanup()
    
    def set_posiotion(self, position):
        '''
            PWM: the class initialized by GPIO.PWM
            angle: the angle you want to rotate (absolute position)
            2% duty cycle -> 0 degree
            12% duty cycle -> 270 degree
            angle/degree = duty cycle * 27 - 54
        '''
        if 0.0 <= position <= 270.0:
            duty_cycle = (position + 54.0) / 27.0
            self.PWM.ChangeDutyCycle(duty_cycle)
            self.cur_position = position
            log_str = 'current position:' + str(self.cur_position)
            self.log.info(log_str)
            self.log.info('current_position=%d degree' % (self.cur_position))
            time.sleep(1)           # wait one second to reach the position
        else:
            self.log.warning("invalid rotation angle")

    def move(self, move_angle, direction):
        if move_angle < 0 or self.cur_position + move_angle > 270:
            self.log.error("exceed the 270 degree")
            return False
        move_angle = -move_angle if direction == 'CW' else move_angle
        self.set_posiotion(self.cur_position + move_angle)
        self.log.info('move_angle=%d degree, direction=%s' % (abs(move_angle), direction))
        return True
    
    def get_motor_type(self):
        return self.name

可以看到，本质还是调用了PWM.ChangeDutyCycle(duty_cycle)这个函数来控制。然而，使用RPi.GPIO这个库似乎并不是非常完美，尽管它很简单。笔者在进行一些控制的时候，发现只要让其移动较小的度数，伺服电机容易发生抽搐的现象，从而导致安装在装置上面的传感器受到影响，不利于控制任务的完成。有空笔者会尝试使用pigpio这个库来重新实现，因为这个库是基于C语言实现的，更接近底层，能够实现硬件级的PWM，因此精度更高。除此之外，它还专门为伺服电机提供了更改占空比的函数，因此我估计它会有更好的表现。

2 步进电机的控制

依旧废话不多说，直接上代码。

class stepper_motor(object):
    '''
        Test script for step motor HW23-601 (https://www.mouser.com/c/?q=HW23-601) 
        with driver STR2 (https://www.applied-motion.com/products/stepper-drives/str2)
    '''
    num_of_steps_for_360 = 2000                 # this could be changed, please see the totorial of STR2/4
    resolution = num_of_steps_for_360 / 360     # resolution, how many steps would trigger 1 degree rotation
    CW_control_port = 21                        # GPIO 21 for clockwise (physically connected to dir+ port in the driver)
    CCW_control_port = 18                       # GPIO 18 for counter clockwise (physically connected to step+ port in the driver)
    inner_speed = 120                           # rotation speed: 120 rpm, outer would be 30 rpm
    gear_ratio = 4                              # radius of the outer gear is 4 times than the radius of the inner gear
    outer_speed = inner_speed / gear_ratio      # acutal speed: 30 rpm, that means it will take around 1 second to rotate 180 degree
    cur_position = 0
    # delay = 1 / 2 * (num_of_steps_for_360 * inner_speed / 60) 
    delay = 0.00075
    name = 'stepper_motor'

    def __init__(self, log):
        self.log = log
        GPIO.setmode(GPIO.BCM)                                    # set the GPIO mode  
        GPIO.setup(self.CCW_control_port, GPIO.OUT)               # set GPIO 18 as output
        GPIO.setup(self.CW_control_port, GPIO.OUT)                # set GPIO 21 as output
        time.sleep(0.5) 
        self.log.info("stepper motor initialization accomplished, outer speed: %d rpm, resolution: %d steps/degree" % (self.outer_speed, self.resolution))

    def __del__(self):
        GPIO.cleanup()

    def send_pulse(self, num_of_pulse, control_port):
        print('send pulse: %d' % num_of_pulse)
        time.sleep(10 / 1000)
        for i in range(int(num_of_pulse)):
            GPIO.output(control_port, GPIO.HIGH)
            time.sleep(self.delay)
            GPIO.output(control_port, GPIO.LOW)
            time.sleep(self.delay)
        print('send pulse done')

    def move(self, angle, direction):
        num_of_steps = self.resolution * angle * self.gear_ratio  # gear ratio is 4, so the outer speed is 1/4 of inner gear, need more pulse
        angle = -angle if direction == 'CCW' else angle           # decrease the angle if direction is CCW
        self.cur_position += angle
        print('start to move')
        if self.cur_position > 360 or self.cur_position < -360:
            self.log.error("rotation angle will exceeded 360 degree, wire might get twisted, current movement cancelled")
            self.log.info("movement:  has been cancelled" % (angle, direction))
            self.cur_position -= angle
            return False
        else:
            if direction == 'CW':
                self.send_pulse(num_of_steps, self.CW_control_port)
            else:
                self.send_pulse(num_of_steps, self.CCW_control_port)
            self.log.info("moved %d degree, direction:%s, current position: %d" % (angle, direction, self.cur_position))
            print('move done')
            return True
        
    def get_motor_type(self):
        return self.name

可以看到函数send_pulse是软件实现的PWM，这就导致了步进电机的转速不够快，笔者在测试的时候，发现如果设置time.sleep()过于短，则步进电机不能运转，循环会直接被阻塞，无法正常发送脉冲。同样的，笔者认为使用pigpio能够解决该问题，未来，应该会和大家分享用该库实现的版本。