极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 35884|回复: 16

拜坊友所赐,我的平衡车制作成功啦!

[复制链接]
发表于 2014-9-16 13:49:03 | 显示全部楼层 |阅读模式
本帖最后由 jkhjt1949 于 2016-2-19 12:09 编辑

我是看到论坛里的视频后才萌生DIY平衡小车的。本人大二党,花了2个月才做到这个程度的,完全出于兴趣,拿出来就是想跟大家交流交流。不好意思,文件和图片上传好多次都不成功。在末尾我留了网盘链接了,里面有材料清单和小车资料,有需要的可以下载。欢迎交流!!!

为了偷懒,视频是用4s拍的,效果一般,屏幕较窄,请见谅。
此小车因为安装了蓝牙模块,可用android手机操控,完成前进后退,左右方向旋转动作





详细图片






[pre lang="arduino"]
#include <I2Cdev.h>
#include <MPU6050.h>
#include<Kalman.h>
#include <Wire.h>

MPU6050 accelgyro;
Kalman kalmanFilter;

#define IN1 5
#define IN2 6
#define IN3 9
#define IN4 10
#define ENA 7
#define ENB 8
#define POSITION_RATIO 0.0572

int16_t ax,ay,az;                                 
int16_t gx,gy,gz;
int data,lastData;
long count,count2;
double aax,aay,aaz,ggx,ggy,ggz,        nowTime, lastTime ;
float accAngleY,kalAngleY,           Ax,Ay,Az,       _position,lastPosition,positionR,positionL,positionDf ;
float  pwm, pwmL, pwmR,      dt,         _speed,lastSpeed,acceleration  ;
float  drift,setPoint,initialSp,turnSpeed,     itermIn,                 k,k2,k3,kp,ki,kd,kps,ksp;

void setup()
{
  Wire.begin();
  Serial.begin(9600);
  
  accelgyro.initialize();
  
   pinMode(ENA,OUTPUT);
   pinMode(ENB,OUTPUT);
   
   pinMode(IN1,OUTPUT);
   pinMode(IN2,OUTPUT);
   pinMode(IN3,OUTPUT);
   pinMode(IN4,OUTPUT);
   
   pinMode(12,INPUT);
   pinMode(11,INPUT);
   
   digitalWrite(IN1,0);
   digitalWrite(IN2,0);
   digitalWrite(IN3,0);
   digitalWrite(IN4,0);
   
   attachInterrupt(0, encoder,FALLING);
    attachInterrupt(1,encoder2,FALLING);
   
    stopCar();
   
    drift = 20                      ;
   
     k = 0.0                  ;  
      k2 = 0.0        ;
    //k3 = 0.0          ;  
     setPoint = 0.0             ;
      turnSpeed = 0.0         ;
      kp = 36.0                 ;
      ki = 0.0          ;
      kd = 1.88                  ;
      kps = 0.145               ;
      ksp = 1.36              ;
}

void loop()
{
      blueTooth();
      loopPeriod();
      angleMeasure();
      speedAndPositionCal();
      //setPointAdj();
      pwmCalAndOut();   
   
      delay(10);                                                         
}


//------------------------------------------------------------------------------------------
void loopPeriod()
{
        nowTime = millis();
      dt = (nowTime-lastTime)/1000;
      lastTime = nowTime;
}
  
  
//------------------------------------------------------------------------------------------
void angleMeasure()
{
       accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
      
       Ax=ax/16384.00;
       Ay=ay/16384.00;
       Az=az/16384.00;
       ggx=gx/131.00;
     
      accAngleY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14;  
     
       kalAngleY = kalmanFilter.getAngle( accAngleY, ggx, dt );
}




//------------------------------------------------------------------
void speedAndPositionCal()
{
        positionR =  count  * POSITION_RATIO ;     // unit : mm
        positionL =  count2  * POSITION_RATIO ;
        positionDf = positionR - positionL;
        _position =  ( positionR + positionR ) / 2;
        _speed = ( _position - lastPosition ) / dt ;                    //unit : mm/s
        acceleration = ( _speed - lastSpeed ) / dt;
        lastPosition = _position;
        lastSpeed = _speed ;
}   




