Diff: main.cpp

Revision:

4:2de56fc46abb

Parent:

3:d03eae745223

Child:

5:79b8cd191fa8

--- a/main.cpp	Mon Nov 27 22:37:15 2017 +0000
+++ b/main.cpp	Thu Nov 30 14:11:03 2017 +0000
@@ -1,102 +1,121 @@
-/* LELEC 2811 - BadmintonLogger - Group G */
+/*
+ *  LELEC 2811 - BadmintonLogger - Group 5
+ */
+
 #include "mbed.h"
 #include "FreescaleIAP.h"   // Library for Flash Access
 #include "MMA8491Q_PG.h"    // Accelerometer
+#include <cmath>
 
 #define MMA8491_I2C_ADDRESS (0x55<<1)
-
-#define DISABLE_STATE           0
-#define ENABLE_STATE            1
+#define KL25Z_VDD 2.89      // Value of VDD : To be measured on board KL25Z pin P3V3 (calibration)
 
 #define LED_ON                  0
 #define LED_OFF                 1
 
+#define CONSOLE                 0 // print all in console
+#define FLASH_MVT               1 // save in flash only mvts
+#define FLASH_ALL               2 // save in flash mvtSets
+
 #define REG_OUT_X_MSB           0x01
 #define REG_OUT_Y_MSB           0x03
 #define REG_OUT_Z_MSB           0x05
-
-#define FLASH_NO_ACQ_DONE       0
-#define FLASH_ACQ_DONE          1
-#define ERASE_FLASH_ERROR       -1
-#define WRITE_FLASH_ERROR       -2
  
-#define SECTOR_SIZE             1024
-#define RESERVED_SECTOR         32
+#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 N_MVTS                  6      // Number of mvts detected
-#define THRESHOLD_MVT           0.6    // threshold to validate a 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
 
 MMA8491Q my8491(PTE0, PTE1, MMA8491_I2C_ADDRESS); // Setup I2C for MMA8491
 
 Serial Host_Comm(USBTX, USBRX); // Set Serial Port
 
-AnalogIn myPTE20(PTE20);
-AnalogIn myPTE21(PTE21);
-AnalogIn myPTE22(PTE22);
+AnalogIn myPTE20(PTE20);        // read Vout_IF
+AnalogIn myPTE21(PTE21);        // read Vout_FILT
+AnalogIn myPTE22(PTE22);        // read Vout_GAIN
 
 Ticker myTick_Acq;              // Periodical timer for Acquisition
- 
+
 DigitalOut Led_Red(LED1);       // Define I/O for LEDs
 DigitalOut Led_Green(LED2);
 DigitalOut Led_Blue(LED3);
- 
+
 DigitalOut Accel_Enable(PTA13);
 
-DigitalOut Start_Pulse_Out(PTC9);   // Used to enter/exit Acquisition mode 
-DigitalIn  Start_Pulse_In(PTC11);   // ShortPin J1_15 and J1_16 to enter in Acq_Mode
+DigitalOut Start_Pulse_Out(PTE4);  // Used to enter/exit Acquisition mode 
+DigitalIn  Start_Pulse_In(PTE5);   // ShortPin J1_15 and J1_16 to enter in Acq_Mode
 
 // --------------------- Structure and Enumeration ---------------------
 struct Data {
     int16_t accX, accY, accZ;
-    unsigned short Vout_IF;   // 2 bytes : Analog_PTE20
-    unsigned short Vout_FILT; // 2 bytes : Analog_PTE21
-    unsigned short Vout_GAIN; // 2 bytes : Analog_PTE22
+    float Vout_IF, Vout_FILT, Vout_GAIN;
 };
 
