链接
    这个函数已经完成了，我就不再继续写下去了
    相关代码：
    #include
    /**定义部分*/
    //———————-下面定义电机控制信号输入接口
    //作用：控制电机转动方向
    //示例：ma1==HIGH，ma2==LOW，电机顺时针转

    const int ma1=2;//电机左+接口2
    const int ma2=3;//电机左-接口3
    const int mb1=4;//电机右+接口4
    const int mb2=5;//电机右-接口5
    const int mc1=6;//以此类推
    const int mc2=7;
    const int md1=8;
    const int md2=9;
    //————————-下面定义电机PWM信号输出口
    //作用：控制电机的转动速度
    const int Ena=20;//电机A转速输入口
    const int Enb=19;//电机B转速输入口
    const int Enc=18;//以此类推
    const int End=21;
    //————————-
    /const int Q1=28;//B相输入
    const int Q2=26;
    const int Q3=22;
    const int Q4=24;    /
    bool rev1,rev2,rev3,rev4;//方向
    //————————-上面是中断读取霍尔传感器转速端口
    int mafor,mago,maturn;
    int mbfor,mbgo,mbturn;
    int mcfor,mcgo,mcturn;
    int mdfor,mdgo,mdturn;
    //————————-4电机的旋转,平移,前后值
    const int L=1015,R=1035;
    double M1PWMOUT,M2PWMOUT,M3PWMOUT,M4PWMOUT;//M1输出pwm，M2输出pwm，以此类推
    double ref1,ref2,ref3,ref4;//四个电机的参考转速
    double in1,in2,in3,in4,M1S,M2S,M3S,M4S;//in1-4是脉冲的个数，M1-4S是转换成转速（没有单位）后的值
    double Kp=6, Ki=1.3, Kd=0.1; //PID系数
    unsigned long t;