新人第一次做直立，调到前进了，有些问题还请各位大大帮忙。

Riot_Kobs

现在的状况是可以直立，但是需要调整角度零偏移值，也就是说它打开后是直立着往某一个方向加速前进，直立只用了PD调节，但是当零偏移角度调整的不错时加上积分后不像各位大大们说的那样推出去会立刻回到原地。

程序贴出来：
void Get_Ang()          //1ms 计算并更新电机状态一次
{     
        u16 ADresult;
        //保存ADC转换结
          /****** 读取一次 ******/
        ADresult    =   ad_once(ADC1, SE4a, ADC_16bit);         //读取 ADC1_SE4a ，16位精度
        A_X=ADresult;
        ADresult    =   ad_once(ADC1, SE5a, ADC_16bit);         //读取 ADC1_SE5a ，16位精度
        A_Z=ADresult;
        ADresult    =   ad_mid(ADC1, SE6a, ADC_16bit);         //读取 ADC1_SE6a ，16位精度
        GYRO_X=ADresult-44268;
        ADresult    =   ad_mid(ADC1, SE7a, ADC_16bit);         //读取 ADC1_SE7a ，16位精度
        GYRO_Y=ADresult-44268;

        Ang_Acc=-57.3*atan2(A_X-32000,A_Z-32000)-Ang_Set;//角度

        Ang_IGyro=Ang_IGyro+GYRO_Y*0.000016;//积分得到的角度

        GYRO_Y_LI=0.00016*GYRO_Y;

        //互补滤波
        Ang_Error=Ang_Acc-Ang_IGyro;
        Ang_IGyro=Ang_IGyro+Ang_Error*0.001;

        Ang=Ang_IGyro;

        IAng=IAng+0.1*Ang;

        Speed_1 = Ang*117 + GYRO_Y*0.06+IAng*0.00;//PID


        Go(Speed_1,Speed_1);

}


这个发出来求个喷，想弄的完美点毕竟是基础部分很重要的

现在碰到最大的难题是：我们用的编码器是单线输出，无法判断车轮的正转和反转，不知道如何解决，请各位大神帮忙指点了。