# -*- coding:utf-8 -*-
import time
#引入pca9685的原始代碼
import machine
#這裏加入了燈光展示
from light import Light
#引入mqtt的paho
import paho.mqtt.client as mqtt
#本地ip
MQTTHOST = "127.0.0.1"
#mqtt的端口號
MQTTPORT = 1883
#初始化mqttClient
mqttClient = mqtt.Client()
#初始化pwm
pwm = machine.PCA9685(0x40, debug=False)
#設置pwm的頻率
pwm.setPWMFreq(50)
#設置初始上下位置爲中間,(500+2500)/2 = 1500
up = 1500
#設置初始左右位置爲中間
dw = 1500
#設置燈的接口位置
led = Light(17)
# 消息處理函數
def on_message_come(client, userdata, msg):
topicmsg = str(msg.payload.decode("utf-8"))
msgList = topicmsg.split(',', 1)
#evalFunc的作用是將接受的命令字符串變成對應的方法進行執行
evalFunc(msgList[0], msgList[1])
# 連接MQTT服務器
def on_mqtt_connect():
mqttClient.connect(MQTTHOST, MQTTPORT, 60)
# subscribe 消息訂閱
def on_subscribe(top):
mqttClient.subscribe(top, 1) # 主題爲"mqtt"
mqttClient.on_message = on_message_come # 消息到來處理函數
mqttClient.loop_forever()
def evalFunc(func, n):
#例如獲取到了mqtt消息,right,1 ,這個表示執行向右1個單位,一共10個單位
print(str(func) + '(' + str(n) + ')')
#eval可以調用方法進行執行
eval(str(func) + '(' + str(n) + ')')
#這個是恢復到初始位置
def reset(n):
led.blink()
global up
global dw
for i in range(1400,1500,1):
pwm.setServoPulse(0,i)
time.sleep(0.00025)
for i in range(1400,1500,1):
pwm.setServoPulse(1,i)
time.sleep(0.00025)
up = 1500
dw = 1500
#這個是向前進的代碼
def front(n):
n = int(n)
#這裏我自定義n乘以5,是因爲我之前每次執行一個單位,我想一次性執行5個單位的向下
n = n * 5
#燈光閃爍
led.blink()
global up
#這裏會判斷是否已經到了閾值範圍,如果到了則不執行下一步
if up != 2500:
#開啓循環進行,從up到up+100*n,每次走100 * n個距離,當然這裏你可以10 * n,這樣舵機移動的角度非常小非常小
#我這裏500~2500一共是2000個,分20下100 * n,當n爲1時,則移動20下從最前邊到最後邊,角度大概是從0度到180度這樣,具體度數描述不太準確,請見諒
#現在n = n * 5,意味着我只需要4下就從最前邊到最後面,具體根據你的度數範圍來,還有根據你移動的角度大小來決定舵機的速度
for i in range(up,up+100*n,1*n):
pwm.setServoPulse(0,i)
time.sleep(0.00025)
#每次執行完畢後,up都會變化
up += 100*n
#這裏防止up變大超過2500,則變爲2500
if up > 2500:
up = 2500
def back(n):
n = int(n)
n = n * 5
led.blink()
global up
if up != 500:
for i in range(up,up-100*n,-1*n):
pwm.setServoPulse(0,i)
time.sleep(0.00025)
up -= 100*n
if up < 500:
up = 500
def left(n):
n = int(n)
n = n * 2
led.blink()
global dw
if dw != 2500:
for i in range(dw,dw+100*n,1*n):
pwm.setServoPulse(1,i)
time.sleep(0.00025)
dw += 100*n
if dw > 2500:
dw = 2500
def right(n):
n = int(n)
n = n * 2
led.blink()
global dw
if dw != 500:
for i in range(dw,dw-100*n,-1*n):
pwm.setServoPulse(1,i)
time.sleep(0.00025)
dw -= 100*n
if dw < 500:
dw = 500
if __name__ == '__main__':
topic='mqtt'
on_mqtt_connect()
on_subscribe(topic)
下面是machine的代碼,也就是PCA9685的python3測試代碼
#!/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
__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)
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))
if __name__=='__main__':
pwm = PCA9685(0x40, debug=False)
pwm.setPWMFreq(50)
while True:
# setServoPulse(2,2500)
for i in range(500,2500,10):
pwm.setServoPulse(0,i)
time.sleep(0.02)
for i in range(2500,500,-10):
pwm.setServoPulse(0,i)
time.sleep(0.02)
Python2的代碼在此
#!/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
__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)
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))
if __name__=='__main__':
pwm = PCA9685(0x40, debug=False)
pwm.setPWMFreq(50)
while True:
# setServoPulse(2,2500)
for i in range(500,2500,10):
pwm.setServoPulse(0,i)
time.sleep(0.02)
for i in range(2500,500,-10):
pwm.setServoPulse(0,i)
time.sleep(0.02)
具體的代碼在此,包括python2和python3還有C的代碼都在此,想多瞭解的可以下載一下,也可以不下載,代碼也貼出來了,點擊下載