智能车制作

标题: 电磁转向问题 [打印本页]

作者: 132424qdw    时间: 2016-4-29 17:39
标题: 电磁转向问题
/********************************车方向控制*********************************************************/
void Direction_Control(void)
{
volatile float D_sub;      //电磁传感器差值
volatile float D_add;      //电磁传感器和值
volatile float DIR_Val;    //电磁传感器的差除以和


Get_ADresult();
L3G4200_XYZ();
Left_Mag_vertical = ADValue0;
Right_Mag_vertical= ADValue1;
DIR_Gyro          = Gyro_Y;

D_sub             = (Right_Mag_vertical-Right_Mag) -(Left_Mag_vertical-Left_Mag);      //电磁传感器差值
D_add             = (Right_Mag_vertical-Right_Mag) +(Left_Mag_vertical-Left_Mag);      //电磁传感器和值
Last_DIR_Val = This_DIR_Val;
if(D_add<(Right_Mag+Left_Mag))
This_DIR_Val = 0;
else
{
   DIR_Val           = D_sub*DIR_P/D_add;
   DIR_Gyro          = DIR_Gyro - DIR_GYRO_ZERO; //减去方向陀螺仪零点
  DIR_Gyro         *= DIR_D;
   DIR_Val          += DIR_Gyro;
   This_DIR_Val = DIR_Val;
   D_sub = 0;
   D_add = 0;
}
DIRPWM4 = (This_DIR_Val - Last_DIR_Val)/4;  //(DIRPWM4是将方向的上次值与当前值作差,分四等分20毫秒送给电机的)
}
/*********************************车模速度控制******************************************************/
/***************************************************************************************************/
float speed=0;
float Car_Left_Speed=0;
float Car_Right_Speed=0;
void Car_Speed_Control(float exp)
{
   
register float delta=0;                   //当前差值

Car_Right_Speed=Right_Speed_Val;          //读入右编码器脉冲数值
Car_Left_Speed =Left_Speed_Val;           //读入左编码器脉冲数值

speed =(Car_Left_Speed+Car_Right_Speed)/2;
delta =exp -speed;                        //左右电机速度平均值与期望值做差得到偏差

integration+=delta;
SpeedControlOld = SpeedControlNew;                     //储存上一次控制量

SpeedControlNew = SPEED_P*delta +SPEED_I*integration;  //更新当前速度控制量
SPEEDPWM20      = (SpeedControlNew-SpeedControlOld)/20;//计算控制差值 得到速度20等分的PWM值
}
/***************************************************************************************************/
//函数:void Speed_Adjustment(int)
//参数:exp :期望值即给定值
//功能描述:调速函数
/***************************************************************************************************/
void Speed_Adjustment(void)        //调速函数
{
    float delta=0;                  //后期最好判断速度范围然后再设定细分系数
   delta=(SET_SPEED)/6.0;          //先进行多阶段匀速调节
   Old_speed=New_speed;
    New_speed=Old_speed+delta;
    SPEED_Adjust=New_speed;         //变档输出
   if(SPEED_Adjust>=SET_SPEED)
    {
     SPEED_Adjust=SET_SPEED;
//    Speed_OK=0;                    //变速停止
   }
}
/***************************************************************************************************/
//函数名: void PWM_OUT(void)
//
//函数功能:电机PWM输出
/***************************************************************************************************/
void PWM_OUT(void)                 //电机PWM输出
{
    float Left_Motor_Val=0;         //存储计算得到的控制电机所需的PWM数值
   float Right_Motor_Val=0;        //存储计算得到的控制电机所需的PWM数值
   
    //直立控制
   Left_Motor_Val  = STAND_PWM;    //左电机直立需补偿的数值
   Right_Motor_Val = STAND_PWM;    //右电机直立需补偿的数值

   //速度控制                                 
    Left_Motor_Val -=SPEED_PWM;     //加上速度控制值         
    Right_Motor_Val-=SPEED_PWM;     //加上速度控制值
   
    //方向控制   
    Left_Motor_Val += DIR_PWM;      //左右电机差速控制 方向控制
   Right_Motor_Val-= DIR_PWM;            
   
    if(Left_Motor_Val>0)           //设定左电机死区输出
       Left_Motor_Val+=Motor_MINPWM;
    if(Left_Motor_Val<0)
        Left_Motor_Val-=Motor_MINPWM;
   
    if(Right_Motor_Val>0)         //设定右电机死区输出
       Right_Motor_Val+=Motor_MINPWM;
    if(Right_Motor_Val<0)
        Right_Motor_Val-=Motor_MINPWM;
   
    //电机输出饱和保护
   if(Left_Motor_Val > PWM_MAX)  //左电机输出饱和保护
     Left_Motor_Val = PWM_MAX;     
    if(Left_Motor_Val < PWM_MIN)
      Left_Motor_Val = PWM_MIN;


    if(Right_Motor_Val > PWM_MAX) //右电机输出饱和保护
     Right_Motor_Val = PWM_MAX;   
    if(Right_Motor_Val < PWM_MIN)
      Right_Motor_Val = PWM_MIN;
    //单极性PWM输出
   if(Left_Motor_Val>= 0)          //从左电机输出
   {
      PWMDTY45 =0;                  
      PWMDTY67 = (int)(Left_Motor_Val);
    }               
    else
    {
      PWMDTY67 =0;
      PWMDTY45= -(int)(Left_Motor_Val);
    }

    if(Right_Motor_Val>= 0)         //从右电机输出
   {
      PWMDTY01 =0;               
      PWMDTY23 =(int)(Right_Motor_Val);
    }
    else
    {
      PWMDTY23 =0;
      PWMDTY01 = -(int)(Right_Motor_Val);
    }
}
/***************************************************************************************************/
//函数:void Car_Control(void)
//
//功能描述:车模总控制
/***************************************************************************************************/
void Car_Control(void)
{
    static uchar count=0;             //控制计数 用来区别控制
   static uchar speed_count=0;       //速度控制计数 包括测速、速度输出控制
   static uchar dir_count=0;         //方向输出控制
   count++;
   
    switch(count)
    {
      case 1:
      {
        Speed_Measure();              //测速 读入5ms的脉冲数  并清零计数器
       Left_Speed_Val  +=Left_Speed; //累加左电机速度测量值
       Right_Speed_Val +=Right_Speed;//累加右电机速度测量值
       Speed_Adjustment();           //进行变速控制
       return;
       }
      
      case 2:
      {
        swj();                      //数据输出在虚拟上位机显示
       return;
       }
      
      case 3:                       //直立控制   执行时间为:276us
      {                             
        Stand_Control();            //直立控制
       PWM_OUT();                   //PWM输出   
        return;
       }
      
      case 4:                       //20次计数 并进行速度控制   每次速度控制数值分成20等分加入  执行时间为:48us--4us
      {
        speed_count++;
        if(speed_count==20)
        {
           speed_count=0;          //表示电机调速100毫秒已到,需要清零
          Car_Speed_Control(SPEED_Adjust);
           SPEED_PWM  = SpeedControlOld;   //加入原有速度控制  
           Left_Speed_Val  = 0;    //清零左电机速度测量值
          Right_Speed_Val = 0;    //清零右电机速度测量值            
         }
        SPEED_PWM +=SPEEDPWM20;            
        return;               
      }
      
      case 5:
      {
        dir_count++;
        if(dir_count == 4)
        {
        dir_count = 0;
        Direction_Control();
        DIR_PWM = Last_DIR_Val;
        }
        DIR_PWM +=DIRPWM4;
        count = 0;
        return;
      }
   }
}

