Diff: main.cpp

Revision:

5:79b8cd191fa8

Parent:

4:2de56fc46abb

Child:

6:4ea2ba88338f

--- a/main.cpp	Thu Nov 30 14:11:03 2017 +0000
+++ b/main.cpp	Fri Dec 01 10:30:27 2017 +0000
@@ -13,6 +13,9 @@
 #define LED_ON                  0
 #define LED_OFF                 1
 
+#define OFFSET                  10000 // OFFSET & RANGE to map inputs between 0 and 1
+#define RANGE                   20000 // RANGE = 2*OFFSET
+
 #define CONSOLE                 0 // print all in console
 #define FLASH_MVT               1 // save in flash only mvts
 #define FLASH_ALL               2 // save in flash mvtSets
@@ -24,8 +27,8 @@
 #define SECTOR_SIZE             1024   // Numbers of bits by memory sector
 #define RESERVED_SECTOR         32     // 32K reserved for Application Code
  
-#define ACQ_TIMER_PERIOD        0.01   // Time between 2 acquisitions (here 10 mSec)
-#define N_PTS                   50     // Number of points for each axis used to detect mvt
+#define ACQ_TIMER_PERIOD        0.005  // Time between 2 acquisitions (here 5 mSec)
+#define N_PTS                   100    // Number of points for each axis used to detect mvt
 #define N_MVTS                  5      // Number of mvts detected
 #define THRESHOLD_MVT           0.5    // threshold to validate a mvt
 #define THRESHOLD_SHOCK         0.5    // threshold to detect shock
@@ -55,7 +58,8 @@
     float Vout_IF, Vout_FILT, Vout_GAIN;
 };
 
