File content as of revision 4:2de56fc46abb:

/*
 *  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 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 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                  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);        // 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(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;
    float Vout_IF, Vout_FILT, Vout_GAIN;
};

enum Mvt { Undefined = 0, Serve, ClearOverhead, DropOverhead, SmashShot, ClearUnderarm, DropUnderarm };

struct MvtSet {
    int16_t inputs [N_PTS*3];
    Mvt mvt;
};

// -------------------------- Globale variable --------------------------
volatile bool bTimer; // 1 means a Timer tick is done

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);           // 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 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

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 ();
    
    int Count;
    
    while(!foundError)
    {
        if (Check_Jumper())
        {
            Clear_Led();
            Count = 5;
            while (Count !=0)
            {
                if (Check_Jumper())
                {
                    Led_Blue = LED_ON; // Blink to alert user "Enter in Logging mode"
                    wait_ms(750);
                    Led_Blue = LED_OFF;
                    wait_ms(250);
                    Count --;
                    if (Count == 0)
                        Log();
                }
                else
                    Count = 0;
            }
        }
        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()
{
    Start_Pulse_In.mode(PullNone);  // Input Pin is programmed as floating
    Accel_Enable = 0;               // Turn Accel Enable to disabled state
    Clear_Led();
    
    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");
    
    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()
{
    Led_Red = LED_OFF;
    Led_Green = LED_OFF;
    Led_Blue = LED_OFF ;
}

// ---------------------------- Check_Jumper ----------------------------
bool Check_Jumper()
{
    int i;
    for (i = 0 ; i < 2 ; i ++)
    {
        Start_Pulse_Out = 1;
        wait_ms(1);
        if (Start_Pulse_In != 1)
            return 0;
    
        Start_Pulse_Out = 0;
        wait_ms(1);
        if (Start_Pulse_In != 0)
            return 0;
    }
    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 = 1; // Rising Edge -> Start measure
    
    int ready = 0;
    while((ready && 0x10) == 0) // Wait for accelerometer to have new data's
        ready = my8491.Read_Status();

    data.accX = my8491.getAccAxis(REG_OUT_X_MSB);
    data.accY = my8491.getAccAxis(REG_OUT_Y_MSB);
    data.accZ = my8491.getAccAxis(REG_OUT_Z_MSB);
        
    Accel_Enable = 0;
    
    // Get Piezo Data's
    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;
}

// -------------------------------- 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
    bool enoughData = 0;
    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 (bTimer == 0) {} // Wait Acq Tick Timer
        bTimer = 0;
        
        currData = ReadData();
        //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;
        
        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)
        {
            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)
        {
            enoughData = 1;
            index_write = 0;
        }
    }
    Clear_Led();
}

// ------------------------------- Rotate -------------------------------
void Rotate(int16_t *AccDataLog, int amount, int16_t *inputs)
{
    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];
}

// ------------------------------ PrintSet ------------------------------
void PrintSet(int16_t *inputs)
{
    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)); }

// ----------------------------- 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]);
    }
    
    Mvt mvt = Undefined;
    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]);
    }
    
    return mvt;
}