电磁转向问题

132424qdw

/********************************车方向控制*********************************************************/
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

同感，D值没什么用

wumin0566

你的D可能方向反了

czy扬扬

132424qdw 发表于 2016-6-7 14:55
嗯嗯，是的，我现在是把数据发送这块给屏蔽了，但是速度这一块感觉又有点问题

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

孤舟月

请问楼主，你们的速度调了有反应吗，我感觉一直没反应

132424qdw

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

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

132424qdw

滚滚红尘56 发表于 2016-5-9 12:26
同感，D值没什么用

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

czy扬扬

不要期望小车边跑边发数据，本来直立车就是不稳定体，加之串口发送数据很费时间（波特率为9600时，发送4个数字，大约费时5ms，如果使用虚拟示波器发送一路信号大约费时8ms），影响直立控制，表现为小车发一次数据，小车抖一下，这种情况下测得的速度也是不可靠的。

znmd308

clarkhongso7 发表于 2016-5-10 07:26
同感，加d没用，两边转向不一致

转向不一致太正常了

1:左右电机性能不同

2:电感肯定也不会对此

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

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

znmd308

陀螺仪x方向作为D值肯定还是有用的，但是在这里和传统的位置式PID公式里面的D不一样，

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

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

二者可以结合使用;

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

clarkhongso7

同感，加d没用，两边转向不一致