Takeuchi Kouichi
/
2bk0819_Gyro_test04
エレキジャックweb mbed入門 ジャイロとサーボを動かそう 課題4です。
main.cpp
- Committer:
- takeuchi
- Date:
- 2011-09-20
- Revision:
- 0:86acb35db6d7
File content as of revision 0:86acb35db6d7:
//Gyro test04 // Akidukidensi AE-GYRO-SMD #include "mbed.h" #include "TextLCD0420.h" #define ON 1 #define OFF 0 #define Gy1_offset 0.01 DigitalOut mled0(LED1); DigitalOut mled1(LED2); AnalogIn gy1_adc(p16); AnalogIn gy2_adc(p17); TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3 double round3(double x){ double y; y=double(int((x+0.005)*100))/100; return(y); } int main() { float gy1_data; float gy1_center; float gy1_sgm=0; float gy1_wk0=0,gy1_wk1=0; float gy1_avg=0,gy1_sum; int i,gy1_lc=9,gy1_lc_old=0; lcd.cls(); lcd.locate(0,0); lcd.printf("*** Gyro test04 ****\n"); gy1_center=0; for(i=0;i<300;i++){ gy1_center=gy1_center+round3(gy1_adc.read()*3.3); wait(0.001); } gy1_center=round3(gy1_center/300); while(1){ gy1_sum=0; for(i=0;i<20;i++){ gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3); wait(0.005); } gy1_avg=round3(gy1_sum/20); gy1_wk1=round3((gy1_avg-gy1_center)*1000/5); if(gy1_wk1 < 0){ gy1_wk1=gy1_wk1*1.1; } gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2); gy1_wk0=gy1_wk1; if(gy1_sgm > 90){ gy1_sgm=90; } else if(gy1_sgm < -90){ gy1_sgm=-90; } lcd.locate(0,1); lcd.printf("%3.3f %3.3f d:%3.0f",gy1_avg,gy1_center,gy1_sgm); lcd.locate(0,2); lcd.printf("--------+--------"); gy1_lc=(gy1_sgm-5)/10+9; if(gy1_lc >20){ gy1_lc=19; } if(gy1_lc <0){ gy1_lc=0; } lcd.locate(gy1_lc_old,3); lcd.printf(" "); lcd.locate(gy1_lc,3); lcd.printf("*"); gy1_lc_old=gy1_lc; }//while }//main