Diff: main.cpp

Revision:

0:123b50ed3ad9

Child:

1:fb672eb52bd6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 17 23:40:36 2017 +0000
@@ -0,0 +1,282 @@
+/******************************************************************************
+ *  NM500 NeuroSheild Board and mpu6050 imu Test example
+ *  revision 1.0, 8/18, 2017
+ *  Copyright (c) 2017 nepes inc.
+ ******************************************************************************/
+
+#include "mbed.h"
+#include <NeuroShield.h>
+#include <NeuroShieldSPI.h>
+#include <mpu6050.h>
+
+// for NM500
+#define MOTION_REPEAT_COUNT 3  // number of samples to assemble a vector
+#define MOTION_SIGNAL_COUNT 8  // d_ax, d_ay, d_az, d_gx, d_gy, d_gz, da, dg
+#define MOTION_CAPTURE_COUNT 20
+
+#define DEFAULT_MAXIF 500
+
+NeuroShield hnn;
+MPU6050 mpu(0x68, PB_9, PB_8);  // SDA:(D14=PB_9) SCL(D15=PB_8) <= SB143/SB138 must close for I2C on A4/A5 and SB147/SB157 must open!!!
+Serial pc(USBTX, USBRX);
+
+DigitalOut sdcard_ss(D6);       // SDCARD_SSn
+DigitalOut arduino_con(D5);     // SPI_SEL
+
+int16_t ax, ay, az, gx, gy, gz;
+
+uint8_t learn_cat = 0;     // category to learn
+uint8_t prev_cat = 0;  // previously recognized category
+uint16_t dist, cat, nid, nsr, ncount;  // response from the neurons
+uint16_t prev_ncount = 0;
+
+int16_t min_a = 0xFFFF, max_a = 0, min_g = 0xFFFF, max_g = 0, da = 0, dg = 0;   // reset, or not, at each feature extraction
+
+uint8_t vector[MOTION_REPEAT_COUNT * MOTION_SIGNAL_COUNT];       // vector holding the pattern to learn or recognize
+
+void mpu6050Calibration()
+{
+    int i, j;
+    long sum_ax = 0, sum_ay = 0, sum_az = 0, sum_gx = 0, sum_gy = 0, sum_gz = 0;
+    int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz;
+    int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;
+    
+    for (i = 0; i < 100; i++) {
+        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+    }
+    
+    for (j = 0; j < 5; j++) {
+        for (i = 0; i < 100; 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;
+        }
+        
+        mean_ax = sum_ax / 100;
+        mean_ay = sum_ay / 100;
+        mean_az = sum_az / 100;
+        mean_gx = sum_gx / 100;
+        mean_gy = sum_gy / 100;
+        mean_gz = sum_gz / 100;
+        
+        // MPU6050_GYRO_FS_1000 : offset = (-1) * mean_g
+        // MPU6050_ACCEL_FS_8   : offset = (-0.5) * mean_a
+        ax_offset = (-mean_ax) / 2;
+        ay_offset = (-mean_ay) / 2;
+        az_offset = (-mean_az) / 2;
+        gx_offset = -mean_gx;
+        gy_offset = -mean_gy;
+        gz_offset = -mean_gz;
+        
+        // set
+        mpu.setXAccelOffset(ax_offset);
+        mpu.setYAccelOffset(ay_offset);
+        mpu.setZAccelOffset(az_offset);
+        mpu.setXGyroOffset(gx_offset);
+        mpu.setYGyroOffset(gy_offset);
+        mpu.setZGyroOffset(gz_offset);
+    }
+}
+
+void extractFeatureVector()
+{
+    int i;
+    int16_t min_ax, min_ay, min_az, max_ax, max_ay, max_az;
+    int16_t min_gx, min_gy, min_gz, max_gx, max_gy, max_gz;
+    uint32_t norm_ax, norm_ay, norm_az, norm_gx, norm_gy, norm_gz;
+    int32_t d_ax, d_ay, d_az, d_gx, d_gy, d_gz;
+    int32_t da_local, dg_local;
+
+    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
+    max_ax = min_ax = ax;
+    max_ay = min_ay = ay;
+    max_az = min_az = az;
+    max_gx = min_gx = gx;
+    max_gy = min_gy = gy;
+    max_gz = min_gz = gz;
+  
+    for (i = 0; i < MOTION_CAPTURE_COUNT; i++) {
+        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+    
+        if (ax < min_ax)
+            min_ax = ax;
+        else if (ax > max_ax)
+            max_ax = ax;
+
+        if (ay < min_ay)
+            min_ay = ay;
+        else if(ay > max_ay)
+            max_ay = ay;
+
+        if (az < min_az)
+            min_az = az;
+        else if (az > max_az)
+            max_az = az;
+
+        if (gx < min_gx)
+            min_gx = gx;
+        else if (gx > max_gx)
+            max_gx = gx;
+
+        if (gy < min_gy)
+            min_gy = gy;
+        else if (gy > max_gy)
+            max_gy = gy;
+
+        if (gz < min_gz)
+            min_gz = gz;
+        else if (gz > max_gz)
+            max_gz = gz;
+    }
+
+    d_ax = max_ax - min_ax;
+    d_ay = max_ay - min_ay;
+    d_az = max_az - min_az;
+
+    d_gx = max_gx - min_gx;
+    d_gy = max_gy - min_gy;
+    d_gz = max_gz - min_gz;
+
+    da_local = d_ax;
+    if (d_ay > da_local)
+        da_local = d_ay;
+    if (d_az > da_local)
+        da_local = d_az;
+
+    dg_local = d_gx;
+    if (d_gy > dg_local)
+        dg_local = d_gy;
+    if (d_gz > dg_local)
+        dg_local = d_gz;
+
+    norm_ax = d_ax; norm_ax = norm_ax * 255 / da_local;
+    norm_ay = d_ay; norm_ay = norm_ay * 255 / da_local;
+    norm_az = d_az; norm_az = norm_az * 255 / da_local;
+
+    norm_gx = d_gx; norm_gx = norm_gx * 255 / dg_local;
+    norm_gy = d_gy; norm_gy = norm_gy * 255 / dg_local;
+    norm_gz = d_gz; norm_gz = norm_gz * 255 / dg_local;
+
+    for (i = 0; i < MOTION_REPEAT_COUNT; i++) {
+        vector[i * MOTION_SIGNAL_COUNT] = norm_ax & 0x00ff;
+        vector[(i * MOTION_SIGNAL_COUNT) + 1] = norm_ay & 0x00ff;
+        vector[(i * MOTION_SIGNAL_COUNT) + 2] = norm_az & 0x00ff;
+        vector[(i * MOTION_SIGNAL_COUNT) + 3] = norm_gx & 0x00ff;
+        vector[(i * MOTION_SIGNAL_COUNT) + 4] = norm_gy & 0x00ff;
+        vector[(i * MOTION_SIGNAL_COUNT) + 5] = norm_gz & 0x00ff;
+        if (da_local >= 4096)
+            vector[(i * MOTION_SIGNAL_COUNT) + 6] = 0xff;
+        else
+            vector[(i * MOTION_SIGNAL_COUNT) + 6] = ((da_local >> 4) & 0x00ff);
+        if (dg_local >= 4096)
+            vector[(i * MOTION_SIGNAL_COUNT) + 7] = 0xff;
+        else
+            vector[(i * MOTION_SIGNAL_COUNT) + 7] = ((dg_local >> 4) & 0x00ff);
+    }
+}
+
+int main()
+{
+    int i, input_key[2];
+    
+    printf("\n#### NM500 SHIELD BOARD ####\n\n");
+    arduino_con = LOW;
+    sdcard_ss = HIGH;
+    wait(0.5);
+    
+    printf("Start NM500 initialzation...\n");
+    if (hnn.begin() != 0) {
+        hnn.forget(DEFAULT_MAXIF);
+        printf("  NM500 initial success (MaxIF = 0x%04x)\n", DEFAULT_MAXIF);
+        printf("  There are %d neurons\n", hnn.total_neurons);
+    }
+    else {
+        printf("  NM500 is NOT properly connected!!\n");
+        printf("  Check the connection and Reboot again!\n");
+        while (1);
+    }
+    
+    // initialize mpu6050
+    printf("\nStart MPU-6050 initialization...\n");
+    mpu.initialize();
+    // set gyro & accel range
+    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
+    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
+    
+    // verify connection
+    for (i = 0; i < 10; i++) {
+        if (mpu.testConnection()) {
+            printf("  MPU-6050 connection success\n");
+            break;
+        }
+        else if (i == 9) {
+            printf("  MPU-6050 connection failed\n");
+            printf("  Check the connection and Reboot again!\n");
+            while (1);
+        }
+        wait(0.1);
+    }
+    
+    // wait for ready
+    printf("  About to calibrate. Make sure your board is stable and upright\n");
+    // start message
+    printf("  Start calibration, don't touch it until you see a finish message\n");
+    // reset offsets
+    mpu.setXAccelOffset(0);
+    mpu.setYAccelOffset(0);
+    mpu.setZAccelOffset(0);
+    mpu.setXGyroOffset(0);
+    mpu.setYGyroOffset(0);
+    mpu.setZGyroOffset(0);
+    mpu6050Calibration();
+    // end message
+    printf("  MPU-6050 calibration end!!\n\n");
+    
+    printf("Move the module vertically or horizontally...\n");
+    printf("type 1 + Enter if up <-> down motion\n");
+    printf("type 2 + Enter if left <-> right motion\n");
+    printf("type 3 + Enter if front <-> back motion\n");
+    printf("type 0 + Enter for any other motion\n");
+    
+    // main loop
+    while (1) {
+        if (pc.readable()) {
+            input_key[0] = input_key[1];
+            input_key[1] = pc.getc();
+            if (input_key[1] == 0x0D) {     // enter key
+                learn_cat = input_key[0] - '0';
+                if (learn_cat < 4) {
+                    printf("Learning motion category %d\n", learn_cat);
+                    for (int i = 0; i < 5; i++) {
+                        extractFeatureVector();
+                        ncount = hnn.learn(vector, MOTION_REPEAT_COUNT * MOTION_SIGNAL_COUNT, learn_cat);
+                        if (ncount != prev_ncount) {
+                            prev_cat = learn_cat;
+                            prev_ncount = ncount;
+                        }
+                    }
+                    printf("Neurons=%d\n", ncount);
+                }
+            }
+        }
+        else {                          // recognize
+            extractFeatureVector();
+            hnn.classify(vector, MOTION_REPEAT_COUNT * MOTION_SIGNAL_COUNT, &dist, &cat, &nid);
+            if (cat != 0xFFFF) {
+                prev_cat = cat;
+                if (cat & 0x8000)
+                    printf("Motion #%d (degenerated)\n", (cat & 0x7FFF));
+                else
+                    printf("Motion #%d \n", cat);
+            }
+            else if (prev_cat != 0xFFFF) {
+                prev_cat = cat;
+            }
+        }
+    }
+}
\ No newline at end of file