SS2 / Mbed 2 deprecated gyro

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?

UserRevisionLine numberNew 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 }