极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

楼主: It's_me

基于Arduino+MPU6050+Tp-link 703n平衡车完美站立(部分代码上传)

  [复制链接]
发表于 2014-4-28 12:38:31 | 显示全部楼层
现在就是向一个方向小车倾斜的时候可以,但是由于l298n的顺时针和逆时针的高低电平设定好了,所以无论怎么调也都是一个方向,从PID输出的Output值虽然有正负,给到IN1和IN2的时候正负的转速值不会改变电机的方向。。。求教啊~怎么能变换两个方向?(用了IF语句也不行。。)
unsigned char Re_buf[11],counter=0;
unsigned char sign=0;
float a[3],w[3],Angle[3],T;
short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
int Pwm_out = 0;
int Turn_Need = 0;
int Speed_Need = 0;
int speed_output_R , speed_output_L;

int speedoutputL = 13;
int speedoutputR = 8;
int L1 =12;
int L2 =11;
int R1 =10;
int R2 =9;

long count_left = 0;
long count_right = 0;

float speeds , speeds_filter, positions;
float diff_speeds,diff_speeds_all;




void setup() {
  // initialize serial:
  Serial.begin(115200);
  Serial3.begin(115200);
  pinMode(8,OUTPUT);
  pinMode(9,OUTPUT);
  pinMode(10,OUTPUT);
  pinMode(11,OUTPUT);
  pinMode(12,OUTPUT);
  pinMode(13,OUTPUT);
}






