PWM舵机驱动

有时候我们也需要去驱动PWM舵机，毕竟价格低廉，实用性强。

这分为两部分，分别在LeServo和PWMServo文件里

一、底层驱动代码

# PWM舵机驱动 
#!/usr/bin/python3
# encoding: utf-8

import pigpio  # 导入PWM波形产生/ 读取库函数
import time    # 导入时间
import threading  # 导入线程
import math    # 导入数学库

'''
PWM舵机基本情况
频率：50HZ
中位位置：1500 
脉冲宽度范围：500-2500
控制角度：180度
'''

# PWM舵机初始化
class PWM_Servo(object):
    # pi: pigpio的实例 pin：树莓派的引脚  freq： PWM的频率 min_width： PWM的最小值 max_width： 
    # PWM的最大值 deviation： 偏差值  control_speed： 控制的速度
    def __init__(self, pi, pin, freq = 50, min_width = 500, max_width=2500, deviation = 0, control_speed = False):
        self.pi = pi # pigpio实例对象
        self.SPin = pin # 树莓派引脚
        self.Position = 1500  # PWM当前位置
        self.Freq = freq # 频率
        self.Min = min_width # 最小宽度
        self.Max = max_width # 最大宽度
        self.Deviation = deviation # 偏差值
        self.stepTime = 20 # 步进时间
        self.incTimes = 0 # 执行次数
        
        self.positionSet = self.Position # 记录当前设置的值
        self.Time = 0  # 当前要设置的时间
        
        self.positionInc = 0.0 # 当前位置和要设置的位置的差值 再分为incTimes份
        self.Time_t = 0  # 预设置时间
        self.positionSet_t = 0 # 预设置位置
        
        # self.speedControl = control_speed # 速度控制
        self.posChanged = False  # 位置改变不成立
        self.servoRunning = False # 舵机运行不成立
        #self.pi.set_PWM_range(self.SPin, int(1000000 / self.Freq)) # 设置PWM的范围
        #self.pi.set_PWM_frequency(self.SPin, self.Freq) # 设置PWM的频率
        # 控制速度就是采用pid去实时更新舵机的位置
        if control_speed is True: # 控制速度
          # 执行下面这条语句意思： 开辟一个线程执行updatePosition函数 ，参数传递为self
            t = threading.Thread(target=PWM_Servo.updatePosition, args=(self,))
            # setDaemon 守护线程是守护主线程，  调用函数里面存在等待时间时，只要设置了守护线程，函数中等待时间下面的代码都不会再执行
            t.setDaemon(True)
            t.start() # 启动线程，即让线程开始执行
            
    # 设置PWM当前的位置
    # pos 要设置的位置  time：要执行的时间
    def setPosition(self, pos, time = 0):
        if pos < self.Min or pos > self.Max:# 要设置的位置超出整个周期的范围
            print(pos)  # 打印出要设置的位置
            return # 返回结束函数
        if time == 0:
            self.Position = pos   # 修改当前的位置为设定的值
            self.positionSet = self.Position # 记录当前设置的值
            self.pi.set_PWM_dutycycle(self.SPin, self.Position + self.Deviation) # 设置树莓派引脚的占空比
        else: # 时间不为0
            if time < 20:  # 最小时间为20ms
                self.Time_t = 20  # 预设置的时间 = 20
            elif time > 30000: # 最大时间为30000ms  30s
                self.Time_t = 30000 # 预设置时间 = 30000
            else:    # 时间符合
                self.Time_t = time   # 预设置时间就等于这个时间
            self.positionSet_t = pos  #预设置的位置就等于这个位置
            self.posChanged = True  # 位置发生了改变
     
    # 获取当前的位置   
    def getPosition(self):
        return self.Position  # 返回当前的位置
    
    # 更新PWM的占空比  如果要控制速度的哦话，这个函数就会在初始化里开启了一个线程，实时更新
    def updatePosition(self):
        while True:  # 这是在线程里执行，所以是死循环  
            if self.posChanged is True: # 位置发生了改变 
                self.posChanged = False # 位置改变标志位清零
                self.Time = self.Time_t # 将预设值的时间给要设置的时间
                self.positionSet = self.positionSet_t # 将预设置的位置给要设置的位置
                self.incTimes = int(self.Time / self.stepTime) # 舵机运行次数等于 运行时间/步进时间
                self.positionInc =  self.Position - self.positionSet  # 当前位置和要设置位置的差值
                self.positionInc = int(self.positionInc / self.incTimes ) # 把差值分为incTimes份，
                self.servoRunning = True  # 启动舵机

            if self.servoRunning is True: # 舵机运行成立
                self.incTimes -= 1  # 运行次数减一
                
                if  self.incTimes <= 0: # 运行次数小于等于0 
                    self.Position = self.positionSet # 舵机的位置等于要设置的位置的值
                    self.servoRunning = False # 舵机启动标志位关闭
                else: # 运行次数大于0
                # int(self.positionInc * self.incTimes) 值会越来越小  因为 incTimes越来越小
                    self.Position = self.positionSet + int(self.positionInc * self.incTimes)
                try:
                      # 设置PWM的脉冲宽度 也就是舵机要转到的位置
                    self.pi.set_servo_pulsewidth(self.SPin, int(self.Position + self.Deviation))
                except:
                    pass
            time.sleep(0.02)   # 延迟20ms
        
    # 设置偏移量
    # newD 要设置的偏移量
    def setDeviation(newD = 0):
        if newD > 300 or newD < -300: # 超出偏移量的范围
            return
        self.Deviation = newD # 更新偏移量的值

