Takeuchi Kouichi
/
2bk0819_Gyro_test07
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp@0:43eac3418598, 2011-09-20 (annotated)
- Committer:
- takeuchi
- Date:
- Tue Sep 20 11:49:26 2011 +0000
- Revision:
- 0:43eac3418598
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| takeuchi | 0:43eac3418598 | 1 | // Gyro test07 |
| takeuchi | 0:43eac3418598 | 2 | // Akidukidensi AE-GYRO-SMD |
| takeuchi | 0:43eac3418598 | 3 | // with 2Gyro+2Servo |
| takeuchi | 0:43eac3418598 | 4 | #include "mbed.h" |
| takeuchi | 0:43eac3418598 | 5 | #include "TextLCD0420.h" |
| takeuchi | 0:43eac3418598 | 6 | |
| takeuchi | 0:43eac3418598 | 7 | #define ON 1 |
| takeuchi | 0:43eac3418598 | 8 | #define OFF 0 |
| takeuchi | 0:43eac3418598 | 9 | #define Gy1_offset 0.01 |
| takeuchi | 0:43eac3418598 | 10 | |
| takeuchi | 0:43eac3418598 | 11 | DigitalOut mled0(LED1); |
| takeuchi | 0:43eac3418598 | 12 | DigitalOut mled1(LED2); |
| takeuchi | 0:43eac3418598 | 13 | AnalogIn gy1_adc(p16); |
| takeuchi | 0:43eac3418598 | 14 | AnalogIn gy2_adc(p17); |
| takeuchi | 0:43eac3418598 | 15 | PwmOut servo1(p21); |
| takeuchi | 0:43eac3418598 | 16 | PwmOut servo2(p22); |
| takeuchi | 0:43eac3418598 | 17 | |
| takeuchi | 0:43eac3418598 | 18 | TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3 |
| takeuchi | 0:43eac3418598 | 19 | |
| takeuchi | 0:43eac3418598 | 20 | double round3(double x){ |
| takeuchi | 0:43eac3418598 | 21 | double y; |
| takeuchi | 0:43eac3418598 | 22 | y=double(int((x+0.005)*100))/100; |
| takeuchi | 0:43eac3418598 | 23 | return(y); |
| takeuchi | 0:43eac3418598 | 24 | } |
| takeuchi | 0:43eac3418598 | 25 | |
| takeuchi | 0:43eac3418598 | 26 | int main() { |
| takeuchi | 0:43eac3418598 | 27 | float gy1_center; |
| takeuchi | 0:43eac3418598 | 28 | float gy1_sgm=0; |
| takeuchi | 0:43eac3418598 | 29 | float gy1_wk0=0,gy1_wk1=0; |
| takeuchi | 0:43eac3418598 | 30 | float gy1_avg=0,gy1_sum; |
| takeuchi | 0:43eac3418598 | 31 | int i,gy1_lc=9,gy1_lc_old=0; |
| takeuchi | 0:43eac3418598 | 32 | float pwidth1; |
| takeuchi | 0:43eac3418598 | 33 | float gy2_center; |
| takeuchi | 0:43eac3418598 | 34 | float gy2_sgm=0; |
| takeuchi | 0:43eac3418598 | 35 | float gy2_wk0=0,gy2_wk1=0; |
| takeuchi | 0:43eac3418598 | 36 | float gy2_avg=0,gy2_sum; |
| takeuchi | 0:43eac3418598 | 37 | int gy2_lc=9,gy2_lc_old=0; |
| takeuchi | 0:43eac3418598 | 38 | float pwidth2; |
| takeuchi | 0:43eac3418598 | 39 | |
| takeuchi | 0:43eac3418598 | 40 | lcd.cls(); |
| takeuchi | 0:43eac3418598 | 41 | lcd.locate(0,0); |
| takeuchi | 0:43eac3418598 | 42 | lcd.printf("*** Gyro test07 ****\n"); |
| takeuchi | 0:43eac3418598 | 43 | servo1.period_ms(20); |
| takeuchi | 0:43eac3418598 | 44 | servo2.period_ms(20); |
| takeuchi | 0:43eac3418598 | 45 | |
| takeuchi | 0:43eac3418598 | 46 | gy1_center=0; |
| takeuchi | 0:43eac3418598 | 47 | gy2_center=0; |
| takeuchi | 0:43eac3418598 | 48 | for(i=0;i<300;i++){ |
| takeuchi | 0:43eac3418598 | 49 | gy1_center=gy1_center+round3(gy1_adc.read()*3.3); |
| takeuchi | 0:43eac3418598 | 50 | gy2_center=gy2_center+round3(gy2_adc.read()*3.3); |
| takeuchi | 0:43eac3418598 | 51 | wait(0.001); |
| takeuchi | 0:43eac3418598 | 52 | } |
| takeuchi | 0:43eac3418598 | 53 | gy1_center=round3(gy1_center/300); |
| takeuchi | 0:43eac3418598 | 54 | gy2_center=round3(gy2_center/300); |
| takeuchi | 0:43eac3418598 | 55 | servo1.pulsewidth(0.0015); |
| takeuchi | 0:43eac3418598 | 56 | servo2.pulsewidth(0.0015); |
| takeuchi | 0:43eac3418598 | 57 | |
| takeuchi | 0:43eac3418598 | 58 | while(1){ |
| takeuchi | 0:43eac3418598 | 59 | gy1_sum=0; |
| takeuchi | 0:43eac3418598 | 60 | gy2_sum=0; |
| takeuchi | 0:43eac3418598 | 61 | for(i=0;i<20;i++){ |
| takeuchi | 0:43eac3418598 | 62 | gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3); |
| takeuchi | 0:43eac3418598 | 63 | gy2_sum=gy2_sum+round3(gy2_adc.read()*3.3); |
| takeuchi | 0:43eac3418598 | 64 | wait(0.005); |
| takeuchi | 0:43eac3418598 | 65 | } |
| takeuchi | 0:43eac3418598 | 66 | gy1_avg=round3(gy1_sum/20); |
| takeuchi | 0:43eac3418598 | 67 | gy2_avg=round3(gy2_sum/20); |
| takeuchi | 0:43eac3418598 | 68 | |
| takeuchi | 0:43eac3418598 | 69 | gy1_wk1=round3((gy1_avg-gy1_center)*1000/5); |
| takeuchi | 0:43eac3418598 | 70 | gy2_wk1=round3((gy2_avg-gy2_center)*1000/5); |
| takeuchi | 0:43eac3418598 | 71 | if(gy1_wk1 > 0){ |
| takeuchi | 0:43eac3418598 | 72 | gy1_wk1=gy1_wk1*1.07;// migi hosei |
| takeuchi | 0:43eac3418598 | 73 | } |
| takeuchi | 0:43eac3418598 | 74 | if(gy1_wk1 < 0){ |
| takeuchi | 0:43eac3418598 | 75 | gy1_wk1=gy1_wk1*1.0;//hidari hosei |
| takeuchi | 0:43eac3418598 | 76 | } |
| takeuchi | 0:43eac3418598 | 77 | |
| takeuchi | 0:43eac3418598 | 78 | if(gy2_wk1 > 0){ |
| takeuchi | 0:43eac3418598 | 79 | gy2_wk1=gy2_wk1*1.0;// migi hosei |
| takeuchi | 0:43eac3418598 | 80 | } |
| takeuchi | 0:43eac3418598 | 81 | if(gy2_wk1 < 0){ |
| takeuchi | 0:43eac3418598 | 82 | gy2_wk1=gy2_wk1*1.1;//hidari hosei |
| takeuchi | 0:43eac3418598 | 83 | } |
| takeuchi | 0:43eac3418598 | 84 | |
| takeuchi | 0:43eac3418598 | 85 | gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2); |
| takeuchi | 0:43eac3418598 | 86 | gy2_sgm=round3(gy2_sgm+(gy2_wk0+gy2_wk1)*0.1/2); |
| takeuchi | 0:43eac3418598 | 87 | gy1_wk0=gy1_wk1; |
| takeuchi | 0:43eac3418598 | 88 | gy2_wk0=gy2_wk1; |
| takeuchi | 0:43eac3418598 | 89 | |
| takeuchi | 0:43eac3418598 | 90 | if(gy1_sgm > 90){ |
| takeuchi | 0:43eac3418598 | 91 | gy1_sgm=90; |
| takeuchi | 0:43eac3418598 | 92 | } |
| takeuchi | 0:43eac3418598 | 93 | else if(gy1_sgm < -90){ |
| takeuchi | 0:43eac3418598 | 94 | gy1_sgm=-90; |
| takeuchi | 0:43eac3418598 | 95 | } |
| takeuchi | 0:43eac3418598 | 96 | |
| takeuchi | 0:43eac3418598 | 97 | if(gy2_sgm > 90){ |
| takeuchi | 0:43eac3418598 | 98 | gy2_sgm=90; |
| takeuchi | 0:43eac3418598 | 99 | } |
| takeuchi | 0:43eac3418598 | 100 | else if(gy2_sgm < -90){ |
| takeuchi | 0:43eac3418598 | 101 | gy2_sgm=-90; |
| takeuchi | 0:43eac3418598 | 102 | } |
| takeuchi | 0:43eac3418598 | 103 | |
| takeuchi | 0:43eac3418598 | 104 | pwidth1=-gy1_sgm; |
| takeuchi | 0:43eac3418598 | 105 | pwidth1=(pwidth1+90)/180/1000+0.001; |
| takeuchi | 0:43eac3418598 | 106 | gy1_lc=(gy1_sgm-5)/10+9; |
| takeuchi | 0:43eac3418598 | 107 | if(gy1_lc >20){ |
| takeuchi | 0:43eac3418598 | 108 | gy1_lc=19; |
| takeuchi | 0:43eac3418598 | 109 | } |
| takeuchi | 0:43eac3418598 | 110 | if(gy1_lc <0){ |
| takeuchi | 0:43eac3418598 | 111 | gy1_lc=0; |
| takeuchi | 0:43eac3418598 | 112 | } |
| takeuchi | 0:43eac3418598 | 113 | |
| takeuchi | 0:43eac3418598 | 114 | pwidth2=gy2_sgm; |
| takeuchi | 0:43eac3418598 | 115 | pwidth2=(pwidth2+90)/180/1000+0.001; |
| takeuchi | 0:43eac3418598 | 116 | gy2_lc=(gy2_sgm-5)/10+9; |
| takeuchi | 0:43eac3418598 | 117 | if(gy2_lc >20){ |
| takeuchi | 0:43eac3418598 | 118 | gy2_lc=19; |
| takeuchi | 0:43eac3418598 | 119 | } |
| takeuchi | 0:43eac3418598 | 120 | if(gy2_lc <0){ |
| takeuchi | 0:43eac3418598 | 121 | gy2_lc=0; |
| takeuchi | 0:43eac3418598 | 122 | } |
| takeuchi | 0:43eac3418598 | 123 | |
| takeuchi | 0:43eac3418598 | 124 | lcd.locate(0,1); |
| takeuchi | 0:43eac3418598 | 125 | //lcd.printf("%3.2f %3.2f %2.0f %2.1f",gy1_avg,gy1_center,gy1_sgm,pwidth*1000); |
| takeuchi | 0:43eac3418598 | 126 | //lcd.locate(0,2); |
| takeuchi | 0:43eac3418598 | 127 | lcd.printf("--------+--------"); |
| takeuchi | 0:43eac3418598 | 128 | lcd.locate(gy1_lc_old,2); |
| takeuchi | 0:43eac3418598 | 129 | lcd.printf(" "); |
| takeuchi | 0:43eac3418598 | 130 | lcd.locate(gy1_lc,2); |
| takeuchi | 0:43eac3418598 | 131 | lcd.printf("*"); |
| takeuchi | 0:43eac3418598 | 132 | gy1_lc_old=gy1_lc; |
| takeuchi | 0:43eac3418598 | 133 | servo1.pulsewidth(pwidth1); |
| takeuchi | 0:43eac3418598 | 134 | lcd.locate(gy2_lc_old,3); |
| takeuchi | 0:43eac3418598 | 135 | lcd.printf(" "); |
| takeuchi | 0:43eac3418598 | 136 | lcd.locate(gy2_lc,3); |
| takeuchi | 0:43eac3418598 | 137 | lcd.printf("*"); |
| takeuchi | 0:43eac3418598 | 138 | gy2_lc_old=gy2_lc; |
| takeuchi | 0:43eac3418598 | 139 | servo2.pulsewidth(pwidth2); |
| takeuchi | 0:43eac3418598 | 140 | }//while |
| takeuchi | 0:43eac3418598 | 141 | }//main |
| takeuchi | 0:43eac3418598 | 142 |