亿加合和智能车制作

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

作者: 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://dns.znczz.com/)