首页 › 智能车 › 基于Arduino+MPU6050+Tp-link 703n平衡车完美站立（部分代码上传）

Gavinlv 发表于 2014-6-26 20:23:22

mark，回去慢慢看。

iohongwal 发表于 2014-6-26 20:49:39

本帖最后由 iohongwal 于 2014-6-30 21:29 编辑


unsigned char Re_buf,counter=0;
unsigned char sign=0;
int M11=5;
int M12=6;
int M21=9;
int M22=10;
int Pwm_out = 0;
int Turn_Need = 0;
int Speed_Need = 0;
int speed_output_R , speed_output_L;
int count_l=13;
int count_r=8;
int count_left;
int count_right;
float speeds , speeds_filter, positions;
float diff_speeds,diff_speeds_all;
float a,w,Angle,T;
short sAccelerat,sAngleVelocity,sAngle,sT;
float Kap = 24.5;       //
float Kad = 10;         //
float Ksp = 2.8;      //
float Ksi = 0.11;       //
void setup()
{
Serial.begin(115200);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
analogWrite(5,0);
analogWrite(6,0);
analogWrite(9,0);
analogWrite(10,0);
}
void Motor(float v1,float v2)
{
if (v1>255){v1=255;analogWrite(M11,0);analogWrite(M12,v1);}
else if (v1>0) {analogWrite(M11,0);analogWrite(M12,v1);}
else if (v1>-255) {analogWrite(M12,0);analogWrite(M11,-v1);}
else{v1=-255;analogWrite(M12,0);analogWrite(M11,-v1);}

if (v2>255){v2=255;analogWrite(M21,0);analogWrite(M22,v2);}
else if (v2>0) {analogWrite(M21,0);analogWrite(M22,v2);}
else if (v2>-255) {analogWrite(M22,0);analogWrite(M21,-v2);}
else {v2=-255;analogWrite(M22,0);analogWrite(M21,-v2);}
}
void loop()
{
}


void PID_PWM()
{
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 + Kad*w + 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);
Motor(speed_output_L , speed_output_R);
count_left = 0;
count_right = 0;

   Serial.print(Angle);Serial.print(" ");
Serial.print(w);Serial.print(" ");
Serial.println(Pwm_out);Serial.print(" ");
}
void serialEvent() {
while (Serial.available()) {
   
    //char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code

    Re_buf=(unsigned char)Serial.read();
    if(counter==0&&Re_buf!=0x55) return;      //第0号数据不是帧头            
    counter++;      
    if(counter==11)             //接收到11个数据
    {   
       counter=0;               //重新赋值，准备下一帧数据的接收
       switch(Re_buf )
        {
        case 0x51:
                a = float(short(Re_buf <<8| Re_buf ))/32768*16;
                a =float(short(Re_buf <<8| Re_buf ))/32768*16;
                a =float(short(Re_buf <<8| Re_buf ))/32768*16;               
                break;
        case 0x52:
                w =float(short(Re_buf <<8| Re_buf ))/32768*250;
                w =float(short(Re_buf <<8| Re_buf ))/32768*250;
                w =float(short(Re_buf <<8| Re_buf ))/32768*250;
                break;
        case 0x53:
              Angle =float(short(Re_buf <<8| Re_buf ))/32768*180;
                Angle =float(short(Re_buf <<8| Re_buf ))/32768*180;
                Angle =float(short(Re_buf <<8| Re_buf ))/32768*180;
                T =float(short(Re_buf <<8| Re_buf ));///340.0+36.25   
                sign=1;
                break;
        }
    }      
}
}




這我是用你的PID改 幫我看看有冇甚麼問題
因為呢我寫進arduino 走不動 和讀不到資料

Santiago 发表于 2014-6-27 14:48:09

楼主可不可以发个买电机的链接啊

shadan0416 发表于 2014-6-28 11:04:58

LZ 电机是采用步进的摸有链接木有

SAGA 发表于 2014-6-29 17:04:01

技术贴！顶！

Lrnxis 发表于 2014-7-20 10:30:59

想问下如何通过tx和rx实现703N和arduino的通信呢？

春日迟迟n 发表于 2014-7-21 13:06:39

很好！:):):):):):):)

McZHA 发表于 2014-8-1 17:17:45

手机怎么接收路由的串口数据

二十一题选C 发表于 2014-8-27 18:21:12

不知道为什么站不起来。。

smallfivecn 发表于 2014-8-29 10:11:53

每个电机测速有A、B相两路信号，直接一路还是接两路呢？接两路可以测正反向，更准确呀！

阿布都 发表于 2014-9-4 17:27:06

有完整的代码吗？

wininw 发表于 2014-9-15 12:43:53

为什么我的电机需要三个接口才能用pwm控制速度呢？

dltwcy 发表于 2014-9-16 13:34:22

学习了，刚开始玩arduino

月夜 发表于 2014-9-24 12:09:30

跟着楼主买了一样的串口MPU6050,求楼主放出完整代码啊

月夜 发表于 2014-9-24 13:46:31

楼主MPU6050里面的代码温度那里有点问题:T =float(short(Re_buf <<8| Re_buf ));///340.0+36.25而那个卖模块给出的代码是:T = (short(Re_buf <<8| Re_buf ))/340.0+36.25;

查看完整版本: 基于Arduino+MPU6050+Tp-link 703n平衡车完美站立（部分代码上传）