void loop()
{
  serialEvent();
  Serial.print(Pwm_out);
  Serial.println(",");
  Serial.print("angle:");
  Serial.print(Angle[0]);
  Serial.println(",");  
  Serial.print("rad");
  Serial.print(w[0]);
  Serial.println(",");
  
  
float Kap = 4.8;       //
float Kad = 10;         //
float Ksp = 2.8;        //
float Ksi = 0.11;       //


  speeds=(count_left + count_right)*0.5;

  diff_speeds = count_left - count_right;
  diff_speeds_all += diff_speeds;

  speeds_filter *=0.85;  //一阶互补滤波
  speeds_filter +=speeds*0.15;

  positions += speeds_filter;
  positions += Speed_Need;
  positions = constrain(positions, -2300, 2300);//抗积分饱和

  Pwm_out = Kap*Angle[0] + Kad*w[0] + Ksp*speeds_filter + Ksi*positions ;  //PID控制器
  if(Turn_Need == 0)
  {
    speed_output_R = int(Pwm_out - diff_speeds_all);
    speed_output_L = int(Pwm_out + diff_speeds_all);
  }
  speed_output_R = int(Pwm_out + Turn_Need);
  speed_output_L = int(Pwm_out - Turn_Need);

    digitalWrite(9,1);
    digitalWrite(10,0);
    digitalWrite(11,0);
    digitalWrite(12,1);
    analogWrite(speedoutputL,speed_output_L);
    analogWrite(speedoutputR,speed_output_R);
   
   
  count_left = 0;
  count_right = 0;
  
  
  
}
void serialEvent() {
  while (Serial3.available()) {
   
    //char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
  
    Re_buf[counter]=(unsigned char)Serial3.read();
    if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头              
    counter++;      
    if(counter==11)             //接收到11个数据
    {   
       counter=0;               //重新赋值,准备下一帧数据的接收
       switch(Re_buf [1])
        {
        case 0x51:
                a[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*16;
                a[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*16;
                a[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*16;               
                break;
        case 0x52:
                w[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*250;
                w[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*250;
                w[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*250;
                break;
        case 0x53:
                Angle[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*180;
                Angle[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*180;
                Angle[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*180;
                T =  float(short(Re_buf [9]<<8| Re_buf [8]));///340.0+36.25   
                sign=1;
                break;
        }
    }      
  }
}


//  Serial.print(Angle[0]);      //主调被调量
//  Serial.print(',');
//  Serial.print(speeds_filter);//副调被调量
//  Serial.print(',');
//  Serial.println(Pwm_out);  //输出量
回复 支持 反对

使用道具 举报

发表于 2014-4-28 13:38:50 | 显示全部楼层
该死的顺丰, 店家星期四发货....今天才送到, 害我不能在刚过去的星期六日开始.

今天可以认真一点尝试了, 再三看 PID 的资料, 还是似懂非懂的.
始终想不通如何由期望的平衡角度/速度, 转化成输出的 PWM 值, 当中的关系是如何建立的 (就是那经典的算法).

想请教楼主 算式中的 Kap, Kad, Ksp 及 Kai 的值, 是如何推算出来的.
我看到程式中是直接设定一些数值, 那些数值是怎样得出来的?  是计算? 量度? 还是推测出来的?  是否不同的电机, 会有不同的数值.  
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-4-28 19:16:31 | 显示全部楼层
Super169 发表于 2014-4-28 13:38
该死的顺丰, 店家星期四发货....今天才送到, 害我不能在刚过去的星期六日开始.

今天可以认真一点尝试了, ...

你学习过自动控制原理和现控原理吗?关于四个参数的值,需要建立小车的状态空间模型(State-space Model ),然后再根据模型去设计控制器(Controller),简单的说如果控制器是PID_Controller,那么设计完成之后就可以通过Mablab的仿真(Simulink)就可以得出这4个参数。我相信你目前还没有接触到这些,所以这四个参数需要用最笨的办法,就是一个一个的试验,当然他也有一个名称叫做试差法(Try and Error)。简单的说,你可以看一下整定PID参数的方法,遵循先比例(P),后积分(I),最后加入微分(D),先内环,后外环的整定顺序。简而言之,按你的现况,我建议你用试差法,然后你参照整定参数的方法去试,就可以实现了,至于参数是怎么来的你还需要学习自控相关知识。
回复 支持 反对

使用道具 举报

发表于 2014-4-28 23:18:11 | 显示全部楼层
楼主  MOTOR代码中
//MOTOR(int (+/-)Left_PWM,int (+/-)Right_PWM)  的 Left_PWM 以及 Right_PWM 的数据是怎么定义的呢?
还是它直接是从电机的码盘芯片读取的?
回复 支持 反对

使用道具 举报

发表于 2014-4-29 00:59:05 | 显示全部楼层
It's_me 发表于 2014-4-28 19:16
你学习过自动控制原理和现控原理吗?关于四个参数的值,需要建立小车的状态空间模型(State-space Model  ...


感谢楼主的资料, 看来真不容易.
试差法对於一个变数, 绝无问题, 两个变数得花点心思, 三个变数已经很难了.  四个变数, 互相影响的因素太多了.
看来只好使出我的投石问路法....(又名随机碰运气法).  就认真的, 我会慢慢研究, 但不是一时三刻可以明白, 暂时只好用试的方法.  

今天收到车架, 实在太多意外了....首先是顺丰的超级慢递, 再来是店家漏发东西, 再来是电机码盘位置问题, 装不上, 要改位重新焊接, 终於用我的瞬间平衡法比车子站"起"来了....不消数秒就要倒下了.

20140429-01.jpg

现在才是真正的开始, 由於送漏了安装用的柱子, 正考虑是否直接把板子用双面贴直接贴上去.
待我装好车子可以真的站起来後, 再向楼主请教 openwrt 及 Wifi 通讯的详情.
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-4-29 19:10:17 | 显示全部楼层
一叶萧然 发表于 2014-4-28 23:18
楼主  MOTOR代码中
//MOTOR(int (+/-)Left_PWM,int (+/-)Right_PWM)  的 Left_PWM 以及 Right_PWM 的数据是 ...

是pid算完以后的pwm值
回复 支持 反对

使用道具 举报

发表于 2014-4-29 22:58:13 | 显示全部楼层
本帖最后由 一叶萧然 于 2014-4-29 23:02 编辑

嗯还有楼主,就是如果我利用外部控制(我用的是无线PS2),是直接控制PID里的参数还是其他的什么能让它稳定前进呢?
是PID中的Turn_Need 吗?
回复 支持 反对

使用道具 举报

发表于 2014-4-30 11:23:44 | 显示全部楼层
楼主,就是一侧偏斜时(也就是输出值为正的时候),串口数据连续,2560上的PWM端口的端口灯可以由暗逐渐变亮(也就是电机随角度增加均匀转动),但是一旦向另一侧倾斜的时候(也就是输出值为负的时候),串口数据仍然显示连续,但是PWM端口的灯会突然变亮而不是又暗变亮,导致电机会突然转动,不知道楼主调试的时候是否遇到过这种问题?
回复 支持 反对

使用道具 举报

发表于 2014-4-30 15:13:12 | 显示全部楼层
一叶萧然 发表于 2014-4-30 11:23
楼主,就是一侧偏斜时(也就是输出值为正的时候),串口数据连续,2560上的PWM端口的端口灯可以由暗逐渐变亮 ...

已经解决了,是输出值符号写反了。
回复 支持 反对

使用道具 举报

发表于 2014-5-3 09:55:01 | 显示全部楼层
赞!非自控及相关专业的表示求PID相关的干货
回复 支持 反对

使用道具 举报

发表于 2014-5-3 20:11:16 | 显示全部楼层
楼主还是电机的问题,电机的引线(就是可以测出转速的两根线),具体是连接哪两个口?是直接INPUT然后analogread()读取换算吗?
回复 支持 反对

使用道具 举报

发表于 2014-5-3 22:30:09 | 显示全部楼层
中断要怎怎寫
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-4 09:40:52 | 显示全部楼层
一叶萧然 发表于 2014-5-3 20:11
楼主还是电机的问题,电机的引线(就是可以测出转速的两根线),具体是连接哪两个口?是直接INPUT然后analo ...

QQ截图20140504093828.png

你看一下这个码盘测速的原理,还是不懂我再给你讲讲

回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-4 09:43:06 | 显示全部楼层
一叶萧然 发表于 2014-4-29 22:58
嗯还有楼主,就是如果我利用外部控制(我用的是无线PS2),是直接控制PID里的参数还是其他的什么能让它稳定前 ...

是的,你直接改变这个值就行
回复 支持 反对

使用道具 举报

发表于 2014-5-4 13:58:44 | 显示全部楼层
It's_me 发表于 2014-5-4 09:40
你看一下这个码盘测速的原理,还是不懂我再给你讲讲

time = millis();//以毫秒为单位,计算当前时间
  //计算出每0.5秒钟内,编码器码盘计得的脉冲数,
  if(abs(time - old_time) >= 500) // 如果计时时间已达0.5秒
  {
    detachInterrupt(0); // 关闭外部中断0
    detachInterrupt(1); // 关闭外部中断1  
     //此直流减速电机的编码器码盘为334个齿,减速比为21.3。
    //把编码器每0.5秒钟计得的脉冲数,换算为当前转速值的计算式
    rpm_right =(float)count_right*60*2/(334*21.3);
    rpm_left =(float)count_left*60*2/(334*21.3);
    //根据左右车轮转速差,乘以比例调节因子2,获得比例调节后的左侧电机PWM功率值
    PWM_left=(float)PWM_left+(rpm_right-rpm_left)*2;      
//根据刚刚调节后的小车电机PWM功率值,
//及时修正小车前进或者后退状态,以使小车走直线
    if(flag=='a')
    advance();
    if(flag=='b')
    back();     
    count_right = 0;   //把脉冲计数值清零,以便计算下一个0.5秒的脉冲计数
    count_left = 0;
    old_time=  millis();     // 记录每次0.5秒测速后的时间节点   
    attachInterrupt(0, Code_right,FALLING); // 重新开放外部中断0
    attachInterrupt(1, Code_left,FALLING); // 重新开放外部中断1
  }
}

//右侧电机编码器码盘计数中断子程序
void Code_right()
{  
  count_right += 1; // 编码器码盘计数加一
}
//左侧电机编码器码盘计数中断子程序
void Code_left()
{
  count_left += 1; // 编码器码盘计数加一   
}


其实这里有这个代码,是为了能够保持转速一致的,想到从端口读取,但是应该直接连到端口就可以了吧,然后INPUT,不知道楼主把那两根线是如何连接的,楼主给的原理图已经看懂~谢谢~~
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )

GMT+8, 2020-7-4 02:09 , Processed in 0.068774 second(s), 24 queries .

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

快速回复 返回顶部 返回列表