哪位大神帮我瞅瞅,关于方向那块,无论怎么调,都调不到卓晴老师视频讲解的那样,还有就是感觉方向的D值没用啊,但是传感器电磁那块测试应该是没问题的,不胜感激!!!

作者: 滚滚红尘56    时间: 2016-5-9 12:26
同感,D值没什么用
作者: QQ游客    时间: 2016-5-9 23:58
加D发抖
作者: clarkhongso7    时间: 2016-5-10 07:26
同感,加d没用,两边转向不一致
作者: znmd308    时间: 2016-5-10 15:46
陀螺仪x方向作为D值肯定还是有用的,但是在这里和传统的位置式PID公式里面的D不一样,

传统的PID里的D值有预判作用,即加速转向,用传统的位置式PD可以让车抓线很紧,

陀螺仪的x方向作为D 对转向有阻止作用,可以让转向更平滑,

二者可以结合使用;

具体调试方法,可以先用传统位置式PD,调到可以很好的抓线后再加陀螺仪x方向实现平滑转向,


作者: znmd308    时间: 2016-5-10 15:50
clarkhongso7 发表于 2016-5-10 07:26
同感,加d没用,两边转向不一致

转向不一致太正常了

1:左右电机性能不同

2:电感肯定也不会对此

3:车模的重心肯定不在正中间

...
...
所有大概一致就可以,正反向可以跑玩就OK

作者: czy扬扬    时间: 2016-5-11 18:49
不要期望小车边跑边发数据,本来直立车就是不稳定体,加之串口发送数据很费时间(波特率为9600时,发送4个数字,大约费时5ms,如果使用虚拟示波器发送一路信号大约费时8ms),影响直立控制,表现为小车发一次数据,小车抖一下,这种情况下测得的速度也是不可靠的。
作者: 132424qdw    时间: 2016-6-7 14:54
滚滚红尘56 发表于 2016-5-9 12:26
同感,D值没什么用

那个问题已经解决是,方向D符号的原因,可能是我陀螺仪模块放的方向有问题,把符号换成负号就可以了

作者: 132424qdw    时间: 2016-6-7 14:55
czy扬扬 发表于 2016-5-11 18:49
不要期望小车边跑边发数据,本来直立车就是不稳定体,加之串口发送数据很费时间(波特率为9600时,发送4个 ...

嗯嗯,是的,我现在是把数据发送这块给屏蔽了,但是速度这一块感觉又有点问题

作者: 孤舟月    时间: 2016-6-8 17:14
请问楼主,你们的速度调了有反应吗,我感觉一直没反应
作者: czy扬扬    时间: 2016-6-24 21:41
132424qdw 发表于 2016-6-7 14:55
嗯嗯,是的,我现在是把数据发送这块给屏蔽了,但是速度这一块感觉又有点问题

现在我也感觉我的程序哪里有BUG了



作者: wumin0566    时间: 2016-7-4 11:32
你的D可能方向反了




欢迎光临 智能车制作 (http://111.231.132.190/) Powered by Discuz! X3.2