//---------------------------------------------------------------------------------------------------------
void pwmCalAndOut()
{
          if( kalAngleY < -drift || kalAngleY > drift)               //角度过大,关闭电机
        {  
            stopCar();
            return;
        }

        itermIn += ( kalAngleY - setPoint ) ;
        if( itermIn < -500 ) itermIn = -500;
         if ( itermIn > 500 )  itermIn = 500;        
         
         if(   (_position > 100) || (_position < -100)   )
         {
               count = 0;
               count2 = 0;
               lastPosition = 0;
         }                 

        pwm  = kp * ( kalAngleY - setPoint )  + ki * itermIn + kd  * ggx + kps * _position + ksp * _speed + k2 * acceleration;
      
        pwmL = pwm  + turnSpeed + positionDf * k;
        pwmR = pwm  - turnSpeed - positionDf * k;  

      if( pwmL < 0 )
        {
               pwmL = -( pwmL - 0 ) ;
               if( pwmL >255 ) pwmL = 255;               
                     
                analogWrite(IN1,0);         
                analogWrite(IN2,pwmL);               
        }
       else
       {        
               pwmL += 0;
               if( pwmL >255 ) pwmL = 255;      
                       
               analogWrite(IN2,0);         
               analogWrite(IN1,pwmL);            
       }   
      
       if( pwmR < 0 )
        {
               pwmR = -pwmR;
               if( pwmR >255 ) pwmR = 255;      
                     
                analogWrite(IN3,0);         
                analogWrite(IN4,pwmR);               
        }
       else
       {         
               if( pwmR >255 ) pwmR = 255;                     
                       
               analogWrite(IN4,0);         
               analogWrite(IN3,pwmR);            
       }   
      
      digitalWrite(ENA,HIGH);         
      digitalWrite(ENB,HIGH);                  
}




//-------------------------------------------------------------------------
void stopCar()
{
       digitalWrite(ENA,LOW);
       digitalWrite(ENB,LOW);
}                                       



//-------------------------------------------------------------------------
void blueTooth()
{
     if (Serial.available())  
     {
            data = Serial.read();        
            if( data != lastData )      initialization();
      
              switch(data)
              {
                 case 1:   k = 0.5                  ;  
                                k2 = 0.031        ;
                                k3 = 0.0          ;  
                              //setPoint = -2.68             ;
                                turnSpeed = 0.0         ;
                                kp = 36.0                 ;
                                ki = 0.0   ;
                                kd = 2.4415                   ;
                                kps = 0.0             ;
                                ksp = 1.921           ;
                                break;
                                 
                 case 2:   k = 0.5                  ;  
                                k2 = 0.031        ;
                                k3 = 0.0          ;  
                              //setPoint = -2.68             ;
                                turnSpeed = 0.0         ;
                                kp = 36.0                 ;
                                ki = 0.0   ;
                                kd = 2.4415                  ;
                                kps = 0.0             ;
                                ksp = 1.921           ;
                                 break;
                                 
                 case 3:   k = 0.0                  ;  
                                 k2 = 0.0        ;
                               //k3 = 0.0          ;
                                setPoint = 0.0             ;
                                 turnSpeed = 30          ;
                                 kp = 36                        ;
                                 ki = 0.1                  ;
                                 kd = 1.88                       ;
                                 kps = 0.0                     ;
                                 ksp = 1.3                     ;   
                                 break;
                           
                 case 4:  k = 0.0                  ;  
                                k2 = 0.0        ;
                              //k3 = 0.0          ;
                                setPoint = 0.0             ;
                                turnSpeed = -30          ;
                                kp = 36                        ;
                                ki = 0.1             ;
                                kd = 1.88                       ;
                                kps = 0.0                     ;
                                ksp = 1.3                     ;
                                break;
                                
                 default:  k = 0.0                  ;  
                                k2 = 0.0        ;
                              //k3 = 0.0          ;  
                               setPoint = 0.0             ;
                                turnSpeed = 0.0         ;
                                kp = 36.0                 ;
                                ki = 0.0          ;
                                kd = 1.88                  ;
                                kps = 0.145               ;
                                ksp = 1.36              ;
              }
               lastData = data;
     }  
     
     if( data == 1 )     
     {
             if( setPoint != -2.68 )
              setPoint -= 0.005;
             if( setPoint < -2.68 )  
             {
                    setPoint = -2.68;
                  //initialSp = setPoint;
                  //data = 10;
              }
       }
   
     if( data == 2 )     
     {
            if( setPoint != 2.68 )
              setPoint += 0.005;
             if( setPoint > 2.68 )  
             {
                     setPoint = 2.68;
                    //initialSp = setPoint;
                    //data = 10;
             }
     }

}
  
  
  
