兩輪平衡車要怎麼平衡呢?
看了MIT的DIY SEGWAY之後，自己也想來做個小型的SEGWAY來過過乾癮，但是卻怎麼也平衡不了!!!請各位幫我看看我的問題吧，，，目前我的第一步是想先能穩定的平衡站著
單晶片: Arduino uno
三軸加速度感測器:MMA7361L
角加速度感測:ENC03M
在網路也看到人家用了很多感測器，但也有人只用單用加速度感測就能平衡，所以我用兩個應該夠吧.....
馬達控制晶片:TA7257P
馬達:減速馬達，扭力應該很夠。
沒有用步進馬達或是有編碼器的，應該也可以平衡吧~(我猜)
程式:
int int0 = 9; // 控制馬達晶片INT0
int int1 = 10;// 控制馬達晶片INT0
void setup() {
Serial.begin(9600);
pinMode(int0, OUTPUT);
pinMode(int1, OUTPUT);
}
int Xacc_an ;
int Gyro_an;
float Xacc;
float Gyro;
float angle ;
floata =1;
float b = 0.2;
void loop() {
Xacc_an = analogRead(A0);//加速度類比讀取值
Gyro_an = analogRead(A1);//角加速度類比讀取值
Xacc= (Xacc_an-330)*0.34; //按照MMA7361L的DATASHEET 計算的加速度
Gyro = (Gyro_an-279)*3; //按照ENC03M的DATASHEET 計算的角加速度
angle = 0.95*(Xacc+Gyro*0.262)+0.05*Xacc;// 仿照MIT DIY SEGWAY 的資料中的 FILTER 方法所計算的角度
motor(angle);
delay(10);
}
void motor(float angle){
if(-10<=angle<=10){//當angle在小角度時，不動
digitalWrite(int0,1);
digitalWrite(int1,1);
}
if(angle>10){ //當anGLE在大於10度時，開始往前
digitalWrite(int0,0);
digitalWrite(int1,10*angle);
}
if(angle<-10){ //當anGLE在大於10度時，開始往前
digitalWrite(int0,-10*angle);
digitalWrite(int1,0);
}
}
我想讓車子單純穩定平衡的話，應該就是當車子往前倒時，馬達往前追回來，而車子要往後倒時，車子就往後補回來，但是
目前這樣的程式，卻不能平衡。實際的狀況是會往某一邊倒下後就回不來了，想問問大家知道問題在哪邊嗎?? 回復 1# everythingqq
先拔掉馬達電源,加入
Serial.print(angle);
delay(2);
Serial.print("\n");
用手前後輕微擺動車子,
看看angle的值,是否如你預期?
how to print float number:
double x;
double y;
double z;
void printDouble( double val, unsigned int precision){
// prints val with number of decimal places determine by precision
// NOTE: precision is 1 followed by the number of zeros for the desired number of decimial places
// example: printDouble( 3.1415, 100); // prints 3.14 (two decimal places)
Serial.print (int(val));//prints the int part
Serial.print("."); // print the decimal point
unsigned int frac;
if(val >= 0)
frac = (val - int(val)) * precision;
else
frac = (int(val)- val ) * precision;
Serial.println(frac,DEC) ;
}
voidsetup(){
Serial.begin(9600);
Serial.println("Print floating point example");
printDouble( 3.1415, 100);// example of call to printDouble to print pi to two decimal places
x = 10;
y = 3.1;
z = x / y;
}
void loop(){
printDouble(z,10); // one decimal place
printDouble(z,100);// two decimal places
printDouble(z,1000); // three decimal places
z = z + .1;
delay(100);
} 請搜尋PID控制
頁:
[1]