-struct NeuralNetwork {
-    float* Weights;
-    float* Biases;
+enum Mvt { Undefined = 0, Serve, ClearOverhead, DropOverhead, SmashShot, ClearUnderarm, DropUnderarm };
+
+struct MvtSet {
+    int16_t inputs [N_PTS*3];
+    Mvt mvt;
 };
 
-enum Mvts { Undefined = 0, Serve, ClearOverhead, DropOverhead, SmashShot, ClearUnderarm, DropUnderarm };
-
 // -------------------------- Globale variable --------------------------
 volatile bool bTimer; // 1 means a Timer tick is done
 
-int Flash_Base_Address = RESERVED_SECTOR * SECTOR_SIZE ; // Store Flash Base Adresse with 32K reserved for Application Code
-int n_sector;
+bool foundError = 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 */
 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 Clear_Led(void);
-bool Check_Jumper(void);     // if J1_15 & J1_16 connected together -> return 1
-void myTimer_Acq_Task(void); // called by the timer
-Data ReadData(void);
+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
+void myTimer_Acq_Task(void);    // called by the timer
 
-void Log(void);
-void PrintSet(Data *data, int n);
-void Print(Data data);
-NeuralNetwork *InitNeuralNetwork(void);    // Initialize the neural network
-void FreeNeuralNetwork(NeuralNetwork *nn); // Free the allocated memory
-float Sigmoid(float x);                    // sigmoid function (used by neural network)
-Mvts SelectMvt(NeuralNetwork *nn, int16_t *AccDataLog, int index_write);
+void EraseAllSectors(void);     // erase all sectors containing data
+void EraseSector(int address);  // erase one sector
+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
 
-extern IAPCode verify_erased(int address, unsigned int length);
+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)
+Mvt SelectMvt(int16_t *inputs); // compute probabilities for each mvt based on the inputs
 
-// ----------------------------------------------------------------------
+// -------------------------------------------------------------------------------------------------------
+// -------------------------------------------------------------------------------------------------------
 
 // -------------------------------- main --------------------------------
 int main()
 {
     Init ();
     
-    uint8_t Count;
+    int Count;
     
-    while(true)
+    while(!foundError)
     {
         if (Check_Jumper())
         {
@@ -107,9 +126,9 @@
                 if (Check_Jumper())
                 {
                     Led_Blue = LED_ON; // Blink to alert user "Enter in Logging mode"
-                    wait_ms(500);
+                    wait_ms(750);
                     Led_Blue = LED_OFF;
-                    wait_ms(500);
+                    wait_ms(250);
                     Count --;
                     if (Count == 0)
                         Log();
@@ -118,39 +137,73 @@
                     Count = 0;
             }
         }
-        wait_ms(500);     
+        Led_Blue = !Led_Blue;
+        Check_Console();
+        wait_ms(100);        
     }
+    
+    Host_Comm.printf("\n\rProgram is exiting due to error...\n\r");
+    Clear_Led();
+    Led_Red = LED_ON;
 }
 
 // -------------------------------- Init --------------------------------
-void Init(void)
+void Init()
 {
     Start_Pulse_In.mode(PullNone);  // Input Pin is programmed as floating
-    Accel_Enable = DISABLE_STATE;   // Turn Accel Enable to disabled state
+    Accel_Enable = 0;               // Turn Accel Enable to disabled state
     Clear_Led();
     
-    KL25_Flash_Size = flash_size(); // Get Size of KL25 Embedded Flash
-    n_sector = (KL25_Flash_Size / SECTOR_SIZE) - RESERVED_SECTOR; // Reserve Max 32K for App Code
-    
     myTick_Acq.attach(&myTimer_Acq_Task, ACQ_TIMER_PERIOD); // Timer for acquisition
     
     Host_Comm.baud(115200);         // Baud rate setting
-    Host_Comm.printf("\nLELEC2811 - Badminton Logger - Group G\n");
-    Host_Comm.printf("Initialization done.\n");
+    Host_Comm.printf("\n\rLELEC2811 - Badminton Logger - Group 5\n\r");
+    
+    KL25_Flash_Size = flash_size(); // Get Size of KL25 Embedded Flash
+    flash_base_address_cmd = KL25_Flash_Size-SECTOR_SIZE;
+    
+    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");
+        EraseAllSectors();
+        flash_next_address = flash_base_address;
+        mode = CONSOLE;
+        UpdateParamFlash();
+    }
+    else {
+        // add 0xFFFF in flash to tag reset
+        mode = base_address_ptr[1];
+        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));
+    
+    Host_Comm.printf("Initialization done.\n\n\r");
+    
+    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");
 }
 
 // ----------------------------- Clear_Led ------------------------------
