gongchuan

#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)