求指导如何调节速度PID保持相对静止的直立。。。。。

小钟

参考官方资料将代码加进去，没有出现错误，可是在调节速度PID的时候，基本没有反应，，，，这个是什么原因？下面是我的代码void SpeedControl(int bmq_l,int bmq_r) {
        float fDelta;
        float fDelta_cha;
        float fDelta_old,fD;
        float fP, fI;

        g_fCarSpeed = (bmq_l + bmq_r) / 2;
        g_fCarSpeed *= CAR_SPEED_CONSTANT;             //计算出电机转速
       
        fDelta = CAR_SPEED_SETfDelta - g_fCarSpeed;         //给定速-电机实际转
       
        fP = fDelta * SPEED_CONTROL_P;               //计算p
        fI = fDelta * SPEED_CONTROL_I;              //计算I
        g_fSpeedControlIntegral += fI;               
        /* if(g_fSpeedControlIntegral>=10)
        {g_fSpeedControlIntegral=10;}
        if(g_fSpeedControlIntegral<=-10)
        {g_fSpeedControlIntegral=-10;}
        if(fP>=1000)
        {fP=100;}
        if(fP<=-1000)
        {fP=-1000;} */
        fDelta_cha = fDelta - fDelta_old;       
        fD = fDelta_cha * SPEED_CONTROL_D;
       // if(fD>=220)
        //{fD=220;}
      //  if(fD<=-220)
        //{fD=-220;}
        g_fSpeedControlOutOld = g_fSpeedControlOutNew;
        g_fSpeedControlOutNew = fP +fD+ g_fSpeedControlIntegral;
        fDelta_old=fDelta;
        }
/************速度输出函数（编码器）**********/
void SpeedControlOutput(void)
{
  float fValue;
  fValue=g_fSpeedControlOutNew - g_fSpeedControlOutOld;
  g_fSpeedControlOut=fValue*(g_nSpeedControlPeriod)/100+g_fSpeedControlOutOld;
  g_nSpeedControlPeriod++;
}



/***********************************************************************/
/*
*  功能说明：直立速度计算
*  参数说明：angle                融合后最终角度
*            angle_dot            陀螺仪角速度
*
*  函数返回：无符号结果值
*  修改时间：2013-2-10
* 备注：参考清华源码
*/


/**************************速度闭环程序****************************************/

void MotorOutput(void)
{
  g_fLeftMotorOut = speed_Start - g_fSpeedControlOut;
  g_fRightMotorOut = speed_Start - g_fSpeedControlOut;

}


/******************************************************************************/
void Speed_Calculate(float angle,float angle_dot)
{
    /***********************************速度计算************************************/
    speed_Start = angle * P_ANGLE  + angle_dot * D_ANGLE ;  //直立时所要的速度
   // printf("\n转速:%d",speed_Start);
    MotorOutput();
    //P_ANGLE  P_GYRO  宏定义 直立所需要的PD参数

    Speed_L =g_fLeftMotorOut;//speed_Start;//左轮总速度
   // printf("\n左轮R:%d",Speed_L);
    Speed_R =g_fRightMotorOut;//speed_Start;//右轮总速度
   // printf("\n右轮R：%d",Speed_R);
    /***********************将最大速度限制在985个PWM内******************************/
    if(Speed_L > 985)  Speed_L = 985;
    if(Speed_L < -985) Speed_L = -985;
    if(Speed_R > 985)  Speed_R = 985;
    if(Speed_R < -985) Speed_R = -985;

    /***************因为驱动部分加了反相器，所以需对速度进行一个最终的处理******************/
    if(Speed_L > 0)     //因为加了反相器，所以PWM要反过来添加
        Speed_L_Last =  Speed_L;
    else
        Speed_L_Last = Speed_L;

    if(Speed_R > 0)     //因为加了反相器，所以PWM要反过来添加
        Speed_R_Last =  Speed_R;
    else
        Speed_R_Last =   Speed_R;

    /*************用所得到的对应角度的速度进行PWM控制********************/
    if(Speed_L >= 0)    //angle大于0，向前，小于0，向后
    {
        FTM_PWM_Duty(FTM0,FTM_CH3,(uint32)(Speed_L_Last-MOTOR_DEAD_VAL_L));    //加入死区电压
        FTM_PWM_Duty(FTM0,FTM_CH4,0);

    }
    else
    {
        FTM_PWM_Duty(FTM0,FTM_CH3,0);
        FTM_PWM_Duty(FTM0,FTM_CH4,(uint32)(-Speed_L_Last-MOTOR_DEAD_VAL_L));  //加入死区电压
    }

    if(Speed_R >= 0)    //angle大于0，向前，小于0，向后
    {
        FTM_PWM_Duty(FTM0,FTM_CH6,0);
        FTM_PWM_Duty(FTM0,FTM_CH5,(uint32)(Speed_R_Last-MOTOR_DEAD_VAL_R));    //加入死区电压
    }
    else
    {
        FTM_PWM_Duty(FTM0,FTM_CH5,0);
        FTM_PWM_Duty(FTM0,FTM_CH6,(uint32)(-Speed_R_Last-MOTOR_DEAD_VAL_R));   //加入死区电压
    }
}

戥家三少

差不多这样的问题，但我们的是跪下了就不能动了

小钟

戥家三少 发表于 2014-3-24 14:40
差不多这样的问题，但我们的是跪下了就不能动了

那你们解决没？

戥家三少

小钟 发表于 2014-3-24 18:07
那你们解决没？

解决了，静电问题

小钟

戥家三少 发表于 2014-3-24 18:20
解决了，静电问题

静电问题？也就是说是硬件电路的干扰么？   你们调节加入速度PID的使车模直立的程序，，和我发的这个类似么？我这个程序是不是还缺少什么部分？

戥家三少

小钟 发表于 2014-3-24 19:44
静电问题？也就是说是硬件电路的干扰么？   你们调节加入速度PID的使车模直立的程序，，和我发的这个类似 ...

没看你的程序，我们都改成自己能看懂的名称了

小钟

戥家三少 发表于 2014-3-24 19:55
没看你的程序，我们都改成自己能看懂的名称了

哦哦，你是那个赛区的，，，可以参考下你们的那个速度这个环节的程序么，，，因为我不确定自己速度环节代码是不是缺少什么，，，，调节的时候基本没有反应

海涛制作

我的这段程序出现问题了
void Speed_Calculate(float angle,float angle_dot)
{
    /***********************************速度计算************************************/
    speed_Start = angle * P_ANGLE  + angle_dot * D_ANGLE ;  //直立时所要的速度
   // printf("\n转速:%d",speed_Start);
    MotorOutput();
    //P_ANGLE  P_GYRO  宏定义 直立所需要的PD参数

    Speed_L =g_fLeftMotorOut;//speed_Start;//左轮总速度
   // printf("\n左轮R:%d",Speed_L);
    Speed_R =g_fRightMotorOut;//speed_Start;//右轮总速度
   // printf("\n右轮R：%d",Speed_R);
    /***********************将最大速度限制在985个PWM内******************************/
    if(Speed_L > 985)  Speed_L = 985;
    if(Speed_L < -985) Speed_L = -985;
    if(Speed_R > 985)  Speed_R = 985;
    if(Speed_R < -985) Speed_R = -985;
输出的不是985 怎么办

小钟

海涛制作 发表于 2014-3-25 12:32
我的这段程序出现问题了
void Speed_Calculate(float angle,float angle_dot)
{

不会吧，这个是不是你的硬件干扰比较大

小钟

戥家三少 发表于 2014-3-24 18:20
解决了，静电问题

你们处理静电干扰是怎么处理的？求指导下