Diff: main.cpp

Revision:

0:c7160959f043

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 12 15:55:31 2020 +0000
@@ -0,0 +1,162 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "Adafruit_SSD1306.h"
+#include "Adafruit_GFX_Config.h"
+#include <string>
+
+#define NUM 10
+
+DigitalOut myled(LED1);
+Serial pc(USBTX, USBRX,115200);
+
+MPU6050 mpu;
+
+DigitalIn  Slides[]  = {
+    DigitalIn( D1 ),
+    DigitalIn( D0 ),
+    DigitalIn( D3 ),
+    DigitalIn( D6 )
+};
+
+DigitalIn  Tacts[]  = {
+    DigitalIn( D8 ),
+    DigitalIn( D9 ),
+    DigitalIn( D11 ),
+    DigitalIn( D12 )
+};
+
+DigitalOut  leds[]  = {
+    DigitalOut( A3 ),
+    DigitalOut( A0 ),
+    DigitalOut( D13 ),
+    DigitalOut( A2 )
+};
+
+// OLEDとの通信に使用するI2Cオブジェクトを生成
+I2C i2c(PB_7,PB_6);
+// OLED制御クラスのインスタンス化
+uint8_t i2cAddress = SSD_I2C_ADDRESS;
+int rawHeight = 32;
+int rawWidth = 128;
+Adafruit_SSD1306_I2c oled(i2c, D10, i2cAddress);
+
+void print(std::string str);
+
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+
+int sum_ax,sum_ay,sum_az;
+int sum_gx,sum_gy,sum_gz;
+char str_ax[15],str_ay[15],str_az[15];
+char str_gx[15],str_gy[15],str_gz[15];
+
+int main()
+{
+//    char a[]= {"HELLO"};
+//    char b[]= {" WORLD"};
+//    char c[]= {"\n"};
+    char title1[] = {"AC[m/ss]"};
+    char title2[] = {"GY[rad/s]"};
+    char but=0x00;
+    for(int i=0; i<4; i++) {
+        Slides[i].mode(PullUp);
+        Tacts[i].mode(PullUp);
+    }
+    pc.printf("\033[2J");
+    pc.printf("MPU6050 test\n\n");
+    pc.printf("MPU6050 initialize \n");
+
+    mpu.initialize();
+    pc.printf("MPU6050 testConnection \n");
+
+    bool mpu6050TestResult = mpu.testConnection();
+    if(mpu6050TestResult) {
+        pc.printf("MPU6050 test passed \n");
+    } else {
+        pc.printf("MPU6050 test failed \n");
+    }
+
+    pc.printf("\033[2J");
+    pc.printf("\033[1;1HACCEL_x:    0.0000[m/s^2]");
+    pc.printf("\033[2;1HACCEL_y:    0.0000[m/s^2]");
+    pc.printf("\033[3;1HACCEL_z:    0.0000[m/s^2]");
+
+    pc.printf("\033[5;1HGYRO_x :    0.0000[rad/s]");
+    pc.printf("\033[6;1HGYRO_y :    0.0000[rad/s]");
+    pc.printf("\033[7;1HGYRO_z :    0.0000[rad/s]");
+    oled.clearDisplay();
+    oled.setTextCursor(0,0);
+    print(title1);
+    oled.setTextCursor(64,0);
+    print(title2);
+    while(1) {
+        myled = !myled;
+        for(int i=0; i < NUM; i++) {
+            mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+            sum_ax += ax;
+            sum_ay += ay;
+            sum_az += az;
+
+            sum_gx += gx;
+            sum_gy += gy;
+            sum_gz += gz;
+        }
+        
+        for(int i=0; i<4; i++) {
+            if ( !(Slides[i]) ) but |= (0x01<<i);
+            if ( !(Tacts[i]) )  but |= (0x10<<i);
+            if( Slides[i] ^ Tacts[i] ) leds[i] = 0;
+            else leds[i] = 1;
+        }
+
+        sprintf(str_ax, "%+08.5g", float(sum_ax)/(2048*NUM));
+        sprintf(str_ay, "%+08.5g", float(sum_ay)/(2048*NUM));
+        sprintf(str_az, "%+08.5g", float(sum_az)/(2048*NUM));
+
+        sprintf(str_gx, "%+08.5g", float(sum_gx)/(16.4*NUM));
+        sprintf(str_gy, "%+08.5g", float(sum_gy)/(16.4*NUM));
+        sprintf(str_gz, "%+08.5g", float(sum_gz)/(16.4*NUM));
+
+        oled.setTextCursor(0,8);
+        print(str_ax);
+        oled.setTextCursor(0,16);
+        print(str_ay);
+        oled.setTextCursor(0,24);
+        print(str_az);
+
+        oled.setTextCursor(64,8);
+        print(str_gx);
+        oled.setTextCursor(64,16);
+        print(str_gy);
+        oled.setTextCursor(64,24);
+        print(str_gz);
+
+//        pc.printf("\033[1;10H%s",str_ax);
+//        pc.printf("\033[2;10H%s",str_ay);
+//        pc.printf("\033[3;10H%s",str_az);
+//
+//        pc.printf("\033[5;9H%s",str_gx);
+//        pc.printf("\033[6;9H%s",str_gy);
+//        pc.printf("\033[7;9H%s",str_gz);
+//        pc.printf("\033[8;1Hbut:0x%x",but);
+
+        sum_ax = 0;
+        sum_ay = 0;
+        sum_az = 0;
+
+        sum_gx = 0;
+        sum_gy = 0;
+        sum_gz = 0;
+
+        but=0x00;
+    }
+}
+
+void print(std::string str)
+{
+    for(size_t i=0; i<str.size() ; i++) {
+        oled._putc(str[i]);
+    }
+    // 表示を更新
+    oled.display();
+}