Takeuchi Kouichi
エレキジャックweb mbed入門 ジャイロとサーボを動かそう 課題3です。
main.cpp@0:4a81aff3ea7b, 2011-09-20 (annotated)
- Committer:
- takeuchi
- Date:
- Tue Sep 20 09:59:05 2011 +0000
- Revision:
- 0:4a81aff3ea7b
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| takeuchi | 0:4a81aff3ea7b | 1 | //Gyro test03 |
| takeuchi | 0:4a81aff3ea7b | 2 | // Akidukidensi AE-GYRO-SMD |
| takeuchi | 0:4a81aff3ea7b | 3 | #include "mbed.h" |
| takeuchi | 0:4a81aff3ea7b | 4 | #include "TextLCD0420.h" |
| takeuchi | 0:4a81aff3ea7b | 5 | |
| takeuchi | 0:4a81aff3ea7b | 6 | #define ON 1 |
| takeuchi | 0:4a81aff3ea7b | 7 | #define OFF 0 |
| takeuchi | 0:4a81aff3ea7b | 8 | #define Gy1_offset 0.01 |
| takeuchi | 0:4a81aff3ea7b | 9 | |
| takeuchi | 0:4a81aff3ea7b | 10 | DigitalOut mled0(LED1); |
| takeuchi | 0:4a81aff3ea7b | 11 | DigitalOut mled1(LED2); |
| takeuchi | 0:4a81aff3ea7b | 12 | AnalogIn gy1_adc(p16); |
| takeuchi | 0:4a81aff3ea7b | 13 | AnalogIn gy2_adc(p17); |
| takeuchi | 0:4a81aff3ea7b | 14 | |
| takeuchi | 0:4a81aff3ea7b | 15 | TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3 |
| takeuchi | 0:4a81aff3ea7b | 16 | |
| takeuchi | 0:4a81aff3ea7b | 17 | double round3(double x){ |
| takeuchi | 0:4a81aff3ea7b | 18 | double y; |
| takeuchi | 0:4a81aff3ea7b | 19 | y=double(int((x+0.005)*100))/100; |
| takeuchi | 0:4a81aff3ea7b | 20 | return(y); |
| takeuchi | 0:4a81aff3ea7b | 21 | } |
| takeuchi | 0:4a81aff3ea7b | 22 | |
| takeuchi | 0:4a81aff3ea7b | 23 | int main() { |
| takeuchi | 0:4a81aff3ea7b | 24 | float gy1_data; |
| takeuchi | 0:4a81aff3ea7b | 25 | float gy1_center; |
| takeuchi | 0:4a81aff3ea7b | 26 | float gy1_sgm=0; |
| takeuchi | 0:4a81aff3ea7b | 27 | float gy1_wk0=0,gy1_wk1=0; |
| takeuchi | 0:4a81aff3ea7b | 28 | float gy1_avg=0,gy1_sum; |
| takeuchi | 0:4a81aff3ea7b | 29 | int i; |
| takeuchi | 0:4a81aff3ea7b | 30 | |
| takeuchi | 0:4a81aff3ea7b | 31 | lcd.cls(); |
| takeuchi | 0:4a81aff3ea7b | 32 | lcd.locate(0,0); |
| takeuchi | 0:4a81aff3ea7b | 33 | lcd.printf("*** Gyro test03 ****\n"); |
| takeuchi | 0:4a81aff3ea7b | 34 | |
| takeuchi | 0:4a81aff3ea7b | 35 | gy1_center=0; |
| takeuchi | 0:4a81aff3ea7b | 36 | for(i=0;i<10;i++){ |
| takeuchi | 0:4a81aff3ea7b | 37 | gy1_center=gy1_center+round3(gy1_adc.read()*3.3); |
| takeuchi | 0:4a81aff3ea7b | 38 | wait(0.01); |
| takeuchi | 0:4a81aff3ea7b | 39 | } |
| takeuchi | 0:4a81aff3ea7b | 40 | gy1_center=round3(gy1_center/10); |
| takeuchi | 0:4a81aff3ea7b | 41 | |
| takeuchi | 0:4a81aff3ea7b | 42 | |
| takeuchi | 0:4a81aff3ea7b | 43 | while(1){ |
| takeuchi | 0:4a81aff3ea7b | 44 | gy1_sum=0; |
| takeuchi | 0:4a81aff3ea7b | 45 | for(i=0;i<10;i++){ |
| takeuchi | 0:4a81aff3ea7b | 46 | gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3); |
| takeuchi | 0:4a81aff3ea7b | 47 | wait(0.01); |
| takeuchi | 0:4a81aff3ea7b | 48 | } |
| takeuchi | 0:4a81aff3ea7b | 49 | gy1_avg=round3(gy1_sum/10); |
| takeuchi | 0:4a81aff3ea7b | 50 | |
| takeuchi | 0:4a81aff3ea7b | 51 | gy1_wk1=round3((gy1_avg-gy1_center)*1000/6.7); |
| takeuchi | 0:4a81aff3ea7b | 52 | gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2); |
| takeuchi | 0:4a81aff3ea7b | 53 | gy1_wk0=gy1_wk1; |
| takeuchi | 0:4a81aff3ea7b | 54 | |
| takeuchi | 0:4a81aff3ea7b | 55 | if(gy1_sgm > 90){ |
| takeuchi | 0:4a81aff3ea7b | 56 | gy1_sgm=90; |
| takeuchi | 0:4a81aff3ea7b | 57 | } |
| takeuchi | 0:4a81aff3ea7b | 58 | else if(gy1_sgm < -90){ |
| takeuchi | 0:4a81aff3ea7b | 59 | gy1_sgm=-90; |
| takeuchi | 0:4a81aff3ea7b | 60 | } |
| takeuchi | 0:4a81aff3ea7b | 61 | |
| takeuchi | 0:4a81aff3ea7b | 62 | lcd.locate(0,1); |
| takeuchi | 0:4a81aff3ea7b | 63 | lcd.printf("%3.4f %3.4f",gy1_avg,gy1_center); |
| takeuchi | 0:4a81aff3ea7b | 64 | lcd.locate(0,2); |
| takeuchi | 0:4a81aff3ea7b | 65 | lcd.printf("Deg:%2.0f",gy1_sgm); |
| takeuchi | 0:4a81aff3ea7b | 66 | }//while |
| takeuchi | 0:4a81aff3ea7b | 67 | }//main |
| takeuchi | 0:4a81aff3ea7b | 68 |