#main.py
import RopoDevice
import RopoServo
import cv2
color_cap,servos = RopoDevice.Init()
while True:
# get a frame
ret, frame = color_cap.read()
color_cap.release()
cv2.destroyAllWindows()
#pca9685.py
import utime
import ustruct
class PCA9685:
def __init__(self, i2c, address=0x40):
self.i2c = i2c
self.address = address
self.reset()
def _write(self, address, value):
self.i2c.writeto_mem(self.address, address, bytearray([value]))
def _read(self, address):
return self.i2c.readfrom_mem(self.address, address, 1)[0]
def reset(self):
self._write(0x00, 0x00) # Mode1
def freq(self, freq=None):
if freq is None:
return int(25000000.0 / 4096 / (self._read(0xfe) - 0.5))
prescale = int(25000000.0 / 4096.0 / freq + 0.5)
old_mode = self._read(0x00) # Mode 1
self._write(0x00, (old_mode & 0x7F) | 0x10) # Mode 1, sleep
self._write(0xfe, prescale) # Prescale
self._write(0x00, old_mode) # Mode 1
utime.sleep_us(5)
self._write(0x00, old_mode | 0xa1) # Mode 1, autoincrement on
def pwm(self, index, on=None, off=None):
if on is None or off is None:
data = self.i2c.readfrom_mem(self.address, 0x06 + 4 * index, 4)
return ustruct.unpack('<HH', data)
data = ustruct.pack('<HH', on, off)
self.i2c.writeto_mem(self.address, 0x06 + 4 * index, data)
def duty(self, index, value=None, invert=False):
if value is None:
pwm = self.pwm(index)
if pwm == (0, 4096):
value = 0
elif pwm == (4096, 0):
value = 4095
value = pwm[1]
if invert:
value = 4095 - value
return value
if not 0 <= value <= 4095:
raise ValueError("Out of range")
if invert:
value = 4095 - value
if value == 0:
self.pwm(index, 0, 4096)
elif value == 4095:
self.pwm(index, 4096, 0)
else:
self.pwm(index, 0, value)
import math
import time
import pca9685
def convert(x):
''' convert 16 bit int x into two 8 bit ints, coarse and fine.
'''
c = x >> 8 # The value of x shifted 8 bits to the right, creating coarse.
f = x % 256 # The remainder of x / 256, creating fine.
return c, f
def low_byte(x):
return convert(x)[0]
def high_byte(x):
return convert(x)[1]
# class ServoGroup:
# def __init__(self, serial, i2c, address=0x40, freq=50, min_us=600, max_us=2400, degrees=180):
# self.period = 1000000 / freq
# self.min_duty = self._us2duty(min_us)
# self.max_duty = self._us2duty(max_us)
# self.degrees = degrees
# self.freq = freq
# self.pca9685 = pca9685.PCA9685(i2c, address)
# self.pca9685.freq(freq)
# self.serial = serial
# def _us2duty(self, value):
# return int(4095 * value / self.period)
# def filter_servo(self,id_servo,x,v=50,a=0):
# #1 2940-3650 顺时针
# #2 250-2650 顺时针
# #3 650-3750 从高到低
# #4 1400-3750 里到外
# length = 10
# instruction = 3
# Time = 0
# address = 41
# check = (id_servo+length+address+instruction+x%256+x//256+v%256+v//256+a)%256
# check = 255-check
# data = b'\xff\xff'+id_servo.to_bytes(1, 'little')+length.to_bytes(1,'little')+instruction.to_bytes(1,'little')
# data = data+address.to_bytes(1,'little')+a.to_bytes(1,'little')+x.to_bytes(2,'little')+Time.to_bytes(2,'little')
# data = data+v.to_bytes(2,'little')+check.to_bytes(1,'little')
# self.serial.write(data)
# time.sleep(1)
# def common_servo(self, index, degrees=None, radians=None, us=None, duty=None):
# span = self.max_duty - self.min_duty
# if degrees is not None:
# duty = self.min_duty + span * degrees / self.degrees
# elif radians is not None:
# duty = self.min_duty + span * radians / math.radians(self.degrees)
# elif us is not None:
# duty = self._us2duty(us)
# elif duty is not None:
# pass
# else:
# return self.pca9685.duty(index)
# duty = min(self.max_duty, max(self.min_duty, int(duty)))
# self.pca9685.duty(index, duty)
# def release(self, index):
# self.pca9685.duty(index, 0)
class ServoGroup:
def __init__(self, serial):
self.serial = serial
def filter_servo(self,id_servo,x,v=50,a=0):
#1 2940-3650 顺时针
#2 250-2650 顺时针
#3 650-3750 从高到低
#4 1400-3750 里到外
length = 10
instruction = 3
Time = 0
address = 41
check = (id_servo+length+address+instruction+x%256+x//256+v%256+v//256+a)%256
check = 255-check
data = b'\xff\xff'+id_servo.to_bytes(1, 'little')+length.to_bytes(1,'little')+instruction.to_bytes(1,'little')
data = data+address.to_bytes(1,'little')+a.to_bytes(1,'little')+x.to_bytes(2,'little')+Time.to_bytes(2,'little')
data = data+v.to_bytes(2,'little')+check.to_bytes(1,'little')
self.serial.write(data)
time.sleep(1)
#RopoDevice.py
import os
import cv2
import numpy as np
import serial
import math
import time
from board import SCL, SDA
import busio
import RopoServo
def gstreamer_pipeline(
capture_width=1920, #摄像头预捕获的图像宽度
capture_height=1080, #摄像头预捕获的图像高度
display_width=1920, #窗口显示的图像宽度
display_height=1080, #窗口显示的图像高度
framerate=60, #捕获帧率
flip_method=0, #是否旋转图像
):
return (
"nvarguscamerasrc ! "
"video/x-raw(memory:NVMM), "
"width=(int)%d, height=(int)%d, "
"format=(string)NV12, framerate=(fraction)%d/1 ! "
"nvvidconv flip-method=%d ! "
"video/x-raw, width=(int)%d, height=(int)%d, format=(string)BGRx ! "
"videoconvert ! "
"video/x-raw, format=(string)BGR ! appsink"
% (
capture_width,
capture_height,
framerate,
flip_method,
display_width,
display_height,
)
)
def Init():
#根据摄像头参数设置
capture_width = 640 #
capture_height = 480
framerate = 60 # 帧数
#展示图片大小
display_width = 640
display_height = 480
flip_method = 0 # 方向
color_cap = cv2.VideoCapture(gstreamer_pipeline(capture_width,
capture_height,
display_width,
display_height,
framerate,
flip_method),
cv2.CAP_GSTREAMER)
# code_cap = cv2.VideoCapture(gstreamer_pipeline(capture_width,
# capture_height,
# display_width,
# display_height,
# framerate,
# flip_method),
# cv2.CAP_GSTREAMER)
Servo_serial = serial.Serial( port="/dev/ttyS0",
baudrate=115200,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
)
# Stm_serail
i2c = busio.I2C(SCL, SDA)
servos = RopoServo.ServoGroup(Servo_serial,i2c, address=0x40, freq=50, min_us=500, max_us=2500, degrees=360)
return color_cap,servos
#RopoServo.py
import math
import time
import pca9685
def convert(x):
''' convert 16 bit int x into two 8 bit ints, coarse and fine.
'''
c = x >> 8 # The value of x shifted 8 bits to the right, creating coarse.
f = x % 256 # The remainder of x / 256, creating fine.
return c, f
def low_byte(x):
return convert(x)[0]
def high_byte(x):
return convert(x)[1]
class ServoGroup:
def __init__(self, serial, i2c, address=0x40, freq=50, min_us=600, max_us=2400, degrees=180):
self.period = 1000000 / freq
self.min_duty = self._us2duty(min_us)
self.max_duty = self._us2duty(max_us)
self.degrees = degrees
self.freq = freq
self.pca9685 = pca9685.PCA9685(i2c, address)
self.pca9685.freq(freq)
self.serial = serial
def _us2duty(self, value):
return int(4095 * value / self.period)
def filter_servo(self,id_servo,x,v=50,a=0):
#1 2940-3650 顺时针
#2 250-2650 顺时针
#3 650-3750 从高到低
#4 1400-3750 里到外
length = 10
instruction = 3
Time = 0
address = 41
check = (id_servo+length+address+instruction+x%256+x//256+v%256+v//256+a)%256
check = 255-check
data = b'\xff\xff'+id_servo.to_bytes(1, 'little')+length.to_bytes(1,'little')+instruction.to_bytes(1,'little')
data = data+address.to_bytes(1,'little')+a.to_bytes(1,'little')+x.to_bytes(2,'little')+Time.to_bytes(2,'little')
data = data+v.to_bytes(2,'little')+check.to_bytes(1,'little')
self.serial.write(data)
time.sleep(1)
def common_servo(self, index, degrees=None, radians=None, us=None, duty=None):
span = self.max_duty - self.min_duty
if degrees is not None:
duty = self.min_duty + span * degrees / self.degrees
elif radians is not None:
duty = self.min_duty + span * radians / math.radians(self.degrees)
elif us is not None:
duty = self._us2duty(us)
elif duty is not None:
pass
else:
return self.pca9685.duty(index)
duty = min(self.max_duty, max(self.min_duty, int(duty)))
self.pca9685.duty(index, duty)
def release(self, index):
self.pca9685.duty(index, 0)