【2023年】F题和E题——基于声传播的智能定位系统/运动目标控制与自动追踪系统——舵机云台方案

代码基于JetsonNano和树莓派
二选一使用即可
使用了PCA9685

PCA9685

PCA9685是一款i2c总线控制的16通道LED控制器，针对LCD进行了优化

红/绿/蓝/琥珀色(RGBA)颜色背光应用。每个LED输出都有它的

拥有12位分辨率(4096步)固定频率的单独PWM控制器

可编程频率从典型的40hz到1000hz，占空比为

从0%到100%可调，以允许LED设置到特定的亮度值。

所有输出设置为相同的PWM频率。

每个LED输出可以关闭或打开(无PWM控制)，或设置在其单独的PWM控制器

价值。LED输出驱动器被编程为开漏，电流为25毫安

吸收能力在5v或图腾柱与25毫安的吸收，10毫安的源能力在5v。的

PCA9685工作电源电压范围为2.3 V至5.5 V，输入和

输出容忍5.5 V。可直接连接LED输出(最多可达

25 mA, 5.5 V)或控制与外部驱动器和离散的最小量

组件用于更大电流或更高电压的led。

PCA9685是新的快速模式Plus (Fm+)系列。Fm+设备提供更高

频率(高达1mhz)和更密集的总线操作(高达4000pf)
驱动代码为：

#!/usr/bin/python

import time
import math
import smbus

# ============================================================================
# Raspi PCA9685 16-Channel PWM Servo Driver
# ============================================================================

class PCA9685:

  # Registers/etc.
  __SUBADR1            = 0x02
  __SUBADR2            = 0x03
  __SUBADR3            = 0x04
  __MODE1              = 0x00
  __MODE2              = 0x01
  __PRESCALE           = 0xFE
  __LED0_ON_L          = 0x06
  __LED0_ON_H          = 0x07
  __LED0_OFF_L         = 0x08
  __LED0_OFF_H         = 0x09
  __ALLLED_ON_L        = 0xFA
  __ALLLED_ON_H        = 0xFB
  __ALLLED_OFF_L       = 0xFC
  __ALLLED_OFF_H       = 0xFD


  def __init__(self, address=0x40, debug=False):
    self.bus = smbus.SMBus(1)
    self.address = address
    self.debug = debug
    if (self.debug):
      print("Reseting PCA9685")
    self.write(self.__MODE1, 0x00)
	
  def write(self, reg, value):
    "Writes an 8-bit value to the specified register/address"
    self.bus.write_byte_data(self.address, reg, value)
    if (self.debug):
      print("I2C: Write 0x%02X to register 0x%02X" % (value, reg))
	  
  def read(self, reg):
    "Read an unsigned byte from the I2C device"
    result = self.bus.read_byte_data(self.address, reg)
    if (self.debug):
      print("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % (self.address, result & 0xFF, reg))
    return result
	
  def setPWMFreq(self, freq):
    "Sets the PWM frequency"
    prescaleval = 25000000.0    # 25MHz
    prescaleval /= 4096.0       # 12-bit
    prescaleval /= float(freq)
    prescaleval -= 1.0
    if (self.debug):
      print("Setting PWM frequency to %d Hz" % freq)
      print("Estimated pre-scale: %d" % prescaleval)
    prescale = math.floor(prescaleval + 0.5)
    if (self.debug):
      print("Final pre-scale: %d" % prescale)

    oldmode = self.read(self.__MODE1);
    newmode = (oldmode & 0x7F) | 0x10        # sleep
    self.write(self.__MODE1, newmode)        # go to sleep
    self.write(self.__PRESCALE, int(math.floor(prescale)))
    self.write(self.__MODE1, oldmode)
    time.sleep(0.005)
    self.write(self.__MODE1, oldmode | 0x80)
    self.write(self.__MODE2, 0x04)

  def setPWM(self, channel, on, off):
    "Sets a single PWM channel"
    self.write(self.__LED0_ON_L+4*channel, on & 0xFF)
    self.write(self.__LED0_ON_H+4*channel, on >> 8)
    self.write(self.__LED0_OFF_L+4*channel, off & 0xFF)
    self.write(self.__LED0_OFF_H+4*channel, off >> 8)
    if (self.debug):
      print("channel: %d  LED_ON: %d LED_OFF: %d" % (channel,on,off))
	  
  def setServoPulse(self, channel, pulse):
    "Sets the Servo Pulse,The PWM frequency must be 50HZ"
    pulse = pulse*4096/20000        #PWM frequency is 50HZ,the period is 20000us
    self.setPWM(channel, 0, int(pulse))
    
  def setRotationAngle(self, channel, Angle): 
    if(Angle >= 0 and Angle <= 180):
        temp = Angle * (2000 / 180) + 501
        self.setServoPulse(channel, temp)
    else:
        print("Angle out of range")
        
  def start_PCA9685(self):
    self.write(self.__MODE2, 0x04)
    #Just restore the stopped state that should be set for exit_PCA9685
    
  def exit_PCA9685(self):
    self.write(self.__MODE2, 0x00)#Please use initialization or __MODE2 =0x04

JetsonNano

舵机云台代码为：

#!/usr/bin/python
import time
import RPi.GPIO as GPIO
from PCA9685 import PCA9685

# try:
#print "This is an PCA9685 routine"
pwm = PCA9685()
pwm.setPWMFreq(50)
#pwm.setServoPulse(1,500) 
pwm.setRotationAngle(1, 0)
while True:
    # setServoPulse(2,2500)
    for i in range(10,170,1): 
        pwm.setRotationAngle(1, i)   
        if(i<80):
            pwm.setRotationAngle(0, i)   
        time.sleep(0.1)

    for i in range(170,10,-1): 
        pwm.setRotationAngle(1, i)   
        if(i<80):
            pwm.setRotationAngle(0, i)            
        time.sleep(0.1)

# except:
    # pwm.exit_PCA9685()
    # GPIO.cleanup()
    # print "\nProgram end"
    # exit()

树莓派

舵机云台代码为：
/*

• 作者：PrairieOne
• csdn：PrairieOne
• 邮箱：prairieone1024@163.com
• 嵌入式技术交流群：826251093
  */

#!/usr/bin/python
import time
import RPi.GPIO as GPIO
from PCA9685 import PCA9685

pwm = PCA9685()
try:
    print ("This is an PCA9685 routine")    
    pwm.setPWMFreq(50)
    #pwm.setServoPulse(1,500) 
    pwm.setRotationAngle(1, 0)
    
    while True:
        # setServoPulse(2,2500)
        for i in range(10,170,1): 
            pwm.setRotationAngle(1, i)   
            if(i<80):
                pwm.setRotationAngle(0, i)   
            time.sleep(0.1)

        for i in range(170,10,-1): 
            pwm.setRotationAngle(1, i)   
            if(i<80):
                pwm.setRotationAngle(0, i)            
            time.sleep(0.1)

except:
    pwm.exit_PCA9685()
    print "\nProgram end"
    exit()

遇事不决，可问春风！