gyro sensor
Dependencies: mbed
main.cpp@0:b9cab5bc1074, 2018-01-15 (annotated)
- Committer:
- kato8
- Date:
- Mon Jan 15 06:55:59 2018 +0000
- Revision:
- 0:b9cab5bc1074
gyro sensor
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kato8 | 0:b9cab5bc1074 | 1 | #include "mbed.h" |
kato8 | 0:b9cab5bc1074 | 2 | |
kato8 | 0:b9cab5bc1074 | 3 | AnalogIn gyro1(A4); |
kato8 | 0:b9cab5bc1074 | 4 | AnalogIn gyro2(A5); |
kato8 | 0:b9cab5bc1074 | 5 | |
kato8 | 0:b9cab5bc1074 | 6 | /* 小数点以下4桁目を四捨五入 */ |
kato8 | 0:b9cab5bc1074 | 7 | double round4(double x) { |
kato8 | 0:b9cab5bc1074 | 8 | double y; |
kato8 | 0:b9cab5bc1074 | 9 | y = double(int((x + 0.0005)*1000))/1000; |
kato8 | 0:b9cab5bc1074 | 10 | return(y); |
kato8 | 0:b9cab5bc1074 | 11 | } |
kato8 | 0:b9cab5bc1074 | 12 | |
kato8 | 0:b9cab5bc1074 | 13 | int main() { |
kato8 | 0:b9cab5bc1074 | 14 | float g1_offset; // オフセット[V] |
kato8 | 0:b9cab5bc1074 | 15 | float g1_sgm = 0; // 積分値[deg] |
kato8 | 0:b9cab5bc1074 | 16 | float g1_deg_s; |
kato8 | 0:b9cab5bc1074 | 17 | float g1_deg; |
kato8 | 0:b9cab5bc1074 | 18 | float g1_wk0 = 0, g1_wk1 = 0; |
kato8 | 0:b9cab5bc1074 | 19 | float g1_avg = 0, g1_sum; |
kato8 | 0:b9cab5bc1074 | 20 | int i,j; |
kato8 | 0:b9cab5bc1074 | 21 | |
kato8 | 0:b9cab5bc1074 | 22 | /* オフセットを計算 */ |
kato8 | 0:b9cab5bc1074 | 23 | g1_offset = 0; |
kato8 | 0:b9cab5bc1074 | 24 | for(i=0; i<300; i++) { |
kato8 | 0:b9cab5bc1074 | 25 | g1_offset = g1_offset + round4(gyro1.read()*3.3); |
kato8 | 0:b9cab5bc1074 | 26 | wait(0.001); |
kato8 | 0:b9cab5bc1074 | 27 | } |
kato8 | 0:b9cab5bc1074 | 28 | g1_offset = g1_offset/300; // オフセット電圧[V] |
kato8 | 0:b9cab5bc1074 | 29 | |
kato8 | 0:b9cab5bc1074 | 30 | while(1) { |
kato8 | 0:b9cab5bc1074 | 31 | /* 平均値を計算 */ |
kato8 | 0:b9cab5bc1074 | 32 | g1_sum = 0; |
kato8 | 0:b9cab5bc1074 | 33 | j = 0; |
kato8 | 0:b9cab5bc1074 | 34 | for(i=0; i<100; i++) { |
kato8 | 0:b9cab5bc1074 | 35 | g1_sum = g1_sum + round4(gyro1.read()*3.3); |
kato8 | 0:b9cab5bc1074 | 36 | wait(0.001); |
kato8 | 0:b9cab5bc1074 | 37 | } |
kato8 | 0:b9cab5bc1074 | 38 | g1_avg = g1_sum/100; // 平均電圧[V] |
kato8 | 0:b9cab5bc1074 | 39 | |
kato8 | 0:b9cab5bc1074 | 40 | /* 回転速度を計算 */ |
kato8 | 0:b9cab5bc1074 | 41 | g1_deg_s = (g1_avg - g1_offset)*1000/0.67; |
kato8 | 0:b9cab5bc1074 | 42 | if(g1_deg_s > 90) { |
kato8 | 0:b9cab5bc1074 | 43 | g1_deg_s = 90; |
kato8 | 0:b9cab5bc1074 | 44 | } |
kato8 | 0:b9cab5bc1074 | 45 | if(g1_deg_s < -90) { |
kato8 | 0:b9cab5bc1074 | 46 | g1_deg_s = -90; |
kato8 | 0:b9cab5bc1074 | 47 | } |
kato8 | 0:b9cab5bc1074 | 48 | |
kato8 | 0:b9cab5bc1074 | 49 | /* 積分値を計算 */ |
kato8 | 0:b9cab5bc1074 | 50 | g1_wk1 = g1_deg_s; |
kato8 | 0:b9cab5bc1074 | 51 | if(g1_wk1 > 0) { |
kato8 | 0:b9cab5bc1074 | 52 | g1_wk1 = g1_wk1*1.0; // 補正 |
kato8 | 0:b9cab5bc1074 | 53 | } |
kato8 | 0:b9cab5bc1074 | 54 | if(g1_wk1 < 0) { |
kato8 | 0:b9cab5bc1074 | 55 | g1_wk1 = g1_wk1*1.0; // 補正 |
kato8 | 0:b9cab5bc1074 | 56 | } |
kato8 | 0:b9cab5bc1074 | 57 | g1_sgm = g1_sgm + (g1_wk0 + g1_wk1)*0.1/2; // 回転角度 |
kato8 | 0:b9cab5bc1074 | 58 | g1_wk0 = g1_wk1; |
kato8 | 0:b9cab5bc1074 | 59 | |
kato8 | 0:b9cab5bc1074 | 60 | if(g1_sgm > 180) { |
kato8 | 0:b9cab5bc1074 | 61 | g1_sgm = 180; |
kato8 | 0:b9cab5bc1074 | 62 | } |
kato8 | 0:b9cab5bc1074 | 63 | else if(g1_sgm < -180) { |
kato8 | 0:b9cab5bc1074 | 64 | g1_sgm = -180; |
kato8 | 0:b9cab5bc1074 | 65 | } |
kato8 | 0:b9cab5bc1074 | 66 | |
kato8 | 0:b9cab5bc1074 | 67 | printf("%5.1f[deg/s]\t", g1_deg_s); |
kato8 | 0:b9cab5bc1074 | 68 | printf("%5.1f[deg]\n", g1_sgm); |
kato8 | 0:b9cab5bc1074 | 69 | } |
kato8 | 0:b9cab5bc1074 | 70 | } |