-void Clear_Led(void)
+void Clear_Led()
 {
     Led_Red = LED_OFF;
     Led_Green = LED_OFF;
-    Led_Blue = LED_OFF ; // Bug on board : Turning On Blue Led decrease consumption...
+    Led_Blue = LED_OFF ;
 }
 
 // ---------------------------- Check_Jumper ----------------------------
 bool Check_Jumper()
 {
-    uint8_t i;
+    int i;
     for (i = 0 ; i < 2 ; i ++)
     {
         Start_Pulse_Out = 1;
@@ -166,18 +219,170 @@
     return 1;
 }
 
+// --------------------------- Check_Console ----------------------------
+void Check_Console()
+{
+    if(Host_Comm.readable()) 
+    {
+        char cmd = Host_Comm.getc();
+        if ((cmd == 'E') || (cmd == 'e'))
+            EraseAllSectors();
+        else if ((cmd == 'C') || (cmd == 'c'))
+            mode = CONSOLE;
+        else if ((cmd == 'M') || (cmd == 'm'))
+            mode = FLASH_MVT;
+        else if ((cmd == 'A') || (cmd == 'a'))
+            mode = FLASH_ALL;
+        else if ((cmd == 'R') || (cmd == 'r'))
+            ReadFlash();
+        
+        UpdateParamFlash();
+    }
+}
+
 // -------------------------- myTimer_Acq_Task --------------------------
 void myTimer_Acq_Task() { bTimer = 1; }
 
+// -------------------------------------------------------------------------------------------------------
+// -------------------------------------------------------------------------------------------------------
+
+// -------------------------- EraseAllSectors ---------------------------
+void EraseAllSectors(void)
+{
+    for (int address = flash_base_address ; address < KL25_Flash_Size ; address += SECTOR_SIZE)
+    {
+        EraseSector(address);
+        if(foundError)
+            return;
+    }
+}
+
+// ---------------------------- EraseSector -----------------------------
+void EraseSector(int address)
+{
+    IAPCode status = erase_sector(address); 
+    if (status != Success) {
+        Host_Comm.printf("\n\rError in EraseSector() : status = %d\n\r", status);
+        foundError = 1;
+    }
+}
+
+// -------------------------- UpdateParamFlash --------------------------
+void UpdateParamFlash()
+{
+    EraseSector(flash_base_address_cmd);
+    if(foundError)
+        return;
+    
+    int toWrite[2] = {flash_next_address,mode};
+    IAPCode status = program_flash(flash_base_address_cmd, (char *) &toWrite, 2*sizeof(int));
+    if (status != Success) {
+        Host_Comm.printf("\n\rError in UpdateParamFlash() : status = %d\n\r", status);
+        foundError = 1;
+    }
+}
+
+// ----------------------------- WriteFlash -----------------------------
+void WriteFlash(MvtSet mvtSet)
+{
+    IAPCode status;
+    
+    if (mode == FLASH_ALL) // inputs (2*3*N_PTS bytes) + mvt (1 byte)
+    {
+        // 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);
+        
+        int16_t toWrite[2] = {0,(int16_t) mvtSet.mvt};
+        if(remainder)
+            toWrite[0] = mvtSet.inputs[3*N_PTS-1];
+        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
+    {
+        /*
+        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;
+        
+        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};
+        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;
+        }
+    }
+    
+    flash_next_address += 4;
+    UpdateParamFlash();
+}
+
+// ----------------------------- ReadFlash ------------------------------
+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));
+    
+    char cmd;
+    int *address_ptr;
+    
+    int remainder = (flash_next_address) % 4;
+    int local_base_address = flash_next_address-remainder;
+    
+    for (int address = flash_base_address+8; address < local_base_address; address+=4)
+    {
+        if(Host_Comm.readable()) 
+        {
+            cmd = Host_Comm.getc();
+            if ((cmd == 'S') || (cmd == 's'))
+                return;
+        }
+        
+        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]);
+    }
+    
+    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");
+}
+
+// -------------------------------------------------------------------------------------------------------
+// -------------------------------------------------------------------------------------------------------
+
 // ------------------------------ ReadData ------------------------------
 Data ReadData()
 {
     Data data;
     
     // Get Accelerometer data's
-    Accel_Enable = ENABLE_STATE; // Rising Edge -> Start measure
+    Accel_Enable = 1; // Rising Edge -> Start measure
     
-    uint8_t ready = 0;
+    int ready = 0;
     while((ready && 0x10) == 0) // Wait for accelerometer to have new data's
         ready = my8491.Read_Status();
 
@@ -185,12 +390,12 @@
     data.accY = my8491.getAccAxis(REG_OUT_Y_MSB);
     data.accZ = my8491.getAccAxis(REG_OUT_Z_MSB);
         
-    Accel_Enable = DISABLE_STATE;
+    Accel_Enable = 0;
     
     // Get Piezo Data's
-    data.Vout_IF = myPTE20.read_u16(); 
-    data.Vout_FILT = myPTE21.read_u16();  
-    data.Vout_GAIN = myPTE22.read_u16();
+    data.Vout_IF = ((float) myPTE20.read_u16() / 0XFFFF) * KL25Z_VDD; // convert in volt
+    data.Vout_FILT = ((float) myPTE21.read_u16() / 0XFFFF) * KL25Z_VDD;
+    data.Vout_GAIN = ((float) myPTE22.read_u16() / 0XFFFF) * KL25Z_VDD;
     
     return data;
 }
@@ -198,16 +403,14 @@
 // -------------------------------- Log ---------------------------------
 void Log()
 {
-    NeuralNetwork *nn = InitNeuralNetwork();
     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
-    bool shockDetected = 0; // if shock detected
-    int n_sinceShock = 0; // number of ReadData() since last chock
-    Mvts mvts [100] = {};  // stocks history of mvts
-    int index_mvt = 0;
+    int index_write = 0;               // current position to write data in AccDataLog
+    bool enoughData = 0;
+    bool shockDetected = 0;            // if shock detected
+    int n_sinceShock = 0;              // number of ReadData() since last chock
     
-    while(Check_Jumper())
+    while(Check_Jumper() && !foundError)
     {
         Led_Green != Led_Green; // LED blinks green while logging
         
@@ -215,84 +418,78 @@
         bTimer = 0;
         
         currData = ReadData();
-        // Host_Comm.printf("%d ; %d ; %d ; %d ; %d ; %d\n", data.accX, data.accY, data.accZ, data.Vout_IF, data.Vout_FILT, data.Vout_GAIN);
+        //Host_Comm.printf("%d ; %d ; %d ; %f\n\r", currData.accX, currData.accY, currData.accZ, currData.Vout_FILT);
         AccDataLog[index_write*3] = currData.accX;
         AccDataLog[index_write*3+1] = currData.accY;
         AccDataLog[index_write*3+2] = currData.accZ;
         
-        if (currData.Vout_FILT >= THRESHOLD_SHOCK)
+        float amplitude = abs(currData.Vout_FILT - KL25Z_VDD/2.0);
+        //Host_Comm.printf("amplitude = %f\n\r",amplitude);
+        if (amplitude >= THRESHOLD_SHOCK && enoughData)
         {
             shockDetected = 1;
             n_sinceShock = 0;
         }
         if (n_sinceShock == N_PTS/2 && shockDetected == 1)
         {
-            mvts[index_mvt] = SelectMvt(nn, AccDataLog, index_write);
-            index_mvt ++;
+            MvtSet mvtSet;
+            Rotate(AccDataLog, N_PTS-1-index_write, mvtSet.inputs);
+            mvtSet.mvt = SelectMvt(mvtSet.inputs);
+            if (mode == CONSOLE)
+                PrintSet(mvtSet.inputs);
+            else
+                WriteFlash(mvtSet);
             shockDetected = 0;
         }
-    
+        
         index_write ++;
         n_sinceShock ++;
-        if (index_write == N_PTS-1)
+        if (index_write == N_PTS)
+        {
+            enoughData = 1;
             index_write = 0;
+        }
     }
-    FreeNeuralNetwork(nn);
     Clear_Led();
 }
 
-// ------------------------- InitNeuralNetwork --------------------------
-NeuralNetwork *InitNeuralNetwork(void)
+// ------------------------------- Rotate -------------------------------
+void Rotate(int16_t *AccDataLog, int amount, int16_t *inputs)
 {
-    NeuralNetwork *nn = (NeuralNetwork*) malloc(sizeof(NeuralNetwork));
-    nn->Weights = (float*) malloc(sizeof(float)*N_PTS*N_MVTS);
-    nn->Biases = (float*) malloc(sizeof(float)*N_MVTS);
-    return nn;
+    for(int i = 0; i < N_PTS; i++)
+        for(int j = 0; j < 3; j++)
+            inputs[((i+amount)%N_PTS)*3+j] = AccDataLog[i*3+j];
 }
 
-// ------------------------- FreeNeuralNetwork --------------------------
-void FreeNeuralNetwork(NeuralNetwork *nn)
+// ------------------------------ PrintSet ------------------------------
+void PrintSet(int16_t *inputs)
 {
-    free(nn->Weights);
-    free(nn->Biases);
-    free(nn);
+    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("------- End Set -------\n\n\r");
 }
 
 // ------------------------------ Sigmoid -------------------------------
-float Sigmoid(float x)
-{
-    return 1/(1+exp(-x));
-}
+float Sigmoid(float x) { return 1/(1+exp(-x)); }
 
 // ----------------------------- SelectMvt ------------------------------
-Mvts SelectMvt(NeuralNetwork *nn, int16_t *AccDataLog, int index_write)
+Mvt SelectMvt(int16_t *inputs)
 {
     int i, j;
-    
-    // inputs = AccDataLog rotated of N_PTS-1-index_write
-    int shift_amount = N_PTS-1-index_write;
-    int16_t inputs [N_PTS*3];
-    for(i = 0; i < N_PTS; i++)
-        for(j = 0; j < 3; j++)
-            inputs[((i+shift_amount)%N_PTS)*3+j] = AccDataLog[i*3+j];
-    
-    /*
-    for(i = 0; i < N_PTS; i++)
-        Host_Comm.printf("%d ; %d ; %d\n", inputs[i*3],inputs[i*3+1],inputs[i*3+2]);
-    */
-    
     float selection [N_MVTS] = {};
     for (j = 0; j < N_MVTS; j++) {
         for (i = 0; i < N_PTS*3; i++)
-            selection[j] += inputs[i] * nn->Weights[i*N_PTS*3+j];
-        
-        selection[j] = Sigmoid(selection[j] + nn->Biases[j]);
+            selection[j] += (float)inputs[i] * Weights[i*N_MVTS+j];
+        selection[j] = Sigmoid(selection[j] + Biases[j]);
     }
     
-    Mvts mvt = Undefined;
-    for (i = 0; i < N_MVTS; i++)
+    Mvt mvt = Undefined;
+    for (i = 0; i < N_MVTS; i++) {
         if (selection[i] > THRESHOLD_MVT)
-            mvt = static_cast<Mvts>(i);
+            mvt = static_cast<Mvt>(i+1);
+        Host_Comm.printf("Proba mvt %d : %f\n\r",i+1,selection[i]);
+    }
     
     return mvt;
 }