買書捐殘盟

2010年7月11日 星期日

990712-Segway再現

一直對Segway兩輪平衡車設計不出來耿耿於懷。話說Segway兩輪平衡車我打從著手設計程式至今,大概有5個多月了吧。讀了很多相關的文章,從陀螺儀的理論、陀螺儀的定位方法、研究所的相關論文;也使用了Robolab、Labview以及現在使用的Bricxcc等工具,現在稍稍有一點點頭緒。

不過,目前還有缺點:
1.車子往後傾斜,車輪不會動?
2.車子在行駛時,在數秒後車輪會有搖擺的現象(影片中可以看到),之後碰到障礙物後,就直接倒下。

以上都是待改進之處。

程式碼:
#define GyroPort IN_3
#define dt 0.001
#define GyroScale 4
#define t_scale 500

int flag=0;
long GyroBiasVal=0;
long curr_time=0;
long GyroCounter=0;
float bias=0.0000;
float theta=0,pre_theta=0,theta_bias=0;
float f1=0,f2=0;
long t_old=0;
float power=0;
float x=0,pre_x=0,dot_x=0;
float aa=0.001;
float K1=0,K2=90,K3=8,K4=10,V=0.195,K_pwr=0;


void initGyro(){
 curr_time=CurrentTick();
 PlayTone(440,50);
 while(CurrentTick()<(3000+curr_time)){ //3秒內,計算gyro bias
  GyroBiasVal=GyroBiasVal+SensorRaw(GyroPort);
  TextOut(0,LCD_LINE5,"Computing Bias..");
  Wait(20);
  GyroCounter++;
 }

 PlayTone(880,50);
 bias=GyroBiasVal/GyroCounter;
}//End of initGyro Function

void get_body_theta(){
 f2=(SensorRaw(GyroPort)-bias)/GyroScale;
 Wait(1);
 theta=pre_theta+(f1+2*f2)*(CurrentTick()-t_old)/t_scale;
 pre_theta=theta;
 f1=(SensorRaw(GyroPort)-bias)/GyroScale;
 t_old=CurrentTick();
 x=MotorTachoCount(OUT_B);
 dot_x=x-pre_x;
 pre_x=x;
 theta_bias=(1-aa)*theta_bias+aa*theta;
}//End of get_body_theta ,use Runge Kutta method order 2


task main(){
 SetSensorHTGyro(GyroPort);
 initGyro();
 ClearScreen();

 t_old=CurrentTick();
 ResetTachoCount(OUT_BC);
 //↓Gain computing based on Battery Voltage
 K_pwr=0.7+(0.7-1.1)/(8816-8196)*(BatteryLevel()-8816); 
 while(true){
  get_body_theta();
  power=K_pwr*(K1*x+K2*(dot_x-V)+K3*(theta-theta_bias)+K4*(f1));
  if(power>100) power=100;
  if(power<-100) power=-100;
  SetOutput(OUT_BC, Power, power, OutputMode,OUT_MODE_MOTORON,
       RunState,OUT_RUNSTATE_RUNNING,
       UpdateFlags,UF_UPDATE_MODE+UF_UPDATE_SPEED);
}
}//End of Task Main

==================
執行結果:


參考文獻:
7.NXTway-GS Building
8.NXTway Model-Based
9.DSP主控之兩輪機器人平衡與兩輪同步控制,蔡僑倫,國立中央大學-93.06
10.以樂高NXT 實作智慧型兩輪平衡車,羅嘉寧等,銘傳大學-2009資訊科技與實務國際研討會
12.數值分析Runge-Kutta Method order 2,4
13.Kalman Filter