PCA9685芯片手册学习求教

概述

项目使用树莓派和PCA9685模块连接舵机进行控制，参照pca9685模块手册写了驱动代码，但是不能按照预期进行控制舵机。
现将代码提供，希望大佬能够指点一二。万分感激。

    %PCA9685 此处提供此类的摘要
    %   初始化输入参数：芯片地址默认（0x40),输出通道(ch),输出频率(freq)
    %   pwm输出参数：脉宽pulse_widthXCZ范围默认(500us，2500us),输出占空比(0-100)

    properties
        device
        i2cdevice
        bus='i2c-1' %设备连接到i2c-1
        address = '0x40' %设备地址默认0x40
        freq = 50 % 设置输出频率默认50Hz，周期20ms，最小24Hz，'0xff'，最大1526Hz,'0x03'        duty=0 % 输出占空比
        delay=0 %无延时
    end
    properties(Access=private)
        MODE1 = 0x00
        MODE2 = 0x01
        SUBADR1 = 0x02
        SUBADR2 = 0x03
        SUBADR3 = 0x04
        ALLCALLADR=0x05
        LED0_ON_L=0x06
        LED0_ON_H=0x07
        LED0_OFF_L=0x08
        LED0_OFF_H=0x09
        ALL_LED_ON_L = 0xFA
        ALL_LED_ON_H = 0xFB
        ALL_LED_OFF_L = 0xFC
        ALL_LED_OFF_H = 0xFD
        PRE_SCALE = 0xFE
    end
    properties
        precaleval = 25e6
        alpha=1 %修正系数
    end

    methods
        function obj = PCA9685(mypi,bus,address)
            % 构造一个PCA9685对象，类似i2c sensor
            % 定义i2c地址。
               obj.device=mypi;
               obj.i2cdevice=i2cdev(obj.device,bus,address);
               obj.bus=bus;
               obj.address = address; 

        end

        function setPWMFreq(obj,value)
            oldmode=readRegister(obj.i2cdevice,obj.MODE1); % 读MODE1寄存器
            %检查bit7(RESTART)是1则清空bit4(SLEEP),延时1000us

            newmode=bitor(bitand(oldmode,hex2dec('7f')),hex2dec('10'));% SLEEP mode
            writeRegister(obj.i2cdevice,obj.MODE1,newmode);% set bit4(SLEEP)
            % tic;
            % for t=0.001:0.001:1/1000
            %     while toc<t
            %     end
            % end 
            pause(5e-3);
            %wait 1ms
            writeRegister(obj.i2cdevice,obj.MODE1,bitor(oldmode,hex2dec('a1')));
            obj.freq=value;
            prescale=round(obj.precaleval/(4096*obj.freq*obj.alpha))-1; %计算填入寄存器的值
            writeRegister(obj.i2cdevice,obj.PRE_SCALE,prescale,'uint16')%填入寄存器
            newmode=bitset(newmode,5,0); % Normal mode
            writeRegister(obj.i2cdevice,obj.MODE1,newmode);% 置于正常模式

        end

        function setAngle(obj,ch,angle)
            %ch为通道号[0,15]
            %setPWMDuty(ch,duty)
            %ch是通道号0-15

                cycle=1000/obj.freq;
                min_led_on_time=round(4096/cycle*500e-6);
                max_led_on_time=round(4096/cycle*2500e-6);
                led_off_time=min_led_on_time+round((max_led_on_time-min_led_on_time)*angle/180);
                writeRegister(obj.i2cdevice,obj.LED0_ON_H+ch*4,bitshift(0,-8,"uint16"));
                writeRegister(obj.i2cdevice,obj.LED0_ON_H+ch*4,bitand(0,hex2dec('ff')));
                writeRegister(obj.i2cdevice,obj.LED0_OFF_H+ch*4,bitshift(led_off_time,-8,"uint16"));
                writeRegister(obj.i2cdevice,obj.LED0_OFF_H+ch*4,bitand(led_off_time,hex2dec('ff')));
            

        end
        function setPWMDuty(obj,ch,on,off)
            min_led_on=102;
            max_led_on=512;
            if off<min_led_on
                off=min_led_on;
            end
            if off> max_led_on
                off=max_led_on;
            end
            writeRegister(obj.i2cdevice,obj.LED0_ON_H+ch*4,bitshift(on,-8,"uint16"));
            writeRegister(obj.i2cdevice,obj.LED0_ON_H+ch*4,bitand(on,hex2dec('ff')));
            writeRegister(obj.i2cdevice,obj.LED0_OFF_H+ch*4,bitshift(off,-8,"uint16"));
            writeRegister(obj.i2cdevice,obj.LED0_OFF_H+ch*4,bitand(off,hex2dec('ff')));
        end

    end
endab

树莓派使用mypi=raspi创建，然后可以创建servos=PCA9685(mypi,‘i2c-1’,‘0x40’)，创建一个i2c设备，设备连接能够正常建立，按照舵机使用参考代码，led_off_time的范围是104~512，但是实际使用时感觉响应不按照预期。请求大佬帮助指点错误和修正方法。。