|
|
之前在论坛上看到了“pww999”大神的自平衡小车，于是想弄辆试试，因为之前没有接出过arduino，制作过程中也碰到不少的问题，从师兄那弄来了Apc220模块，自己买了两块扩展板，l298p和v5扩展板。结合之前的看的《arduino开发实战指南》，于是改了一下楼主的代码，想用Apc220 控制，可是上电后，两电机只会往相反方向转，遥控就更不起作用了。。在检验跟下载时都没有出现问题，心顿时凉了一半。。
请问论坛上有哪位大神用过这模块或者弄过自平衡小车的，能抽空帮菜鸟看一下代码，不胜感激。。。{:soso_e154:}
#include "Wire.h"
#include "SPI.h"
#include "I2Cdev.h"
#include "MPU6050.h"
//--------MPU6050参数
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gry_offset -20 // 陀螺仪偏移量
#define Gyr_Gain 0.00763358 //对应的1G
#define pi 3.14159
//---------PID控制器参数
float kp, ki, kd,kpp;
float angleA,omega;
float LOutput,ROutput;
float LSpeed_Need=0.0,RSpeed_Need=0.0;
int data,adata;
unsigned long now;
unsigned long preTime = 0;
float SampleTime = 0.08; //-------------------互补滤波+PID 采样时间0.08 s
unsigned long lastTime;
float Input, Output, Setpoint;
float errSum,dErr,error,lastErr,f_angle;
int timeChange;
/*----------l298接m2560引脚，因为用的是扩展板l298p，
E1 M1
L X 电机1控制禁止
H H 电机1反转
H L 电机1正转
PWM X 对电机1调速
E2 M2
L X 电机2控制禁止
H H 电机2反转
H L 电机2正转
PWM X 对电机2调速*/
int E1 = 5;
int M1 = 4;
int E2 = 6;
int M2 = 7;
int comtemp; //定义一个变量来储存串口APC220收到的数据
void setup()
{
Wire.begin();
Serial.begin(9600);
accelgyro.initialize();
delay(100);
pinMode(M1, OUTPUT);
pinMode(M2, OUTPUT);
pinMode(E1, OUTPUT);
pinMode(E2, OUTPUT);
delay(100);
Serial.begin(9600);
}
void loop()
{ if ( Serial.available() ) //接受电脑串口的数据
comtemp= Serial.read();
switch(comtemp)
{case 'w': //前进
{LSpeed_Need=4;
RSpeed_Need=4;}
break;
case 's': //后退
{LSpeed_Need=-4;
RSpeed_Need=-4;}
break;
case 'q': //左转
{RSpeed_Need=18;}
break;
case 'e': //右转
{LSpeed_Need=18;}
break;
case 't': //停止
{LSpeed_Need=0;
RSpeed_Need=0;
Setpoint;}
break;
defaut: {LSpeed_Need=0; //否则也停止
RSpeed_Need=0;
Setpoint;}
break; }
//下面的就跟pww99大神的代码。。。
// if (abs(angleA>38)){LSpeed_Need*=0.6;RSpeed_Need*=0.6;}
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//------------------------------------------------------------------------
angleA= atan2(ay , az) * 180 / pi+0.5; // 根据加速度分量得到的角度(degree)加0.5偏移量
//180度至0至-180（360度）取0度为坚直时中立点 因为坚直时有偏差，所以加0.5....
omega= Gyr_Gain * (gx + Gry_offset); // 当前角速度(degree/s)
if (abs(angleA)<45) { // 角度小于45度 运行程序
PIDD();//---------------------互补滤波+PID--------------------------
PWMB(); //----------------------PWM调速输出--------------------------
delay(3);
}
else { // 角度大于45度 停止PWM输出
analogWrite(E1, 0); //两电机调速端口结合自己的电机驱动板改了下..
analogWrite(E2, 0);
}
}
void PIDD(){
kp = analogRead(8)*0.03; //取0~1024* (这些值是小车调试后得出，请按自己小车调试后修改)
ki = analogRead(9)*0.0002;//取0~1024* .........
kd = analogRead(10)*1; //取0~1024* .........
//kpp = analogRead(11)*0.005;
//------------------互补滤波 ------------------------
unsigned long now = millis(); // 当前时间(ms)
float dt = (now - preTime) / 1000.0; // 微分时间(ms)
preTime = now;
float K = 0.8;
float A = K / (K + dt);
f_angle = A * (f_angle + omega * dt) + (1-A) * angleA; // 互补滤波算法
//----------------------------PID控制器 ------------------------------
//now = millis();
timeChange = (now - lastTime);
if(timeChange>=SampleTime)
{
Setpoint=LSpeed_Need;// =0，(+ -值使电机前进或后退)
Input =f_angle;
error = Setpoint- Input;
errSum += error* timeChange;
dErr = (error - lastErr)/ timeChange;
//PID Output
Output = kp * error + ki * errSum + kd * dErr;
LOutput=Output+RSpeed_Need;//左电机
ROutput=Output-RSpeed_Need;//右电机
lastErr = error;
lastTime = now;
}
// Serial.print(angleA); Serial.print("\t");
// Serial.print(omega);Serial.print("\t");
// Serial.print(f_angle); Serial.print("\t");
/// Serial.println(Output);
}
void PWMB(){
if(LOutput>0.3)//左电机-------或者取0
{ digitalWrite(M1,HIGH);}
else if(LOutput<-0.3)//-------或者取0
{ digitalWrite(M1,LOW); }
else //刹车-------取0后可以不用
{ digitalWrite(M1,0);}
if(ROutput>0.3)//右电机--------或者取0
{ digitalWrite(M2,LOW); }
else if(ROutput<-0.3)//-------或者取0
{ digitalWrite(M2,HIGH); }
else //刹车-------取0后可以不用
{ digitalWrite(M1,0);}
analogWrite(E1,min(255,abs(LOutput)+25)); //PWM调速a==0-255
analogWrite(E2,min(255,abs(ROutput)+23));
} |
|
发表于 2013-3-17 18:43:15
