File content as of revision 3:d03eae745223:

/* LELEC 2811 - BadmintonLogger - Group G */
#include "mbed.h"
#include "FreescaleIAP.h"   // Library for Flash Access
#include "MMA8491Q_PG.h"    // Accelerometer

#define MMA8491_I2C_ADDRESS (0x55<<1)

#define DISABLE_STATE           0
#define ENABLE_STATE            1

#define LED_ON                  0
#define LED_OFF                 1

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

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

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

struct NeuralNetwork {
    float* Weights;
    float* Biases;
};

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;
uint32_t KL25_Flash_Size;

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

extern IAPCode verify_erased(int address, unsigned int length);

// ----------------------------------------------------------------------

// -------------------------------- main --------------------------------
int main()
{
    Init ();
    
    uint8_t Count;
    
    while(true)
    {
        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(500);
                    Led_Blue = LED_OFF;
                    wait_ms(500);
                    Count --;
                    if (Count == 0)
                        Log();
                }
                else
                    Count = 0;
            }
        }
        wait_ms(500);     
    }
}

// -------------------------------- Init --------------------------------
void Init(void)
{
    Start_Pulse_In.mode(PullNone);  // Input Pin is programmed as floating
    Accel_Enable = DISABLE_STATE;   // 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");
}

// ----------------------------- Clear_Led ------------------------------
void Clear_Led(void)
{
    Led_Red = LED_OFF;
    Led_Green = LED_OFF;
    Led_Blue = LED_OFF ; // Bug on board : Turning On Blue Led decrease consumption...
}

// ---------------------------- Check_Jumper ----------------------------
bool Check_Jumper()
{
    uint8_t 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;
}

// -------------------------- myTimer_Acq_Task --------------------------
void myTimer_Acq_Task() { bTimer = 1; }

// ------------------------------ ReadData ------------------------------
Data ReadData()
{
    Data data;
    
    // Get Accelerometer data's
    Accel_Enable = ENABLE_STATE; // Rising Edge -> Start measure
    
    uint8_t 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 = DISABLE_STATE;
    
    // Get Piezo Data's
    data.Vout_IF = myPTE20.read_u16(); 
    data.Vout_FILT = myPTE21.read_u16();  
    data.Vout_GAIN = myPTE22.read_u16();
    
    return data;
}

// -------------------------------- 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;
    
    while(Check_Jumper())
    {
        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 ; %d ; %d ; %d\n", data.accX, data.accY, data.accZ, data.Vout_IF, data.Vout_FILT, data.Vout_GAIN);
        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)
        {
            shockDetected = 1;
            n_sinceShock = 0;
        }
        if (n_sinceShock == N_PTS/2 && shockDetected == 1)
        {
            mvts[index_mvt] = SelectMvt(nn, AccDataLog, index_write);
            index_mvt ++;
            shockDetected = 0;
        }
    
        index_write ++;
        n_sinceShock ++;
        if (index_write == N_PTS-1)
            index_write = 0;
    }
    FreeNeuralNetwork(nn);
    Clear_Led();
}

// ------------------------- InitNeuralNetwork --------------------------
NeuralNetwork *InitNeuralNetwork(void)
{
    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;
}

// ------------------------- FreeNeuralNetwork --------------------------
void FreeNeuralNetwork(NeuralNetwork *nn)
{
    free(nn->Weights);
    free(nn->Biases);
    free(nn);
}

// ------------------------------ Sigmoid -------------------------------
float Sigmoid(float x)
{
    return 1/(1+exp(-x));
}

// ----------------------------- SelectMvt ------------------------------
Mvts SelectMvt(NeuralNetwork *nn, int16_t *AccDataLog, int index_write)
{
    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]);
    }
    
    Mvts mvt = Undefined;
    for (i = 0; i < N_MVTS; i++)
        if (selection[i] > THRESHOLD_MVT)
            mvt = static_cast<Mvts>(i);
    
    return mvt;
}