Takeuchi Kouichi
/
2bk0819_Gyro_test04
エレキジャックweb mbed入門 ジャイロとサーボを動かそう 課題4です。
Diff: main.cpp
- Revision:
- 0:86acb35db6d7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Sep 20 09:56:24 2011 +0000 @@ -0,0 +1,82 @@ +//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 +