Diff: main.cpp

Revision:

0:b9cab5bc1074

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jan 15 06:55:59 2018 +0000
@@ -0,0 +1,70 @@
+#include "mbed.h"
+
+AnalogIn gyro1(A4);
+AnalogIn gyro2(A5);
+
+/* 小数点以下4桁目を四捨五入 */
+double round4(double x) {
+  double y;
+  y = double(int((x + 0.0005)*1000))/1000;
+  return(y);
+}
+
+int main() {
+  float g1_offset;              // オフセット[V]
+  float g1_sgm = 0;                 // 積分値[deg]
+  float g1_deg_s;
+  float g1_deg;
+  float g1_wk0 = 0, g1_wk1 = 0;
+  float g1_avg = 0, g1_sum;
+  int i,j;
+  
+  /* オフセットを計算 */
+  g1_offset = 0;
+  for(i=0; i<300; i++) {
+    g1_offset = g1_offset + round4(gyro1.read()*3.3);
+    wait(0.001);
+  }
+  g1_offset = g1_offset/300; // オフセット電圧[V]
+  
+  while(1) {
+    /* 平均値を計算 */
+    g1_sum = 0;
+    j = 0;
+    for(i=0; i<100; i++) {
+      g1_sum = g1_sum + round4(gyro1.read()*3.3);
+      wait(0.001);
+    }
+    g1_avg = g1_sum/100; // 平均電圧[V]
+    
+    /* 回転速度を計算 */
+    g1_deg_s = (g1_avg - g1_offset)*1000/0.67;
+    if(g1_deg_s > 90) {
+      g1_deg_s = 90;
+    }
+    if(g1_deg_s < -90) {
+      g1_deg_s = -90;
+    }
+    
+    /* 積分値を計算 */
+    g1_wk1 = g1_deg_s;
+    if(g1_wk1 > 0) {
+      g1_wk1 = g1_wk1*1.0; // 補正
+    }
+    if(g1_wk1 < 0) {
+      g1_wk1 = g1_wk1*1.0; // 補正
+    }
+    g1_sgm = g1_sgm + (g1_wk0 + g1_wk1)*0.1/2; // 回転角度
+    g1_wk0 = g1_wk1;
+    
+    if(g1_sgm > 180) {
+      g1_sgm = 180;
+    }
+    else if(g1_sgm < -180) {
+      g1_sgm = -180;
+    }
+    
+    printf("%5.1f[deg/s]\t", g1_deg_s);
+    printf("%5.1f[deg]\n", g1_sgm);
+  }
+}
\ No newline at end of file