Project Badminton Logger
Code for the project of LELEC2811 2017
Dependencies: FreescaleIAP MMA8491Q_PG mbed
Fork of
LELEC_2811_Accelerometer
by 
LELEC2811 - I&S
Diff: main.cpp
- Revision:
- 3:d03eae745223
- Parent:
- 2:f146ae6546b5
- Child:
- 4:2de56fc46abb
--- a/main.cpp Mon Nov 27 15:14:24 2017 +0000
+++ b/main.cpp Mon Nov 27 22:37:15 2017 +0000
@@ -5,18 +5,12 @@
#define MMA8491_I2C_ADDRESS (0x55<<1)
-#define NO_JUMPER 0
-#define JUMPER_PRESENT 1
-
-#define LEVEL_0 0
-#define LEVEL_1 1
+#define DISABLE_STATE 0
+#define ENABLE_STATE 1
#define LED_ON 0
#define LED_OFF 1
-#define DISABLE_STATE 0
-#define ENABLE_STATE 1
-
#define REG_OUT_X_MSB 0x01
#define REG_OUT_Y_MSB 0x03
#define REG_OUT_Z_MSB 0x05
@@ -25,368 +19,280 @@
#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 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
-typedef struct{
- int16_t X;
- int16_t Y;
- int16_t Z;
-} Accel_Data;
+MMA8491Q my8491(PTE0, PTE1, MMA8491_I2C_ADDRESS); // Setup I2C for MMA8491
-// --- Setup I2C for MMA8491
-MMA8491Q my8491(PTE0, PTE1, MMA8491_I2C_ADDRESS);
+Serial Host_Comm(USBTX, USBRX); // Set Serial Port
-// --- Set Serial Port
-Serial Host_Comm(USBTX, USBRX); // tx, rxSerial pc(USBTX, USBRX); // tx, rx
+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
+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(PTC9); // Used to enter/exit Acquisition mode
+DigitalIn Start_Pulse_In(PTC11); // ShortPin J1_15 and J1_16 to enter in Acq_Mode
-// Globale variable
-volatile bool bTimer; // 1 means a Timer tick is done
+// --------------------- 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 Nb_Sector;
+int n_sector;
uint32_t KL25_Flash_Size;
-// Function Declaration
-
+// ------------------------ Function Declaration ------------------------
+void Init(void);
void Clear_Led(void);
-int Acquisition_Flash(void);
-int Read_Data_Logging(void);
-bool Check_Jumper(void);
-void myTimer_Acq_Task(void);
-void Acquisition_Task(void);
-void Read_Task(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);
-int main() {
+// ----------------------------------------------------------------------
+// -------------------------------- main --------------------------------
+int main()
+{
+ Init ();
+
uint8_t Count;
- Start_Pulse_In.mode(PullNone); // Input Pin is programmed as floating
- Accel_Enable = DISABLE_STATE; // Turn Accel Enable to disabled state
-
- // --- Baud rate setting
- Host_Comm.baud(115200);
-
- Clear_Led();
-
- KL25_Flash_Size = flash_size(); // Get Size of KL25 Embedded Flash
- Nb_Sector = (KL25_Flash_Size / SECTOR_SIZE) - RESERVED_SECTOR; // Reserve Max 32K for App Code
-
- myTick_Acq.attach(&myTimer_Acq_Task, ACQ_TIMER_PERIOD);
-
- Host_Comm.printf("\n\rLELEC2811 Badminton Logger V1.0 Nov2017 Group G\n\r");
-
- for (;;)
+ while(true)
{
- if (Check_Jumper() == JUMPER_PRESENT)
+ if (Check_Jumper())
{
Clear_Led();
-
Count = 5;
while (Count !=0)
{
- if (Check_Jumper() == JUMPER_PRESENT)
+ if (Check_Jumper())
{
- Led_Blue = LED_ON; // Blink to alert user "Enter in Acquisition"
- wait_ms(900);
+ Led_Blue = LED_ON; // Blink to alert user "Enter in Logging mode"
+ wait_ms(500);
Led_Blue = LED_OFF;
- wait_ms(100);
+ wait_ms(500);
Count --;
if (Count == 0)
- {
- Acquisition_Task();
- }
+ Log();
}
else
- {
Count = 0;
- }
- }
- }
- else
- {
- Read_Task();
+ }
}
- }
-}
-
-void Read_Task()
-{
- char host_cmd;
- IAPCode Flash_State;
- bool bAcq_Done;
-
- Flash_State = verify_erased(Flash_Base_Address, KL25_Flash_Size - (RESERVED_SECTOR * SECTOR_SIZE));
- if (Flash_State == 0) // Virgin Flash ?
- {
- bAcq_Done = 0;
- }
- else
- {
- bAcq_Done = 1;
- }
-
- Clear_Led();
- wait_ms(500);
-
- if (bAcq_Done == 1)
- {
- Led_Green = LED_ON;
- Host_Comm.putc('1');
- }
- else
- {
- Led_Red = LED_ON;
- Host_Comm.putc('0');
- }
-
- if(Host_Comm.readable()) // Did we receive a char from Host ?
- {
- host_cmd = Host_Comm.getc(); // Get it
-
- if ((host_cmd == 'R') || (host_cmd == 'r')) // Read Flash Command ?
- {
- Read_Data_Logging(); // Read and send acquisition data
- }
- }
- wait_ms(50);
-}
-
-void Acquisition_Task()
-{
- int Acq_Status;
-
- Clear_Led();
-
- Acq_Status = Acquisition_Flash();
-
- Clear_Led();
-
- while (Check_Jumper() == JUMPER_PRESENT)
- {
- if (Acq_Status != FLASH_ACQ_DONE)
- {
- Led_Red = !Led_Red;
- }
- else
- {
- Led_Green = !Led_Green;
- }
- wait_ms(100);
+ 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...
+ Led_Blue = LED_OFF ; // Bug on board : Turning On Blue Led decrease consumption...
}
-bool Check_Jumper() // If J1_15 and J1_16 connected together -> return JUMPER_PRESENT
+// ---------------------------- Check_Jumper ----------------------------
+bool Check_Jumper()
{
uint8_t i;
-
for (i = 0 ; i < 2 ; i ++)
{
- Start_Pulse_Out = LEVEL_1;
+ Start_Pulse_Out = 1;
wait_ms(1);
- if (Start_Pulse_In != LEVEL_1)
- {
- return NO_JUMPER;
- }
+ if (Start_Pulse_In != 1)
+ return 0;
- Start_Pulse_Out = LEVEL_0;
+ Start_Pulse_Out = 0;
wait_ms(1);
- if (Start_Pulse_In != LEVEL_0)
- {
- return NO_JUMPER;
- }
+ if (Start_Pulse_In != 0)
+ return 0;
}
- return JUMPER_PRESENT;
+ return 1;
}
-int Acquisition_Flash(void)
+// -------------------------- myTimer_Acq_Task --------------------------
+void myTimer_Acq_Task() { bTimer = 1; }
+
+// ------------------------------ ReadData ------------------------------
+Data ReadData()
{
- int Status;
- int Flash_Ptr ;
- uint8_t Data_Ptr;
- Accel_Data myData[2];
- uint8_t Ready;
- int Led_Counter;
+ 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;
- for (Flash_Ptr = Flash_Base_Address ; Flash_Ptr < KL25_Flash_Size ; Flash_Ptr += 0x400)
+ // 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())
{
- Status = erase_sector(Flash_Ptr); // Erase sector
-
- if (Status !=0)
- {
- return ERASE_FLASH_ERROR;
- }
- }
-
- Flash_Ptr = Flash_Base_Address;
-
- Led_Blue = LED_ON;
-
- Led_Counter = 0;
- Data_Ptr = 0;
- while (Flash_Ptr < KL25_Flash_Size ) // Acq Loop
- {
- while (bTimer == 0) // Wait Acq Tick Timer
- {
-
- }
+ Led_Green != Led_Green; // LED blinks green while logging
+
+ while (bTimer == 0) {} // Wait Acq Tick Timer
bTimer = 0;
- Accel_Enable = ENABLE_STATE; // Rising Edge -> Start Accel Measure
+ 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 ((float) Led_Counter * ACQ_TIMER_PERIOD == 1.0) // Blink at 1 Hz
- {
- Led_Counter = 0;
- Led_Blue = !Led_Blue;
- }
-
-
- Ready = 0;
- while((Ready && 0x10) == 0) // Wait Accelerometer have new data's
+ if (currData.Vout_FILT >= THRESHOLD_SHOCK)
{
- Ready = my8491.Read_Status();
+ shockDetected = 1;
+ n_sinceShock = 0;
}
-
- myData[Data_Ptr].X = my8491.getAccAxis(REG_OUT_X_MSB);
- myData[Data_Ptr].Y = my8491.getAccAxis(REG_OUT_Y_MSB);
- myData[Data_Ptr].Z = my8491.getAccAxis(REG_OUT_Z_MSB);
-
- Led_Counter++;
-
- Accel_Enable = DISABLE_STATE;
-
- //Host_Comm.printf("\n\r%x\tX = %f", Flash_Ptr, float(myData[Data_Ptr].X)*4.0/4096.0 );
- //Host_Comm.printf("\tY = %f", float(myData[Data_Ptr].Y)*4.0/4096.0 );
- //Host_Comm.printf("\tZ = %f", float(myData[Data_Ptr].Z)*4.0/4096.0 );
-
- Data_Ptr ++;
-
- if (Data_Ptr == 2)// Save 2 acquistions -> 2 * 3 * 2 bytes = 12 bytes
+ if (n_sinceShock == N_PTS/2 && shockDetected == 1)
{
- Data_Ptr = 0;
- Status = program_flash(Flash_Ptr, (char *) &myData[0].X, 4); // Write 4 bytes in the Flash
- if (Status !=0)
- {
- return WRITE_FLASH_ERROR;
- }
-
- Flash_Ptr += 4;
- Status = program_flash(Flash_Ptr, (char *) &myData[0].Z, 4); // Write 4 bytes in the Flash
- if (Status !=0)
- {
- return WRITE_FLASH_ERROR;
- }
-
- Flash_Ptr += 4;
- //Status = program_flash(Flash_Ptr, (char *) &myData[1].Y, 4); // Write 4 bytes in the Flash
- Status = program_flash(Flash_Ptr, (char *) &myData[0].Y, 4); // Bug corrected 23/11/2016
- if (Status !=0)
- {
- return WRITE_FLASH_ERROR;
- }
-
- Flash_Ptr += 4;
-
- if ((Flash_Ptr & 0x3FC) == 0x3FC)
- {
- Flash_Ptr += 4; //170 * 6 = 1020 ---> skip 4 last bytes of each sector of 1024 bytes
- }
+ mvts[index_mvt] = SelectMvt(nn, AccDataLog, index_write);
+ index_mvt ++;
+ shockDetected = 0;
}
- if (Check_Jumper() != JUMPER_PRESENT) // If Jumper remoded -> Stop Acquisition
- {
- return FLASH_ACQ_DONE ;
- }
+
+ index_write ++;
+ n_sinceShock ++;
+ if (index_write == N_PTS-1)
+ index_write = 0;
}
- return FLASH_ACQ_DONE ;
+ 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;
}
-int Read_Data_Logging()
-{
- int16_t *data = (int16_t *) Flash_Base_Address; // uint16 pointer of data stored in Flash
- int Flash_Ptr;
- char cmd;
- int Record_Counter;
- float X_Val, Y_Val, Z_Val;
- int16_t Raw_X, Raw_Y, Raw_Z;
- int Max_Record;
-
- Clear_Led();
-
- Max_Record = Nb_Sector * (SECTOR_SIZE / sizeof(Accel_Data));
- Record_Counter = 0;
- Flash_Ptr = 0;
-
- //Host_Comm.printf("\n\rBegin of Data");
+// ------------------------- FreeNeuralNetwork --------------------------
+void FreeNeuralNetwork(NeuralNetwork *nn)
+{
+ free(nn->Weights);
+ free(nn->Biases);
+ free(nn);
+}
- while (Record_Counter < Max_Record)
- {
- Led_Green = !Led_Green;
- Led_Blue = !Led_Green;
-
- if(Host_Comm.readable())
- {
- cmd = Host_Comm.getc();
- if ((cmd == 'S') || (cmd == 's')) // Receiving 'S' or 's' means stop Read Flash
- {
- Clear_Led();
- return 0;
- }
- }
-
- Record_Counter ++;
-
- Raw_X = data[Flash_Ptr++];
- Raw_Y = data[Flash_Ptr++];
- Raw_Z = data[Flash_Ptr++];
-
- if ((Raw_X == -1) && (Raw_Y == -1) && (Raw_Z == -1)) // Valid data ? (!= 0xFFFFFFFF from empty Flash sector)
- {
- }
- else
- {
- X_Val = float(Raw_X) * 4.0/4096.0;
- Y_Val = float(Raw_Y) * 4.0/4096.0;
- Z_Val = float(Raw_Z) * 4.0/4096.0;
-
- Host_Comm.printf("\n\r%d\tX=%.4f\tY=%.4f\tZ=%.4f", Record_Counter, X_Val, Y_Val, Z_Val);
- }
-
- if ((Flash_Ptr & 0x1FE) == 0x1FE)
- {
- Flash_Ptr +=2; // skip the last bytes at the end of a sector
- }
- }
- //Host_Comm.printf("\n\rEnd of Data");
- Clear_Led();
- return 0;
+// ------------------------------ Sigmoid -------------------------------
+float Sigmoid(float x)
+{
+ return 1/(1+exp(-x));
}
-/* Interrupt Task */
-void myTimer_Acq_Task()
-{
- bTimer = 1;
+// ----------------------------- 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;
}