//-----------------------------------------------------------------
void encoder()
{
        if( digitalRead(12) == LOW )
        {
             count ++;
         }
        else
          count --;
        
         if( count > 50000 || count < -50000 )
         {
              count = 0;
         }
}

void encoder2()
{
        if( digitalRead(11) == LOW )
        {
             count2 ++;
        }
        else
          count2 --;
         
         if( count2 > 50000 || count2 < -50000 )
         {
              count2 = 0;
         }
}

void initialization()
{
        itermIn = 0;
        count = 0;
        count2 = 0;
        lastPosition = 0;  
}

/*
void setPointAdj()
{
     if( data == 10 )
     {
           if( acceleration > 100 )
           {
                setPoint -= k3 * acceleration;   
           }
           
           else if(   acceleration < -100    )
           {
                setPoint -= k3 * acceleration;   
           }
           
           else setPoint = initialSp;
     }  
}
                                                                                */
[/code]

链接地址:http://pan.baidu.com/s/1hqvJ2cs

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复

使用道具 举报

发表于 2014-9-16 23:31:08 | 显示全部楼层
有教程不,,,
回复 支持 反对

使用道具 举报

发表于 2014-9-20 10:21:48 | 显示全部楼层
求教程。。。。
回复 支持 反对

使用道具 举报

发表于 2014-9-24 21:57:12 | 显示全部楼层
没图你说个毛线啊
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-10-2 10:57:15 | 显示全部楼层
慢、节奏 发表于 2014-9-24 21:57
没图你说个毛线啊

视频已上传
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-10-2 11:55:27 | 显示全部楼层
努力微笑 发表于 2014-9-16 23:31
有教程不,,,

帖子已更新
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-10-2 11:55:52 | 显示全部楼层
胡萝卜喜欢怪嘎 发表于 2014-9-20 10:21
求教程。。。。

帖子已更新
回复 支持 反对

使用道具 举报

发表于 2014-11-28 11:08:47 | 显示全部楼层
亲,你手机遥控它到处跑是通过改变程序中的哪个值实现的,我的那个可以平衡,但是如果改变位置值是它到处跑,车立马就倒,我想是因为变得太突然了
回复 支持 反对

使用道具 举报

发表于 2015-5-28 21:04:10 | 显示全部楼层
给个接线图呗,我小白,谢谢
回复 支持 反对

使用道具 举报

发表于 2015-7-23 01:16:00 | 显示全部楼层
感谢楼主无私分享,学习了!!
回复 支持 反对

使用道具 举报

发表于 2015-7-25 19:51:24 | 显示全部楼层
我最近也在做,PCB板刚到,等弄好了,也发帖。
回复 支持 反对

使用道具 举报

发表于 2015-7-25 21:47:02 | 显示全部楼层
你使用的电机是带码盘的吗?求详细硬件链接图。邮箱[email protected]
回复 支持 反对

使用道具 举报

发表于 2015-9-26 01:25:32 | 显示全部楼层
MPU6050部分如何接线呢?以及光电编码电机AB两相如何介入nano中进行鉴别正反转的?
回复 支持 反对

使用道具 举报

发表于 2016-4-1 09:11:12 | 显示全部楼层
楼主  我是新人  求带  也是大二
回复 支持 反对

使用道具 举报

发表于 2016-4-1 13:43:05 | 显示全部楼层
楼主  加个QQ不懂的程序问问 好吗javascript:;
回复 支持 反对

使用道具 举报

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

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-19 16:54 , Processed in 0.062534 second(s), 29 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

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