二、二次封装代码

# PWM舵机二次封装调用
# PWM舵机本系统有两个，主要控制摄像头的运动

#!/usr/bin/env python3
# -*- coding: UTF-8 -*-
import pigpio
import time
from LeServo import PWM_Servo # PWM舵机驱动

Servos = ()  # 这是一个元组 存放的数据是PWM_Servo的对象
pi = None # pigpio的实例对象

# 初始化
# d:偏移量列表
def initLeArm(d):
    global Servos  # 全局变量
    global pi      # 全局变量
    pi = pigpio.pi() # 创建实例对象
    
    # 初始化PWM舵机  使用的是BCM编码
    # control_speed=True  位置一发生变化，舵机就会更新
    # 一设置位置  舵机就会更新 更新函数是一个线程
    servo1 = PWM_Servo(pi, 5,  deviation=d[0], control_speed=True)  # 扩展板上的7
    servo2 = PWM_Servo(pi, 6, deviation=d[1], control_speed=True)   # 扩展板上的8
    Servos = (servo1, servo2)  # 存放两个舵机的变量

# 9克舵机的转动范围0~180度，对应的PWM值为 500~2500
# 设置偏差值
d = [0, 0]
# 初始化云台舵机
initLeArm(d)

# 设置舵机ID   位置  时间
# servoId:ID  pos:位置 time：时间
def setServo(servoId, pos, time):
    if servoId < 1 or servoId > 2: # 如果不是在1或在2之间
        return # 返回
    if pos > 2500:   # 当前的位置是超出2500
        pos = 2500   # 当前的位置最大是2500
    elif pos < 500:  # 如果当前位置小于500
        pos = 500    # 当前位置最小是500
    if time > 30000: # 时间最大是30000ms  30s
        time = 30000
    elif time < 20: # 时间最小是20ms
        time = 20
    else:
        pass
    Servos[servoId - 1].setPosition(pos, time) # 给这个舵机设置位置，时间

# 设置舵机偏差值
# servoId： 舵机ID d：偏差值
def setDeviation(servoId, d):
    if servoId < 1 or servoId > 2: # 舵机号是1或2
        return
    if d < -300 or d > 300: # 偏差值的范围 -300 -- 300
        return
    Servos[servoId - 1].setDeviation(d) # 给这个舵机设置偏差值

# if __name__ == "__main__":  
    

使用这些函数就可以去驱动PWM舵机的使用。