main.cpp

00001 /******************************************************************************
00002  *  NM500 NeuroShield Board and mpu6050 imu Test example
00003  *  revision 1.1.5, 2020/02/11
00004  *  Copyright (c) 2017 nepes inc.
00005  *
00006  *  Please use the NeuroShield library v1.1.4 or later
00007  ******************************************************************************/
00008 
00009 #include "mbed.h"
00010 #include <NeuroShield.h>
00011 #include <NeuroShieldSPI.h>
00012 #include <mpu6050.h>
00013 
00014 // for NM500
00015 #define MOTION_REPEAT_COUNT 3  // number of samples to assemble a vector
00016 #define MOTION_SIGNAL_COUNT 8  // d_ax, d_ay, d_az, d_gx, d_gy, d_gz, da, dg
00017 #define MOTION_CAPTURE_COUNT 20
00018 
00019 #define DEFAULT_MAXIF 500
00020 
00021 NeuroShield hnn;
00022 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!!!
00023 Serial pc(USBTX, USBRX);
00024 
00025 DigitalOut sdcard_ss(D6);       // SDCARD_SSn
00026 DigitalOut arduino_con(D5);     // SPI_SEL
00027 
00028 int16_t ax, ay, az, gx, gy, gz;
00029 
00030 uint8_t learn_cat = 0;     // category to learn
00031 uint8_t prev_cat = 0;  // previously recognized category
00032 uint16_t dist, cat, nid, nsr, ncount;  // response from the neurons
00033 uint16_t prev_ncount = 0;
00034 uint16_t fpga_version;
00035 
00036 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
00037 
00038 uint8_t vector[MOTION_REPEAT_COUNT * MOTION_SIGNAL_COUNT];       // vector holding the pattern to learn or recognize
00039 
00040 void mpu6050Calibration()
00041 {
00042     int i, j;
00043     long sum_ax = 0, sum_ay = 0, sum_az = 0, sum_gx = 0, sum_gy = 0, sum_gz = 0;
00044     int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz;
00045     int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;
00046     
00047     for (i = 0; i < 100; i++) {
00048         mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00049     }
00050     
00051     for (j = 0; j < 5; j++) {
00052         for (i = 0; i < 100; i++) {
00053             mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00054             sum_ax += ax;
00055             sum_ay += ay;
00056             sum_az += az;
00057             sum_gx += gx;
00058             sum_gy += gy;
00059             sum_gz += gz;
00060         }
00061         
00062         mean_ax = sum_ax / 100;
00063         mean_ay = sum_ay / 100;
00064         mean_az = sum_az / 100;
00065         mean_gx = sum_gx / 100;
00066         mean_gy = sum_gy / 100;
00067         mean_gz = sum_gz / 100;
00068         
00069         // MPU6050_GYRO_FS_1000 : offset = (-1) * mean_g
00070         // MPU6050_ACCEL_FS_8   : offset = (-0.5) * mean_a
00071         ax_offset = (-mean_ax) / 2;
00072         ay_offset = (-mean_ay) / 2;
00073         az_offset = (-mean_az) / 2;
00074         gx_offset = -mean_gx;
00075         gy_offset = -mean_gy;
00076         gz_offset = -mean_gz;
00077         
00078         // set
00079         mpu.setXAccelOffset(ax_offset);
00080         mpu.setYAccelOffset(ay_offset);
00081         mpu.setZAccelOffset(az_offset);
00082         mpu.setXGyroOffset(gx_offset);
00083         mpu.setYGyroOffset(gy_offset);
00084         mpu.setZGyroOffset(gz_offset);
00085     }
00086 }
00087 
00088 void extractFeatureVector()
00089 {
00090     int i;
00091     int16_t min_ax, min_ay, min_az, max_ax, max_ay, max_az;
00092     int16_t min_gx, min_gy, min_gz, max_gx, max_gy, max_gz;
00093     uint32_t norm_ax, norm_ay, norm_az, norm_gx, norm_gy, norm_gz;
00094     int32_t d_ax, d_ay, d_az, d_gx, d_gy, d_gz;
00095     int32_t da_local, dg_local;
00096 
00097     mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00098 
00099     max_ax = min_ax = ax;
00100     max_ay = min_ay = ay;
00101     max_az = min_az = az;
00102     max_gx = min_gx = gx;
00103     max_gy = min_gy = gy;
00104     max_gz = min_gz = gz;
00105   
00106     for (i = 0; i < MOTION_CAPTURE_COUNT; i++) {
00107         mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00108     
00109         if (ax < min_ax)
00110             min_ax = ax;
00111         else if (ax > max_ax)
00112             max_ax = ax;
00113 
00114         if (ay < min_ay)
00115             min_ay = ay;
00116         else if(ay > max_ay)
00117             max_ay = ay;
00118 
00119         if (az < min_az)
00120             min_az = az;
00121         else if (az > max_az)
00122             max_az = az;
00123 
00124         if (gx < min_gx)
00125             min_gx = gx;
00126         else if (gx > max_gx)
00127             max_gx = gx;
00128 
00129         if (gy < min_gy)
00130             min_gy = gy;
00131         else if (gy > max_gy)
00132             max_gy = gy;
00133 
00134         if (gz < min_gz)
00135             min_gz = gz;
00136         else if (gz > max_gz)
00137             max_gz = gz;
00138     }
00139 
00140     d_ax = max_ax - min_ax;
00141     d_ay = max_ay - min_ay;
00142     d_az = max_az - min_az;
00143 
00144     d_gx = max_gx - min_gx;
00145     d_gy = max_gy - min_gy;
00146     d_gz = max_gz - min_gz;
00147 
00148     da_local = d_ax;
00149     if (d_ay > da_local)
00150         da_local = d_ay;
00151     if (d_az > da_local)
00152         da_local = d_az;
00153 
00154     dg_local = d_gx;
00155     if (d_gy > dg_local)
00156         dg_local = d_gy;
00157     if (d_gz > dg_local)
00158         dg_local = d_gz;
00159 
00160     norm_ax = d_ax; norm_ax = norm_ax * 255 / da_local;
00161     norm_ay = d_ay; norm_ay = norm_ay * 255 / da_local;
00162     norm_az = d_az; norm_az = norm_az * 255 / da_local;
00163 
00164     norm_gx = d_gx; norm_gx = norm_gx * 255 / dg_local;
00165     norm_gy = d_gy; norm_gy = norm_gy * 255 / dg_local;
00166     norm_gz = d_gz; norm_gz = norm_gz * 255 / dg_local;
00167 
00168     for (i = 0; i < MOTION_REPEAT_COUNT; i++) {
00169         vector[i * MOTION_SIGNAL_COUNT] = norm_ax & 0x00ff;
00170         vector[(i * MOTION_SIGNAL_COUNT) + 1] = norm_ay & 0x00ff;
00171         vector[(i * MOTION_SIGNAL_COUNT) + 2] = norm_az & 0x00ff;
00172         vector[(i * MOTION_SIGNAL_COUNT) + 3] = norm_gx & 0x00ff;
00173         vector[(i * MOTION_SIGNAL_COUNT) + 4] = norm_gy & 0x00ff;
00174         vector[(i * MOTION_SIGNAL_COUNT) + 5] = norm_gz & 0x00ff;
00175         if (da_local >= 4096)
00176             vector[(i * MOTION_SIGNAL_COUNT) + 6] = 0xff;
00177         else
00178             vector[(i * MOTION_SIGNAL_COUNT) + 6] = ((da_local >> 4) & 0x00ff);
00179         if (dg_local >= 4096)
00180             vector[(i * MOTION_SIGNAL_COUNT) + 7] = 0xff;
00181         else
00182             vector[(i * MOTION_SIGNAL_COUNT) + 7] = ((dg_local >> 4) & 0x00ff);
00183     }
00184 }
00185 
00186 int main()
00187 {
00188     int input_key[2];
00189     
00190     arduino_con = LOW;
00191     sdcard_ss = HIGH;
00192     wait(0.5);
00193     
00194     if (hnn.begin() != 0) {
00195         fpga_version = hnn.fpgaVersion();
00196         if ((fpga_version & 0xFF00) == 0x0000) {
00197             printf("\n\n#### NeuroShield Board (Board v%d.0 / FPGA v%d.0) ####\n", ((fpga_version >> 4) & 0x000F), (fpga_version & 0x000F));
00198         }
00199         else if ((fpga_version & 0xFF00) == 0x0100) {
00200             printf("\n\n#### Prodigy Board (Board v%d.0 / FPGA v%d.0) ####\n", ((fpga_version >> 4) & 0x000F), (fpga_version & 0x000F));
00201         }
00202         else {
00203             printf("\n\n#### Unknown Board (Board v%d.0 / FPGA v%d.0) ####\n", ((fpga_version >> 4) & 0x000F), (fpga_version & 0x000F));
00204         }
00205         printf("\nStart NM500 initialzation...\n");
00206         printf("  NM500 is initialized!\n");
00207         printf("  There are %d neurons\n", hnn.total_neurons);
00208     }
00209     else {
00210         printf("\n\nStart NM500 initialzation...\n");
00211         printf("  NM500 is not connected properly!!\n");
00212         printf("  Please check the connection and reboot!\n");
00213         while (1);
00214     }
00215     
00216     // initialize mpu6050
00217     printf("\nStart MPU-6050 initialization...\n");
00218     mpu.initialize();
00219     // set gyro & accel range
00220     mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
00221     mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
00222     
00223     // verify connection
00224     for (int i = 0; i < 10; i++) {
00225         if (mpu.testConnection()) {
00226             printf("  MPU-6050 is connected successfully\n");
00227             break;
00228         }
00229         else if (i == 9) {
00230             printf("  MPU-6050 connection failed\n");
00231             printf("  Please check the connection and reboot!\n");
00232             while (1);
00233         }
00234         wait(0.1);
00235     }
00236     
00237     // wait for ready
00238     printf("  Trying to calibrate. Make sure the board is stable and upright\n");
00239     // reset offsets
00240     mpu.setXAccelOffset(0);
00241     mpu.setYAccelOffset(0);
00242     mpu.setZAccelOffset(0);
00243     mpu.setXGyroOffset(0);
00244     mpu.setYGyroOffset(0);
00245     mpu.setZGyroOffset(0);
00246     mpu6050Calibration();
00247     // end message
00248     printf("  MPU-6050 calibration is complete!!\n\n");
00249     
00250     printf("Move the board horizontally or vertically...\n");
00251     printf("Type '1' and enter, to learn up <-> down motion\n");
00252     printf("Type '2' and enter, to learn left <-> right motion\n");
00253     printf("Type '0' and enter, to learn by category 0\n");
00254     
00255     // main loop
00256     while (1) {
00257         if (pc.readable()) {
00258             input_key[0] = input_key[1];
00259             input_key[1] = pc.getc();
00260             if (input_key[1] == 0x0D) {     // enter key
00261                 learn_cat = input_key[0] - '0';
00262                 if (learn_cat < 3) {
00263                     printf("Learning motion category %d\n", learn_cat);
00264                     for (int i = 0; i < 5; i++) {
00265                         extractFeatureVector();
00266                         ncount = hnn.learn(vector, MOTION_REPEAT_COUNT * MOTION_SIGNAL_COUNT, learn_cat);
00267                         if (ncount != prev_ncount) {
00268                             prev_cat = learn_cat;
00269                             prev_ncount = ncount;
00270                         }
00271                     }
00272                     printf("Neurons=%d\n", ncount);
00273                 }
00274             }
00275         }
00276         else {                          // recognize
00277             extractFeatureVector();
00278             hnn.classify(vector, MOTION_REPEAT_COUNT * MOTION_SIGNAL_COUNT, &dist, &cat, &nid);
00279             if (cat != 0xFFFF) {
00280                 prev_cat = cat;
00281                 if (cat & 0x8000)
00282                     printf("Motion #%d (degenerated)\n", (cat & 0x7FFF));
00283                 else
00284                     printf("Motion #%d \n", cat);
00285             }
00286             else if (prev_cat != 0xFFFF) {
00287                 prev_cat = cat;
00288             }
00289         }
00290     }
00291 }