兩輪平衡車要怎麼平衡呢?

everythingqq

看了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 ;

float  a =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);
  }
}


我想讓車子單純穩定平衡的話，應該就是當車子往前倒時，馬達往前追回來，而車子要往後倒時，車子就往後補回來，但是
目前這樣的程式，卻不能平衡。實際的狀況是會往某一邊倒下後就回不來了，想問問大家知道問題在哪邊嗎??

vegewell

回復 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) ;
}


void  setup(){
  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);
}

mojamoja1202

請搜尋PID控制