-enum Mvt { Undefined = 0, Serve, ClearOverhead, DropOverhead, SmashShot, ClearUnderarm, DropUnderarm };
+//enum Mvt { Undefined = 0, Serve, ClearOverhead, DropOverhead, SmashShot, ClearUnderarm, DropUnderarm };
+enum Mvt { Undefined = 0, Serve, ClearOverhead, SmashShot, ClearUnderarm, DropUnderarm };
 
 struct MvtSet {
     int16_t inputs [N_PTS*3];
@@ -66,27 +70,31 @@
 volatile bool bTimer; // 1 means a Timer tick is done
 
 bool foundError = 0;
+bool flashFull = 0;
 int mode = CONSOLE;
 
 /* in flash :
     - [ 0x0 ; flash_base_address [ : code
-    - flash_base_address : flash_next_address (int)
-    - flash_base_address+4 : mode (int)
-    - flash_base_address+8 : first address to store data */
+    - [ flash_base_address ; flash_base_address_cmd [ : data
+    - flash_base_address_cmd : flash_next_address (int)
+    - flash_base_address_cmd+4 : mode (int)
+    - flash_base_address_cmd+8 : flashFull (int) */
 uint32_t KL25_Flash_Size;
 int flash_base_address = RESERVED_SECTOR * SECTOR_SIZE ; // Store Flash Base Address
 int flash_next_address; // next address for saving data in flash
 int flash_base_address_cmd; // base address where the parameters are saved
-
+/*
 const float Weights[3*N_PTS*N_MVTS] = {
     #include "Weights.txt"
 };
 const float Biases[N_MVTS] = {
     #include "Biases.txt"
 };
-
+*/
 // ------------------------ Function Declaration ------------------------
 void Init(void);
+void DisplayFlashInfos(void);   // display memory use
+void DisplayInstructions(void); // display available cmds
 void Clear_Led(void);           // switch off led's
 bool Check_Jumper(void);        // if J1_15 & J1_16 connected together -> return 1
 void Check_Console(void);       // detect input from user in console
@@ -97,12 +105,13 @@
 void UpdateParamFlash(void);    // update next_address_flash and mode in flash
 void WriteFlash(MvtSet mvtSet); // write only mvt or all set depending on 'all'
 void ReadFlash(void);           // print memory content in console
+void LineHandler(int16_t *line, int16_t next); // handle display of data set (used in ReadFlash)
 
 Data ReadData(void);            // read data from accelerometer and piezo
 void Log(void);                 // read data, detect shock and movement
 void Rotate(int16_t *AccDataLog, int amount, int16_t *inputs); // inputs = AccDataLog rotated of amount
-void PrintSet(int16_t *inputs);  // display set of data
-float Sigmoid(float x);         // sigmoid function (used by neural network)
+void PrintSet(MvtSet mvtSet);   // display set of data
+void Softmax (float *inputs, float *result); // softmax function (used by neural network)
 Mvt SelectMvt(int16_t *inputs); // compute probabilities for each mvt based on the inputs
 
 // -------------------------------------------------------------------------------------------------------
@@ -117,7 +126,7 @@
     
     while(!foundError)
     {
-        if (Check_Jumper())
+        if (Check_Jumper() && !flashFull)
         {
             Clear_Led();
             Count = 5;
@@ -125,9 +134,9 @@
             {
                 if (Check_Jumper())
                 {
-                    Led_Blue = LED_ON; // Blink to alert user "Enter in Logging mode"
+                    Led_Green = LED_ON; // Blink to alert user "Enter in Logging mode"
                     wait_ms(750);
-                    Led_Blue = LED_OFF;
+                    Led_Green = LED_OFF;
                     wait_ms(250);
                     Count --;
                     if (Count == 0)
@@ -137,7 +146,10 @@
                     Count = 0;
             }
         }
-        Led_Blue = !Led_Blue;
+        if (flashFull)
+            Led_Red = !Led_Red;
+        else
+            Led_Blue = !Led_Blue;
         Check_Console();
         wait_ms(100);        
     }
@@ -157,39 +169,61 @@
     myTick_Acq.attach(&myTimer_Acq_Task, ACQ_TIMER_PERIOD); // Timer for acquisition
     
     Host_Comm.baud(115200);         // Baud rate setting
-    Host_Comm.printf("\n\rLELEC2811 - Badminton Logger - Group 5\n\r");
+    Host_Comm.printf("\n\r*****\n\rLELEC2811 - Badminton Logger - Group 5\n\r*****\n\n\r");
     
     KL25_Flash_Size = flash_size(); // Get Size of KL25 Embedded Flash
     flash_base_address_cmd = KL25_Flash_Size-SECTOR_SIZE;
     
+    /* TO CHECK SIZE OF PROGRAM IN FLASH :
+    int *ptr;
+    int n_sectors = KL25_Flash_Size/SECTOR_SIZE;
+    Host_Comm.printf("Number of sectors : %d\n\r",n_sectors);
+    for (int i = 0; i < n_sectors; i++){
+        ptr = (int*) (i*SECTOR_SIZE);
+        Host_Comm.printf("Sector %d : %d\n\r",i, ptr[0]);
+    }
+    */
+    
     int *base_address_ptr = (int*)flash_base_address_cmd;
     flash_next_address = base_address_ptr[0];
     if (flash_next_address >= flash_base_address_cmd || flash_next_address < flash_base_address)
     {
-        Host_Comm.printf("First run (or error with previous flash_next_address)\n\r");
+        Host_Comm.printf("First run (or error with previous flash_next_address).\n\r");
         EraseAllSectors();
         flash_next_address = flash_base_address;
         mode = CONSOLE;
         UpdateParamFlash();
     }
     else {
-        // add 0xFFFF in flash to tag reset
         mode = base_address_ptr[1];
+        flashFull = base_address_ptr[2];
         if (mode != CONSOLE && mode != FLASH_MVT && mode != FLASH_ALL) {
             mode = CONSOLE;
             UpdateParamFlash();
         }
     }
-    
-    Host_Comm.printf("flash_next_address = %d ; mode = %d\n\r",flash_next_address, mode);
-    Host_Comm.printf("Memory used = %f %%\n\r",(flash_next_address-flash_base_address)/(flash_base_address_cmd-flash_base_address));
+    DisplayFlashInfos();
     
     Host_Comm.printf("Initialization done.\n\n\r");
-    
+    DisplayInstructions();
+}
+
+// -------------------------- DisplayFlashInfos -------------------------
+void DisplayFlashInfos()
+{
+    Host_Comm.printf("flash_next_address = %d\n\r",flash_next_address);
+    Host_Comm.printf("mode = %d\n\r",mode);
+    Host_Comm.printf("Memory used = %f %%\n\r",((float)flash_next_address-flash_base_address)/((float)flash_base_address_cmd-flash_base_address)*100);
+}
+
+// ------------------------- DisplayInstructions ------------------------
+void DisplayInstructions()
+{
     Host_Comm.printf("When the jumper is removed, use the keyboard :\n\r");
-    Host_Comm.printf("- to erase flash : 'e' = erase flash\n\r");
-    Host_Comm.printf("- to change mode : 'c' = console mode ; 'm' = write_mvt mode ; 'a' = write_all mode\n\r");
-    Host_Comm.printf("- to read flash : 'r' = read flash ; 's' = stop reading\n\r");
+    Host_Comm.printf("- to erase flash : 'E' = erase flash\n\r");
+    Host_Comm.printf("- to read flash : 'R' = read flash ; 'S' = stop reading\n\r");
+    Host_Comm.printf("- to change mode : 'C' = console mode ; 'M' = write_mvt mode ; 'A' = write_all mode\n\r");
+    Host_Comm.printf("!!! If mode change, ReadData() will fail => also press 'E' !!!\n\n\r");
 }
 
 // ----------------------------- Clear_Led ------------------------------
@@ -225,14 +259,23 @@
     if(Host_Comm.readable()) 
     {
         char cmd = Host_Comm.getc();
-        if ((cmd == 'E') || (cmd == 'e'))
+        if ((cmd == 'E') || (cmd == 'e')) {
             EraseAllSectors();
-        else if ((cmd == 'C') || (cmd == 'c'))
+            flash_next_address = flash_base_address;
+            Host_Comm.printf("Erase done.\n\r");
+        }
+        else if ((cmd == 'C') || (cmd == 'c')) {
             mode = CONSOLE;
-        else if ((cmd == 'M') || (cmd == 'm'))
+            Host_Comm.printf("Mode console (0) actived.\n\r");
+        }
+        else if ((cmd == 'M') || (cmd == 'm')) {
             mode = FLASH_MVT;
-        else if ((cmd == 'A') || (cmd == 'a'))
+            Host_Comm.printf("Mode flash_mvt (1) actived.\n\r");
+        }
+        else if ((cmd == 'A') || (cmd == 'a')) {
             mode = FLASH_ALL;
+            Host_Comm.printf("Mode flash_all (2) actived.\n\r");
+        }
         else if ((cmd == 'R') || (cmd == 'r'))
             ReadFlash();
         
@@ -255,6 +298,7 @@
         if(foundError)
             return;
     }
+    flashFull = 0;
 }
 
 // ---------------------------- EraseSector -----------------------------
@@ -274,8 +318,8 @@
     if(foundError)
         return;
     
-    int toWrite[2] = {flash_next_address,mode};
-    IAPCode status = program_flash(flash_base_address_cmd, (char *) &toWrite, 2*sizeof(int));
+    int toWrite[3] = {flash_next_address, mode, flashFull};
+    IAPCode status = program_flash(flash_base_address_cmd, (char *) &toWrite, 12);
     if (status != Success) {
         Host_Comm.printf("\n\rError in UpdateParamFlash() : status = %d\n\r", status);
         foundError = 1;
@@ -286,52 +330,51 @@
 void WriteFlash(MvtSet mvtSet)
 {
     IAPCode status;
+    int toWrite;
     
     if (mode == FLASH_ALL) // inputs (2*3*N_PTS bytes) + mvt (1 byte)
     {
+        // check if enough place
+        if (flash_next_address+(2*(3*N_PTS)+4) > flash_base_address_cmd) {
+            Host_Comm.printf("\n\rFlash is full.\n\r");
+            flashFull = 1; return;
+        }
+        
         // add all bytes one behind the other : 2bytes*3*N_PTS (inputs) + 1byte (mvt)
         int remainder = (3*N_PTS) % 2; // modulo 2 because compacting 2bytes into 4bytes words
-        
-        status = program_flash(flash_next_address, (char*) &(mvtSet.inputs), 2*(3*N_PTS-remainder));
-        if (status != Success) {
-            Host_Comm.printf("\n\rError in WriteFlash() (0) : status = %d\n\r", status);
-            foundError = 1; return;
-        }
-        flash_next_address += 2*(3*N_PTS-remainder);
+        int even = 3*N_PTS-remainder;
         
-        int16_t toWrite[2] = {0,(int16_t) mvtSet.mvt};
-        if(remainder)
-            toWrite[0] = mvtSet.inputs[3*N_PTS-1];
+        for (int i = 0; i < even; i+=2)
+        {
+            toWrite = (mvtSet.inputs[i] << 16) | (mvtSet.inputs[i+1] & 0x0000FFFF);
+            status = program_flash(flash_next_address, (char*) &toWrite, 4);
+            if (status != Success) {
+                Host_Comm.printf("\n\rError in WriteFlash() (0) : status = %d\n\r", status);
+                foundError = 1; return;
+            }
+            flash_next_address += 4;
+        }
+        
+        toWrite = mvtSet.mvt;
+        if(remainder == 1)
+            toWrite = toWrite | (mvtSet.inputs[3*N_PTS-1] << 16);
         status = program_flash(flash_next_address, (char*) &toWrite, 4);
-        if (status != Success) {
-            Host_Comm.printf("\n\rError in WriteFlash() (1) : status = %d\n\r", status);
-            foundError = 1; return;
-        }
     }
-    else // 4 mvts (4 bytes) compacted in one 32bits slot
+    else
     {
-        /*
-        int innerPosition = flash_next_address % 4;
-        int local_base_address = flash_next_address-innerPosition;
-        int *address_ptr = (int*)(local_base_address);
-        
-        int currValue = address_ptr[0];
-        uint8_t toWrite[4] = {(uint8_t)(currValue>>24),(uint8_t)(currValue>>16),(uint8_t)(currValue>>8),(uint8_t)currValue};
-        toWrite[innerPosition] = (uint8_t)mvtSet.mvt;
+        // check if enough place
+        if (flash_next_address+4 > flash_base_address_cmd) {
+            Host_Comm.printf("\n\rFlash is full.\n\r");
+            flashFull = 1; return;
+        }
         
-        status = program_flash(local_base_address, (char*) &toWrite, 4);
-        if (status != Success) {
-            Host_Comm.printf("\n\rError in WriteFlash() (2) : status = %d\n\r", status);
-            foundError = 1; return;
-        }
-        flash_next_address++;
-        */
-        uint8_t toWrite[4] = {0,0,0,mvtSet.mvt};
+        toWrite = mvtSet.mvt;
         status = program_flash(flash_next_address, (char*) &toWrite, 4);
-        if (status != Success) {
-            Host_Comm.printf("\n\rError in WriteFlash() (2) : status = %d\n\r", status);
-            foundError = 1; return;
-        }
+    }
+    
+    if (status != Success) {
+        Host_Comm.printf("\n\rError in WriteFlash() (1) : status = %d\n\r", status);
+        foundError = 1; return;
     }
     
     flash_next_address += 4;
@@ -342,35 +385,72 @@
 void ReadFlash()
 {
     Host_Comm.printf("\n\r------ Begin Read Flash ------\n\r");
-    Host_Comm.printf("flash_next_address = %d ; mode = %d\n\r",flash_next_address, mode);
-    Host_Comm.printf("Memory used = %f %%\n\r",(flash_next_address-flash_base_address)/(flash_base_address_cmd-flash_base_address));
+    DisplayFlashInfos();
     
     char cmd;
-    int *address_ptr;
+    int *currAddress_ptr = (int*)flash_base_address;
+    int *stopAddress_ptr = (int*)flash_next_address;
     
-    int remainder = (flash_next_address) % 4;
-    int local_base_address = flash_next_address-remainder;
+    Host_Comm.printf("ReadFlash : size = %d\n\r",stopAddress_ptr-currAddress_ptr);
     
-    for (int address = flash_base_address+8; address < local_base_address; address+=4)
+    if (mode == FLASH_ALL)
     {
-        if(Host_Comm.readable()) 
+        int remainder = (3*N_PTS) % 2;
+        int n_words_by_set = (3*N_PTS - remainder)/2+1;
+        int word;
+        int16_t line [4] = {}; // 3 components of acc (line[3] = position in line)
+        
+        while (currAddress_ptr < stopAddress_ptr)
         {
-            cmd = Host_Comm.getc();
-            if ((cmd == 'S') || (cmd == 's'))
-                return;
+            // check for user input
+            if(Host_Comm.readable()) {
+                cmd = Host_Comm.getc();
+                if ((cmd == 'S') || (cmd == 's'))
+                    return;
+            }
+            
+            // print all set
+            for (word = 0; word < n_words_by_set-1; word++) {
+                LineHandler(line,(int16_t)(currAddress_ptr[word]>>16));
+                LineHandler(line,(int16_t)currAddress_ptr[word]);
+            }
+            if (remainder == 1)
+                LineHandler(line,(int16_t)(currAddress_ptr[word]>>16));
+            Host_Comm.printf("Mvt = %d\n\r",(int16_t)currAddress_ptr[word]);
+            currAddress_ptr += n_words_by_set;
         }
-        
-        address_ptr = (int*)address;
-        Host_Comm.printf("%d %d %d %d\n\r",(uint8_t)(address_ptr[0]>>24),(uint8_t)(address_ptr[0]>>16),(uint8_t)(address_ptr[0]>>8),(uint8_t)address_ptr[0]);
+    }
+    else
+    {
+        while (currAddress_ptr < stopAddress_ptr)
+        {
+            // check for user input
+            if(Host_Comm.readable()) {
+                cmd = Host_Comm.getc();
+                if ((cmd == 'S') || (cmd == 's'))
+                    return;
+            }
+            
+            // read mvt
+            Host_Comm.printf("Mvt = %d\n\r",currAddress_ptr[0]);
+            currAddress_ptr ++;
+        }
     }
     
-    address_ptr = (int*)local_base_address;
-    for (int i = 0; i < remainder; i++)
-        Host_Comm.printf("%d ",(uint8_t)(address_ptr[0]>>(8*(3-i))));
-    
     Host_Comm.printf("\n\r------- End Read Flash -------\n\n\r");
 }
 
+// ----------------------------- LineHandler ----------------------------
+void LineHandler(int16_t *line, int16_t next)
+{
+    line[line[3]] = next;
+    line[3] ++;
+    if (line[3] == 3) {
+        Host_Comm.printf("%d %d %d\n\r",line[0],line[1],line[2]);
+        line[3] = 0;
+    }
+}
+
 // -------------------------------------------------------------------------------------------------------
 // -------------------------------------------------------------------------------------------------------
 
@@ -402,7 +482,7 @@
 
 // -------------------------------- Log ---------------------------------
 void Log()
-{
+{    
     Data currData;
     int16_t AccDataLog [N_PTS*3] = {}; // array to save latest data read
     int index_write = 0;               // current position to write data in AccDataLog
@@ -410,10 +490,8 @@
     bool shockDetected = 0;            // if shock detected
     int n_sinceShock = 0;              // number of ReadData() since last chock
     
-    while(Check_Jumper() && !foundError)
-    {
-        Led_Green != Led_Green; // LED blinks green while logging
-        
+    while(Check_Jumper() && !foundError && !flashFull)
+    {        
         while (bTimer == 0) {} // Wait Acq Tick Timer
         bTimer = 0;
         
@@ -432,14 +510,20 @@
         }
         if (n_sinceShock == N_PTS/2 && shockDetected == 1)
         {
+            Led_Green = LED_ON;
+            
             MvtSet mvtSet;
             Rotate(AccDataLog, N_PTS-1-index_write, mvtSet.inputs);
             mvtSet.mvt = SelectMvt(mvtSet.inputs);
+            
             if (mode == CONSOLE)
-                PrintSet(mvtSet.inputs);
+                PrintSet(mvtSet);
             else
                 WriteFlash(mvtSet);
+            
             shockDetected = 0;
+            wait_ms(100);
+            Led_Green = LED_OFF;
         }
         
         index_write ++;
@@ -462,34 +546,53 @@
 }
 
 // ------------------------------ PrintSet ------------------------------
-void PrintSet(int16_t *inputs)
+void PrintSet(MvtSet mvtSet)
 {
     Host_Comm.printf("------ Begin Set ------\n\r");
     for(int i = 0; i < N_PTS; i++)
-        Host_Comm.printf("%d %d %d\n\r", inputs[i*3],inputs[i*3+1],inputs[i*3+2]);
+        Host_Comm.printf("%d %d %d\n\r",mvtSet.inputs[i*3],mvtSet.inputs[i*3+1],mvtSet.inputs[i*3+2]);
+    Host_Comm.printf("Mvt = %d\n\r",mvtSet.mvt);
     Host_Comm.printf("------- End Set -------\n\n\r");
 }
 
-// ------------------------------ Sigmoid -------------------------------
-float Sigmoid(float x) { return 1/(1+exp(-x)); }
+// ------------------------------ Softmax -------------------------------
+void Softmax (float *inputs, float *result)
+{
+    float exps [N_MVTS];
+    float sum = 0;
+    for (int i = 0; i < N_MVTS; i++)
+    {
+        exps[i] = exp(inputs[i]);
+        sum += exps[i];
+    }
+    
+    for (int i = 0; i < N_MVTS; i++)
+        result[i] = exps[i] / sum;
+}
 
 // ----------------------------- SelectMvt ------------------------------
 Mvt SelectMvt(int16_t *inputs)
 {
+    /*
     int i, j;
     float selection [N_MVTS] = {};
     for (j = 0; j < N_MVTS; j++) {
         for (i = 0; i < N_PTS*3; i++)
-            selection[j] += (float)inputs[i] * Weights[i*N_MVTS+j];
-        selection[j] = Sigmoid(selection[j] + Biases[j]);
+            selection[j] += (inputs[i]+OFFSET)/RANGE * Weights[i*N_MVTS+j];
+        selection[j] = selection[j]+Biases[j];
     }
+    Softmax(selection,selection);
     
     Mvt mvt = Undefined;
+    Host_Comm.printf("Proba mvt : ");
     for (i = 0; i < N_MVTS; i++) {
         if (selection[i] > THRESHOLD_MVT)
             mvt = static_cast<Mvt>(i+1);
-        Host_Comm.printf("Proba mvt %d : %f\n\r",i+1,selection[i]);
+        Host_Comm.printf("%f ",selection[i]);
     }
+    Host_Comm.printf("\n\r");
     
     return mvt;
+    */
+    return SmashShot;
 }