Project Badminton Logger / Mbed 2 deprecated BadmintonLogger

Code for the project of LELEC2811 2017

Dependencies:   FreescaleIAP MMA8491Q_PG mbed

Fork of LELEC_2811_Accelerometer by LELEC2811 - I&S

main.cpp@6:4ea2ba88338f, 2017-12-02 (annotated)

Committer:
ATCuriosity
Date:
Sat Dec 02 21:14:53 2017 +0000
Revision:
6:4ea2ba88338f
Parent:
5:79b8cd191fa8 
finally complete ! (95% accurate on all the data sets)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ATCuriosity 4:2de56fc46abb 1 /*
ATCuriosity 4:2de56fc46abb 2 * LELEC 2811 - BadmintonLogger - Group 5
ATCuriosity 4:2de56fc46abb 3 */
ATCuriosity 4:2de56fc46abb 4
Salamandre 0:a18d6e69c993 5 #include "mbed.h"
Salamandre 0:a18d6e69c993 6 #include "FreescaleIAP.h" // Library for Flash Access
ATCuriosity 2:f146ae6546b5 7 #include "MMA8491Q_PG.h" // Accelerometer
ATCuriosity 4:2de56fc46abb 8 #include <cmath>
Salamandre 0:a18d6e69c993 9
Salamandre 0:a18d6e69c993 10 #define MMA8491_I2C_ADDRESS (0x55<<1)
ATCuriosity 4:2de56fc46abb 11 #define KL25Z_VDD 2.89 // Value of VDD : To be measured on board KL25Z pin P3V3 (calibration)
Salamandre 0:a18d6e69c993 12
Salamandre 0:a18d6e69c993 13 #define LED_ON 0
Salamandre 0:a18d6e69c993 14 #define LED_OFF 1
Salamandre 0:a18d6e69c993 15
ATCuriosity 5:79b8cd191fa8 16 #define OFFSET 10000 // OFFSET & RANGE to map inputs between 0 and 1
ATCuriosity 5:79b8cd191fa8 17 #define RANGE 20000 // RANGE = 2*OFFSET
ATCuriosity 5:79b8cd191fa8 18
ATCuriosity 4:2de56fc46abb 19 #define CONSOLE 0 // print all in console
ATCuriosity 4:2de56fc46abb 20 #define FLASH_MVT 1 // save in flash only mvts
ATCuriosity 4:2de56fc46abb 21 #define FLASH_ALL 2 // save in flash mvtSets
ATCuriosity 4:2de56fc46abb 22
Salamandre 0:a18d6e69c993 23 #define REG_OUT_X_MSB 0x01
Salamandre 0:a18d6e69c993 24 #define REG_OUT_Y_MSB 0x03
Salamandre 0:a18d6e69c993 25 #define REG_OUT_Z_MSB 0x05
ATCuriosity 3:d03eae745223 26
ATCuriosity 4:2de56fc46abb 27 #define SECTOR_SIZE 1024 // Numbers of bits by memory sector
ATCuriosity 4:2de56fc46abb 28 #define RESERVED_SECTOR 32 // 32K reserved for Application Code
ATCuriosity 3:d03eae745223 29
ATCuriosity 5:79b8cd191fa8 30 #define ACQ_TIMER_PERIOD 0.005 // Time between 2 acquisitions (here 5 mSec)
ATCuriosity 5:79b8cd191fa8 31 #define N_PTS 100 // Number of points for each axis used to detect mvt
ATCuriosity 4:2de56fc46abb 32 #define N_MVTS 5 // Number of mvts detected
ATCuriosity 6:4ea2ba88338f 33 #define THRESHOLD_MVT 0.4 // threshold to validate a mvt
ATCuriosity 6:4ea2ba88338f 34 #define THRESHOLD_SHOCK 0.45 // threshold to detect shock
Salamandre 0:a18d6e69c993 35
ATCuriosity 3:d03eae745223 36 MMA8491Q my8491(PTE0, PTE1, MMA8491_I2C_ADDRESS); // Setup I2C for MMA8491
Salamandre 0:a18d6e69c993 37
ATCuriosity 3:d03eae745223 38 Serial Host_Comm(USBTX, USBRX); // Set Serial Port
Salamandre 0:a18d6e69c993 39
ATCuriosity 4:2de56fc46abb 40 AnalogIn myPTE20(PTE20); // read Vout_IF
ATCuriosity 4:2de56fc46abb 41 AnalogIn myPTE21(PTE21); // read Vout_FILT
ATCuriosity 4:2de56fc46abb 42 AnalogIn myPTE22(PTE22); // read Vout_GAIN
Salamandre 0:a18d6e69c993 43
ATCuriosity 3:d03eae745223 44 Ticker myTick_Acq; // Periodical timer for Acquisition
ATCuriosity 4:2de56fc46abb 45
ATCuriosity 3:d03eae745223 46 DigitalOut Led_Red(LED1); // Define I/O for LEDs
Salamandre 0:a18d6e69c993 47 DigitalOut Led_Green(LED2);
Salamandre 0:a18d6e69c993 48 DigitalOut Led_Blue(LED3);
ATCuriosity 4:2de56fc46abb 49
Salamandre 0:a18d6e69c993 50 DigitalOut Accel_Enable(PTA13);
Salamandre 0:a18d6e69c993 51
ATCuriosity 4:2de56fc46abb 52 DigitalOut Start_Pulse_Out(PTE4); // Used to enter/exit Acquisition mode
ATCuriosity 4:2de56fc46abb 53 DigitalIn Start_Pulse_In(PTE5); // ShortPin J1_15 and J1_16 to enter in Acq_Mode
Salamandre 0:a18d6e69c993 54
ATCuriosity 3:d03eae745223 55 // --------------------- Structure and Enumeration ---------------------
ATCuriosity 3:d03eae745223 56 struct Data {
ATCuriosity 3:d03eae745223 57 int16_t accX, accY, accZ;
ATCuriosity 4:2de56fc46abb 58 float Vout_IF, Vout_FILT, Vout_GAIN;
ATCuriosity 3:d03eae745223 59 };
ATCuriosity 3:d03eae745223 60
ATCuriosity 5:79b8cd191fa8 61 //enum Mvt { Undefined = 0, Serve, ClearOverhead, DropOverhead, SmashShot, ClearUnderarm, DropUnderarm };
ATCuriosity 5:79b8cd191fa8 62 enum Mvt { Undefined = 0, Serve, ClearOverhead, SmashShot, ClearUnderarm, DropUnderarm };
ATCuriosity 4:2de56fc46abb 63
ATCuriosity 4:2de56fc46abb 64 struct MvtSet {
ATCuriosity 4:2de56fc46abb 65 int16_t inputs [N_PTS*3];
ATCuriosity 4:2de56fc46abb 66 Mvt mvt;
ATCuriosity 3:d03eae745223 67 };
ATCuriosity 3:d03eae745223 68
ATCuriosity 3:d03eae745223 69 // -------------------------- Globale variable --------------------------
ATCuriosity 3:d03eae745223 70 volatile bool bTimer; // 1 means a Timer tick is done
Salamandre 0:a18d6e69c993 71
ATCuriosity 4:2de56fc46abb 72 bool foundError = 0;
ATCuriosity 5:79b8cd191fa8 73 bool flashFull = 0;
ATCuriosity 4:2de56fc46abb 74 int mode = CONSOLE;
ATCuriosity 4:2de56fc46abb 75
ATCuriosity 4:2de56fc46abb 76 /* in flash :
ATCuriosity 4:2de56fc46abb 77 - [ 0x0 ; flash_base_address [ : code
ATCuriosity 5:79b8cd191fa8 78 - [ flash_base_address ; flash_base_address_cmd [ : data
ATCuriosity 5:79b8cd191fa8 79 - flash_base_address_cmd : flash_next_address (int)
ATCuriosity 5:79b8cd191fa8 80 - flash_base_address_cmd+4 : mode (int)
ATCuriosity 5:79b8cd191fa8 81 - flash_base_address_cmd+8 : flashFull (int) */
Salamandre 0:a18d6e69c993 82 uint32_t KL25_Flash_Size;
ATCuriosity 4:2de56fc46abb 83 int flash_base_address = RESERVED_SECTOR * SECTOR_SIZE ; // Store Flash Base Address
ATCuriosity 4:2de56fc46abb 84 int flash_next_address; // next address for saving data in flash
ATCuriosity 4:2de56fc46abb 85 int flash_base_address_cmd; // base address where the parameters are saved
ATCuriosity 6:4ea2ba88338f 86
ATCuriosity 6:4ea2ba88338f 87 const float w_s_l [3*N_PTS] = {
ATCuriosity 6:4ea2ba88338f 88 #include "weights_samples_layer.txt"
ATCuriosity 6:4ea2ba88338f 89 };
ATCuriosity 6:4ea2ba88338f 90 const float b_s_l [N_PTS] = {
ATCuriosity 6:4ea2ba88338f 91 #include "biases_samples_layer.txt"
ATCuriosity 4:2de56fc46abb 92 };
ATCuriosity 6:4ea2ba88338f 93 const float w_o_l [N_PTS*N_MVTS] = {
ATCuriosity 6:4ea2ba88338f 94 #include "weights_output_layer.txt"
ATCuriosity 4:2de56fc46abb 95 };
ATCuriosity 6:4ea2ba88338f 96 const float b_o_l [N_MVTS] = {
ATCuriosity 6:4ea2ba88338f 97 #include "biases_output_layer.txt"
ATCuriosity 6:4ea2ba88338f 98 };
ATCuriosity 6:4ea2ba88338f 99
ATCuriosity 3:d03eae745223 100 // ------------------------ Function Declaration ------------------------
ATCuriosity 3:d03eae745223 101 void Init(void);
ATCuriosity 5:79b8cd191fa8 102 void DisplayFlashInfos(void); // display memory use
ATCuriosity 5:79b8cd191fa8 103 void DisplayInstructions(void); // display available cmds
ATCuriosity 4:2de56fc46abb 104 void Clear_Led(void); // switch off led's
ATCuriosity 4:2de56fc46abb 105 bool Check_Jumper(void); // if J1_15 & J1_16 connected together -> return 1
ATCuriosity 4:2de56fc46abb 106 void Check_Console(void); // detect input from user in console
ATCuriosity 4:2de56fc46abb 107 void myTimer_Acq_Task(void); // called by the timer
ATCuriosity 3:d03eae745223 108
ATCuriosity 4:2de56fc46abb 109 void EraseAllSectors(void); // erase all sectors containing data
ATCuriosity 4:2de56fc46abb 110 void EraseSector(int address); // erase one sector
ATCuriosity 4:2de56fc46abb 111 void UpdateParamFlash(void); // update next_address_flash and mode in flash
ATCuriosity 4:2de56fc46abb 112 void WriteFlash(MvtSet mvtSet); // write only mvt or all set depending on 'all'
ATCuriosity 4:2de56fc46abb 113 void ReadFlash(void); // print memory content in console
ATCuriosity 5:79b8cd191fa8 114 void LineHandler(int16_t *line, int16_t next); // handle display of data set (used in ReadFlash)
Salamandre 0:a18d6e69c993 115
ATCuriosity 4:2de56fc46abb 116 Data ReadData(void); // read data from accelerometer and piezo
ATCuriosity 4:2de56fc46abb 117 void Log(void); // read data, detect shock and movement
ATCuriosity 4:2de56fc46abb 118 void Rotate(int16_t *AccDataLog, int amount, int16_t *inputs); // inputs = AccDataLog rotated of amount
ATCuriosity 5:79b8cd191fa8 119 void PrintSet(MvtSet mvtSet); // display set of data
ATCuriosity 6:4ea2ba88338f 120
ATCuriosity 6:4ea2ba88338f 121 float Map (float x); // inputs mapped between 0 and 1
ATCuriosity 6:4ea2ba88338f 122 float Sigmoid (float x); // sigmoid function
ATCuriosity 6:4ea2ba88338f 123 void Softmax (float *inputs, float *result); // softmax function
ATCuriosity 4:2de56fc46abb 124 Mvt SelectMvt(int16_t *inputs); // compute probabilities for each mvt based on the inputs
Salamandre 0:a18d6e69c993 125
ATCuriosity 4:2de56fc46abb 126 // -------------------------------------------------------------------------------------------------------
ATCuriosity 4:2de56fc46abb 127 // -------------------------------------------------------------------------------------------------------
Salamandre 0:a18d6e69c993 128
ATCuriosity 3:d03eae745223 129 // -------------------------------- main --------------------------------
ATCuriosity 3:d03eae745223 130 int main()
ATCuriosity 3:d03eae745223 131 {
ATCuriosity 3:d03eae745223 132 Init ();
ATCuriosity 3:d03eae745223 133
ATCuriosity 4:2de56fc46abb 134 int Count;
Salamandre 0:a18d6e69c993 135
ATCuriosity 4:2de56fc46abb 136 while(!foundError)
Salamandre 0:a18d6e69c993 137 {
ATCuriosity 5:79b8cd191fa8 138 if (Check_Jumper() && !flashFull)
Salamandre 0:a18d6e69c993 139 {
Salamandre 0:a18d6e69c993 140 Clear_Led();
Salamandre 0:a18d6e69c993 141 Count = 5;
Salamandre 0:a18d6e69c993 142 while (Count !=0)
Salamandre 0:a18d6e69c993 143 {
ATCuriosity 3:d03eae745223 144 if (Check_Jumper())
Salamandre 0:a18d6e69c993 145 {
ATCuriosity 5:79b8cd191fa8 146 Led_Green = LED_ON; // Blink to alert user "Enter in Logging mode"
ATCuriosity 4:2de56fc46abb 147 wait_ms(750);
ATCuriosity 5:79b8cd191fa8 148 Led_Green = LED_OFF;
ATCuriosity 4:2de56fc46abb 149 wait_ms(250);
Salamandre 0:a18d6e69c993 150 Count --;
Salamandre 0:a18d6e69c993 151 if (Count == 0)
ATCuriosity 3:d03eae745223 152 Log();
Salamandre 0:a18d6e69c993 153 }
Salamandre 0:a18d6e69c993 154 else
Salamandre 0:a18d6e69c993 155 Count = 0;
ATCuriosity 3:d03eae745223 156 }
Salamandre 0:a18d6e69c993 157 }
ATCuriosity 5:79b8cd191fa8 158 if (flashFull)
ATCuriosity 5:79b8cd191fa8 159 Led_Red = !Led_Red;
ATCuriosity 5:79b8cd191fa8 160 else
ATCuriosity 5:79b8cd191fa8 161 Led_Blue = !Led_Blue;
ATCuriosity 4:2de56fc46abb 162 Check_Console();
ATCuriosity 4:2de56fc46abb 163 wait_ms(100);
Salamandre 0:a18d6e69c993 164 }
ATCuriosity 4:2de56fc46abb 165
ATCuriosity 4:2de56fc46abb 166 Host_Comm.printf("\n\rProgram is exiting due to error...\n\r");
ATCuriosity 4:2de56fc46abb 167 Clear_Led();
ATCuriosity 4:2de56fc46abb 168 Led_Red = LED_ON;
Salamandre 0:a18d6e69c993 169 }
Salamandre 0:a18d6e69c993 170
ATCuriosity 3:d03eae745223 171 // -------------------------------- Init --------------------------------
ATCuriosity 4:2de56fc46abb 172 void Init()
ATCuriosity 3:d03eae745223 173 {
ATCuriosity 3:d03eae745223 174 Start_Pulse_In.mode(PullNone); // Input Pin is programmed as floating
ATCuriosity 4:2de56fc46abb 175 Accel_Enable = 0; // Turn Accel Enable to disabled state
ATCuriosity 3:d03eae745223 176 Clear_Led();
ATCuriosity 3:d03eae745223 177
ATCuriosity 3:d03eae745223 178 myTick_Acq.attach(&myTimer_Acq_Task, ACQ_TIMER_PERIOD); // Timer for acquisition
ATCuriosity 3:d03eae745223 179
ATCuriosity 3:d03eae745223 180 Host_Comm.baud(115200); // Baud rate setting
ATCuriosity 5:79b8cd191fa8 181 Host_Comm.printf("\n\r*****\n\rLELEC2811 - Badminton Logger - Group 5\n\r*****\n\n\r");
ATCuriosity 4:2de56fc46abb 182
ATCuriosity 4:2de56fc46abb 183 KL25_Flash_Size = flash_size(); // Get Size of KL25 Embedded Flash
ATCuriosity 4:2de56fc46abb 184 flash_base_address_cmd = KL25_Flash_Size-SECTOR_SIZE;
ATCuriosity 4:2de56fc46abb 185
ATCuriosity 5:79b8cd191fa8 186 /* TO CHECK SIZE OF PROGRAM IN FLASH :
ATCuriosity 5:79b8cd191fa8 187 int *ptr;
ATCuriosity 5:79b8cd191fa8 188 int n_sectors = KL25_Flash_Size/SECTOR_SIZE;
ATCuriosity 5:79b8cd191fa8 189 Host_Comm.printf("Number of sectors : %d\n\r",n_sectors);
ATCuriosity 5:79b8cd191fa8 190 for (int i = 0; i < n_sectors; i++){
ATCuriosity 5:79b8cd191fa8 191 ptr = (int*) (i*SECTOR_SIZE);
ATCuriosity 5:79b8cd191fa8 192 Host_Comm.printf("Sector %d : %d\n\r",i, ptr[0]);
ATCuriosity 5:79b8cd191fa8 193 }
ATCuriosity 5:79b8cd191fa8 194 */
ATCuriosity 5:79b8cd191fa8 195
ATCuriosity 4:2de56fc46abb 196 int *base_address_ptr = (int*)flash_base_address_cmd;
ATCuriosity 4:2de56fc46abb 197 flash_next_address = base_address_ptr[0];
ATCuriosity 4:2de56fc46abb 198 if (flash_next_address >= flash_base_address_cmd || flash_next_address < flash_base_address)
ATCuriosity 4:2de56fc46abb 199 {
ATCuriosity 5:79b8cd191fa8 200 Host_Comm.printf("First run (or error with previous flash_next_address).\n\r");
ATCuriosity 4:2de56fc46abb 201 EraseAllSectors();
ATCuriosity 4:2de56fc46abb 202 flash_next_address = flash_base_address;
ATCuriosity 4:2de56fc46abb 203 mode = CONSOLE;
ATCuriosity 4:2de56fc46abb 204 UpdateParamFlash();
ATCuriosity 4:2de56fc46abb 205 }
ATCuriosity 4:2de56fc46abb 206 else {
ATCuriosity 4:2de56fc46abb 207 mode = base_address_ptr[1];
ATCuriosity 5:79b8cd191fa8 208 flashFull = base_address_ptr[2];
ATCuriosity 4:2de56fc46abb 209 if (mode != CONSOLE && mode != FLASH_MVT && mode != FLASH_ALL) {
ATCuriosity 4:2de56fc46abb 210 mode = CONSOLE;
ATCuriosity 4:2de56fc46abb 211 UpdateParamFlash();
ATCuriosity 4:2de56fc46abb 212 }
ATCuriosity 4:2de56fc46abb 213 }
ATCuriosity 5:79b8cd191fa8 214 DisplayFlashInfos();
ATCuriosity 4:2de56fc46abb 215
ATCuriosity 4:2de56fc46abb 216 Host_Comm.printf("Initialization done.\n\n\r");
ATCuriosity 5:79b8cd191fa8 217 DisplayInstructions();
ATCuriosity 5:79b8cd191fa8 218 }
ATCuriosity 5:79b8cd191fa8 219
ATCuriosity 5:79b8cd191fa8 220 // -------------------------- DisplayFlashInfos -------------------------
ATCuriosity 5:79b8cd191fa8 221 void DisplayFlashInfos()
ATCuriosity 5:79b8cd191fa8 222 {
ATCuriosity 5:79b8cd191fa8 223 Host_Comm.printf("flash_next_address = %d\n\r",flash_next_address);
ATCuriosity 5:79b8cd191fa8 224 Host_Comm.printf("mode = %d\n\r",mode);
ATCuriosity 5:79b8cd191fa8 225 Host_Comm.printf("Memory used = %f %%\n\r",((float)flash_next_address-flash_base_address)/((float)flash_base_address_cmd-flash_base_address)*100);
ATCuriosity 5:79b8cd191fa8 226 }
ATCuriosity 5:79b8cd191fa8 227
ATCuriosity 5:79b8cd191fa8 228 // ------------------------- DisplayInstructions ------------------------
ATCuriosity 5:79b8cd191fa8 229 void DisplayInstructions()
ATCuriosity 5:79b8cd191fa8 230 {
ATCuriosity 4:2de56fc46abb 231 Host_Comm.printf("When the jumper is removed, use the keyboard :\n\r");
ATCuriosity 5:79b8cd191fa8 232 Host_Comm.printf("- to erase flash : 'E' = erase flash\n\r");
ATCuriosity 5:79b8cd191fa8 233 Host_Comm.printf("- to read flash : 'R' = read flash ; 'S' = stop reading\n\r");
ATCuriosity 5:79b8cd191fa8 234 Host_Comm.printf("- to change mode : 'C' = console mode ; 'M' = write_mvt mode ; 'A' = write_all mode\n\r");
ATCuriosity 5:79b8cd191fa8 235 Host_Comm.printf("!!! If mode change, ReadData() will fail => also press 'E' !!!\n\n\r");
ATCuriosity 3:d03eae745223 236 }
ATCuriosity 3:d03eae745223 237
ATCuriosity 3:d03eae745223 238 // ----------------------------- Clear_Led ------------------------------
ATCuriosity 4:2de56fc46abb 239 void Clear_Led()
Salamandre 0:a18d6e69c993 240 {
Salamandre 0:a18d6e69c993 241 Led_Red = LED_OFF;
Salamandre 0:a18d6e69c993 242 Led_Green = LED_OFF;
ATCuriosity 4:2de56fc46abb 243 Led_Blue = LED_OFF ;
Salamandre 0:a18d6e69c993 244 }
Salamandre 0:a18d6e69c993 245
ATCuriosity 3:d03eae745223 246 // ---------------------------- Check_Jumper ----------------------------
ATCuriosity 3:d03eae745223 247 bool Check_Jumper()
Salamandre 0:a18d6e69c993 248 {
ATCuriosity 4:2de56fc46abb 249 int i;
Salamandre 0:a18d6e69c993 250 for (i = 0 ; i < 2 ; i ++)
Salamandre 0:a18d6e69c993 251 {
ATCuriosity 3:d03eae745223 252 Start_Pulse_Out = 1;
Salamandre 0:a18d6e69c993 253 wait_ms(1);
ATCuriosity 3:d03eae745223 254 if (Start_Pulse_In != 1)
ATCuriosity 3:d03eae745223 255 return 0;
Salamandre 0:a18d6e69c993 256
ATCuriosity 3:d03eae745223 257 Start_Pulse_Out = 0;
Salamandre 0:a18d6e69c993 258 wait_ms(1);
ATCuriosity 3:d03eae745223 259 if (Start_Pulse_In != 0)
ATCuriosity 3:d03eae745223 260 return 0;
Salamandre 0:a18d6e69c993 261 }
ATCuriosity 3:d03eae745223 262 return 1;
Salamandre 0:a18d6e69c993 263 }
Salamandre 0:a18d6e69c993 264
ATCuriosity 4:2de56fc46abb 265 // --------------------------- Check_Console ----------------------------
ATCuriosity 4:2de56fc46abb 266 void Check_Console()
ATCuriosity 4:2de56fc46abb 267 {
ATCuriosity 4:2de56fc46abb 268 if(Host_Comm.readable())
ATCuriosity 4:2de56fc46abb 269 {
ATCuriosity 4:2de56fc46abb 270 char cmd = Host_Comm.getc();
ATCuriosity 5:79b8cd191fa8 271 if ((cmd == 'E') || (cmd == 'e')) {
ATCuriosity 6:4ea2ba88338f 272 Host_Comm.printf("Press 'E' again to confirm.\n\r");
ATCuriosity 6:4ea2ba88338f 273 wait_ms(1000);
ATCuriosity 6:4ea2ba88338f 274 if(Host_Comm.readable()) {
ATCuriosity 6:4ea2ba88338f 275 cmd = Host_Comm.getc();
ATCuriosity 6:4ea2ba88338f 276 if ((cmd == 'E') || (cmd == 'e')) {
ATCuriosity 6:4ea2ba88338f 277 EraseAllSectors();
ATCuriosity 6:4ea2ba88338f 278 flash_next_address = flash_base_address;
ATCuriosity 6:4ea2ba88338f 279 Host_Comm.printf("Erase done.\n\r");
ATCuriosity 6:4ea2ba88338f 280 }
ATCuriosity 6:4ea2ba88338f 281 }
ATCuriosity 6:4ea2ba88338f 282 else
ATCuriosity 6:4ea2ba88338f 283 Host_Comm.printf("Erase aborded.\n\r");
ATCuriosity 5:79b8cd191fa8 284 }
ATCuriosity 5:79b8cd191fa8 285 else if ((cmd == 'C') || (cmd == 'c')) {
ATCuriosity 4:2de56fc46abb 286 mode = CONSOLE;
ATCuriosity 5:79b8cd191fa8 287 Host_Comm.printf("Mode console (0) actived.\n\r");
ATCuriosity 5:79b8cd191fa8 288 }
ATCuriosity 5:79b8cd191fa8 289 else if ((cmd == 'M') || (cmd == 'm')) {
ATCuriosity 4:2de56fc46abb 290 mode = FLASH_MVT;
ATCuriosity 5:79b8cd191fa8 291 Host_Comm.printf("Mode flash_mvt (1) actived.\n\r");
ATCuriosity 5:79b8cd191fa8 292 }
ATCuriosity 5:79b8cd191fa8 293 else if ((cmd == 'A') || (cmd == 'a')) {
ATCuriosity 4:2de56fc46abb 294 mode = FLASH_ALL;
ATCuriosity 5:79b8cd191fa8 295 Host_Comm.printf("Mode flash_all (2) actived.\n\r");
ATCuriosity 5:79b8cd191fa8 296 }
ATCuriosity 4:2de56fc46abb 297 else if ((cmd == 'R') || (cmd == 'r'))
ATCuriosity 4:2de56fc46abb 298 ReadFlash();
ATCuriosity 4:2de56fc46abb 299
ATCuriosity 4:2de56fc46abb 300 UpdateParamFlash();
ATCuriosity 4:2de56fc46abb 301 }
ATCuriosity 4:2de56fc46abb 302 }
ATCuriosity 4:2de56fc46abb 303
ATCuriosity 3:d03eae745223 304 // -------------------------- myTimer_Acq_Task --------------------------
ATCuriosity 3:d03eae745223 305 void myTimer_Acq_Task() { bTimer = 1; }
ATCuriosity 3:d03eae745223 306
ATCuriosity 4:2de56fc46abb 307 // -------------------------------------------------------------------------------------------------------
ATCuriosity 4:2de56fc46abb 308 // -------------------------------------------------------------------------------------------------------
ATCuriosity 4:2de56fc46abb 309
ATCuriosity 4:2de56fc46abb 310 // -------------------------- EraseAllSectors ---------------------------
ATCuriosity 4:2de56fc46abb 311 void EraseAllSectors(void)
ATCuriosity 4:2de56fc46abb 312 {
ATCuriosity 4:2de56fc46abb 313 for (int address = flash_base_address ; address < KL25_Flash_Size ; address += SECTOR_SIZE)
ATCuriosity 4:2de56fc46abb 314 {
ATCuriosity 4:2de56fc46abb 315 EraseSector(address);
ATCuriosity 4:2de56fc46abb 316 if(foundError)
ATCuriosity 4:2de56fc46abb 317 return;
ATCuriosity 4:2de56fc46abb 318 }
ATCuriosity 5:79b8cd191fa8 319 flashFull = 0;
ATCuriosity 4:2de56fc46abb 320 }
ATCuriosity 4:2de56fc46abb 321
ATCuriosity 4:2de56fc46abb 322 // ---------------------------- EraseSector -----------------------------
ATCuriosity 4:2de56fc46abb 323 void EraseSector(int address)
ATCuriosity 4:2de56fc46abb 324 {
ATCuriosity 4:2de56fc46abb 325 IAPCode status = erase_sector(address);
ATCuriosity 4:2de56fc46abb 326 if (status != Success) {
ATCuriosity 4:2de56fc46abb 327 Host_Comm.printf("\n\rError in EraseSector() : status = %d\n\r", status);
ATCuriosity 4:2de56fc46abb 328 foundError = 1;
ATCuriosity 4:2de56fc46abb 329 }
ATCuriosity 4:2de56fc46abb 330 }
ATCuriosity 4:2de56fc46abb 331
ATCuriosity 4:2de56fc46abb 332 // -------------------------- UpdateParamFlash --------------------------
ATCuriosity 4:2de56fc46abb 333 void UpdateParamFlash()
ATCuriosity 4:2de56fc46abb 334 {
ATCuriosity 4:2de56fc46abb 335 EraseSector(flash_base_address_cmd);
ATCuriosity 4:2de56fc46abb 336 if(foundError)
ATCuriosity 4:2de56fc46abb 337 return;
ATCuriosity 4:2de56fc46abb 338
ATCuriosity 5:79b8cd191fa8 339 int toWrite[3] = {flash_next_address, mode, flashFull};
ATCuriosity 5:79b8cd191fa8 340 IAPCode status = program_flash(flash_base_address_cmd, (char *) &toWrite, 12);
ATCuriosity 4:2de56fc46abb 341 if (status != Success) {
ATCuriosity 4:2de56fc46abb 342 Host_Comm.printf("\n\rError in UpdateParamFlash() : status = %d\n\r", status);
ATCuriosity 4:2de56fc46abb 343 foundError = 1;
ATCuriosity 4:2de56fc46abb 344 }
ATCuriosity 4:2de56fc46abb 345 }
ATCuriosity 4:2de56fc46abb 346
ATCuriosity 4:2de56fc46abb 347 // ----------------------------- WriteFlash -----------------------------
ATCuriosity 4:2de56fc46abb 348 void WriteFlash(MvtSet mvtSet)
ATCuriosity 4:2de56fc46abb 349 {
ATCuriosity 4:2de56fc46abb 350 IAPCode status;
ATCuriosity 5:79b8cd191fa8 351 int toWrite;
ATCuriosity 4:2de56fc46abb 352
ATCuriosity 4:2de56fc46abb 353 if (mode == FLASH_ALL) // inputs (2*3*N_PTS bytes) + mvt (1 byte)
ATCuriosity 4:2de56fc46abb 354 {
ATCuriosity 5:79b8cd191fa8 355 // check if enough place
ATCuriosity 5:79b8cd191fa8 356 if (flash_next_address+(2*(3*N_PTS)+4) > flash_base_address_cmd) {
ATCuriosity 5:79b8cd191fa8 357 Host_Comm.printf("\n\rFlash is full.\n\r");
ATCuriosity 5:79b8cd191fa8 358 flashFull = 1; return;
ATCuriosity 5:79b8cd191fa8 359 }
ATCuriosity 5:79b8cd191fa8 360
ATCuriosity 4:2de56fc46abb 361 // add all bytes one behind the other : 2bytes*3*N_PTS (inputs) + 1byte (mvt)
ATCuriosity 4:2de56fc46abb 362 int remainder = (3*N_PTS) % 2; // modulo 2 because compacting 2bytes into 4bytes words
ATCuriosity 5:79b8cd191fa8 363 int even = 3*N_PTS-remainder;
ATCuriosity 4:2de56fc46abb 364
ATCuriosity 5:79b8cd191fa8 365 for (int i = 0; i < even; i+=2)
ATCuriosity 5:79b8cd191fa8 366 {
ATCuriosity 5:79b8cd191fa8 367 toWrite = (mvtSet.inputs[i] << 16) | (mvtSet.inputs[i+1] & 0x0000FFFF);
ATCuriosity 5:79b8cd191fa8 368 status = program_flash(flash_next_address, (char*) &toWrite, 4);
ATCuriosity 5:79b8cd191fa8 369 if (status != Success) {
ATCuriosity 5:79b8cd191fa8 370 Host_Comm.printf("\n\rError in WriteFlash() (0) : status = %d\n\r", status);
ATCuriosity 5:79b8cd191fa8 371 foundError = 1; return;
ATCuriosity 5:79b8cd191fa8 372 }
ATCuriosity 5:79b8cd191fa8 373 flash_next_address += 4;
ATCuriosity 5:79b8cd191fa8 374 }
ATCuriosity 5:79b8cd191fa8 375
ATCuriosity 5:79b8cd191fa8 376 toWrite = mvtSet.mvt;
ATCuriosity 5:79b8cd191fa8 377 if(remainder == 1)
ATCuriosity 5:79b8cd191fa8 378 toWrite = toWrite | (mvtSet.inputs[3*N_PTS-1] << 16);
ATCuriosity 4:2de56fc46abb 379 status = program_flash(flash_next_address, (char*) &toWrite, 4);
ATCuriosity 4:2de56fc46abb 380 }
ATCuriosity 5:79b8cd191fa8 381 else
ATCuriosity 4:2de56fc46abb 382 {
ATCuriosity 5:79b8cd191fa8 383 // check if enough place
ATCuriosity 5:79b8cd191fa8 384 if (flash_next_address+4 > flash_base_address_cmd) {
ATCuriosity 5:79b8cd191fa8 385 Host_Comm.printf("\n\rFlash is full.\n\r");
ATCuriosity 5:79b8cd191fa8 386 flashFull = 1; return;
ATCuriosity 5:79b8cd191fa8 387 }
ATCuriosity 4:2de56fc46abb 388
ATCuriosity 5:79b8cd191fa8 389 toWrite = mvtSet.mvt;
ATCuriosity 4:2de56fc46abb 390 status = program_flash(flash_next_address, (char*) &toWrite, 4);
ATCuriosity 5:79b8cd191fa8 391 }
ATCuriosity 5:79b8cd191fa8 392
ATCuriosity 5:79b8cd191fa8 393 if (status != Success) {
ATCuriosity 5:79b8cd191fa8 394 Host_Comm.printf("\n\rError in WriteFlash() (1) : status = %d\n\r", status);
ATCuriosity 5:79b8cd191fa8 395 foundError = 1; return;
ATCuriosity 4:2de56fc46abb 396 }
ATCuriosity 4:2de56fc46abb 397
ATCuriosity 4:2de56fc46abb 398 flash_next_address += 4;
ATCuriosity 4:2de56fc46abb 399 UpdateParamFlash();
ATCuriosity 4:2de56fc46abb 400 }
ATCuriosity 4:2de56fc46abb 401
ATCuriosity 4:2de56fc46abb 402 // ----------------------------- ReadFlash ------------------------------
ATCuriosity 4:2de56fc46abb 403 void ReadFlash()
ATCuriosity 4:2de56fc46abb 404 {
ATCuriosity 4:2de56fc46abb 405 Host_Comm.printf("\n\r------ Begin Read Flash ------\n\r");
ATCuriosity 5:79b8cd191fa8 406 DisplayFlashInfos();
ATCuriosity 4:2de56fc46abb 407
ATCuriosity 4:2de56fc46abb 408 char cmd;
ATCuriosity 5:79b8cd191fa8 409 int *currAddress_ptr = (int*)flash_base_address;
ATCuriosity 5:79b8cd191fa8 410 int *stopAddress_ptr = (int*)flash_next_address;
ATCuriosity 4:2de56fc46abb 411
ATCuriosity 5:79b8cd191fa8 412 Host_Comm.printf("ReadFlash : size = %d\n\r",stopAddress_ptr-currAddress_ptr);
ATCuriosity 4:2de56fc46abb 413
ATCuriosity 5:79b8cd191fa8 414 if (mode == FLASH_ALL)
ATCuriosity 4:2de56fc46abb 415 {
ATCuriosity 5:79b8cd191fa8 416 int remainder = (3*N_PTS) % 2;
ATCuriosity 5:79b8cd191fa8 417 int n_words_by_set = (3*N_PTS - remainder)/2+1;
ATCuriosity 5:79b8cd191fa8 418 int word;
ATCuriosity 5:79b8cd191fa8 419 int16_t line [4] = {}; // 3 components of acc (line[3] = position in line)
ATCuriosity 5:79b8cd191fa8 420
ATCuriosity 5:79b8cd191fa8 421 while (currAddress_ptr < stopAddress_ptr)
ATCuriosity 4:2de56fc46abb 422 {
ATCuriosity 5:79b8cd191fa8 423 // check for user input
ATCuriosity 5:79b8cd191fa8 424 if(Host_Comm.readable()) {
ATCuriosity 5:79b8cd191fa8 425 cmd = Host_Comm.getc();
ATCuriosity 5:79b8cd191fa8 426 if ((cmd == 'S') || (cmd == 's'))
ATCuriosity 5:79b8cd191fa8 427 return;
ATCuriosity 5:79b8cd191fa8 428 }
ATCuriosity 5:79b8cd191fa8 429
ATCuriosity 5:79b8cd191fa8 430 // print all set
ATCuriosity 5:79b8cd191fa8 431 for (word = 0; word < n_words_by_set-1; word++) {
ATCuriosity 5:79b8cd191fa8 432 LineHandler(line,(int16_t)(currAddress_ptr[word]>>16));
ATCuriosity 5:79b8cd191fa8 433 LineHandler(line,(int16_t)currAddress_ptr[word]);
ATCuriosity 5:79b8cd191fa8 434 }
ATCuriosity 5:79b8cd191fa8 435 if (remainder == 1)
ATCuriosity 5:79b8cd191fa8 436 LineHandler(line,(int16_t)(currAddress_ptr[word]>>16));
ATCuriosity 5:79b8cd191fa8 437 Host_Comm.printf("Mvt = %d\n\r",(int16_t)currAddress_ptr[word]);
ATCuriosity 5:79b8cd191fa8 438 currAddress_ptr += n_words_by_set;
ATCuriosity 4:2de56fc46abb 439 }
ATCuriosity 5:79b8cd191fa8 440 }
ATCuriosity 5:79b8cd191fa8 441 else
ATCuriosity 5:79b8cd191fa8 442 {
ATCuriosity 5:79b8cd191fa8 443 while (currAddress_ptr < stopAddress_ptr)
ATCuriosity 5:79b8cd191fa8 444 {
ATCuriosity 5:79b8cd191fa8 445 // check for user input
ATCuriosity 5:79b8cd191fa8 446 if(Host_Comm.readable()) {
ATCuriosity 5:79b8cd191fa8 447 cmd = Host_Comm.getc();
ATCuriosity 5:79b8cd191fa8 448 if ((cmd == 'S') || (cmd == 's'))
ATCuriosity 5:79b8cd191fa8 449 return;
ATCuriosity 5:79b8cd191fa8 450 }
ATCuriosity 5:79b8cd191fa8 451
ATCuriosity 5:79b8cd191fa8 452 // read mvt
ATCuriosity 5:79b8cd191fa8 453 Host_Comm.printf("Mvt = %d\n\r",currAddress_ptr[0]);
ATCuriosity 5:79b8cd191fa8 454 currAddress_ptr ++;
ATCuriosity 5:79b8cd191fa8 455 }
ATCuriosity 4:2de56fc46abb 456 }
ATCuriosity 4:2de56fc46abb 457
ATCuriosity 4:2de56fc46abb 458 Host_Comm.printf("\n\r------- End Read Flash -------\n\n\r");
ATCuriosity 4:2de56fc46abb 459 }
ATCuriosity 4:2de56fc46abb 460
ATCuriosity 5:79b8cd191fa8 461 // ----------------------------- LineHandler ----------------------------
ATCuriosity 5:79b8cd191fa8 462 void LineHandler(int16_t *line, int16_t next)
ATCuriosity 5:79b8cd191fa8 463 {
ATCuriosity 5:79b8cd191fa8 464 line[line[3]] = next;
ATCuriosity 5:79b8cd191fa8 465 line[3] ++;
ATCuriosity 5:79b8cd191fa8 466 if (line[3] == 3) {
ATCuriosity 5:79b8cd191fa8 467 Host_Comm.printf("%d %d %d\n\r",line[0],line[1],line[2]);
ATCuriosity 5:79b8cd191fa8 468 line[3] = 0;
ATCuriosity 5:79b8cd191fa8 469 }
ATCuriosity 5:79b8cd191fa8 470 }
ATCuriosity 5:79b8cd191fa8 471
ATCuriosity 4:2de56fc46abb 472 // -------------------------------------------------------------------------------------------------------
ATCuriosity 4:2de56fc46abb 473 // -------------------------------------------------------------------------------------------------------
ATCuriosity 4:2de56fc46abb 474
ATCuriosity 3:d03eae745223 475 // ------------------------------ ReadData ------------------------------
ATCuriosity 3:d03eae745223 476 Data ReadData()
Salamandre 0:a18d6e69c993 477 {
ATCuriosity 3:d03eae745223 478 Data data;
ATCuriosity 3:d03eae745223 479
ATCuriosity 3:d03eae745223 480 // Get Accelerometer data's
ATCuriosity 4:2de56fc46abb 481 Accel_Enable = 1; // Rising Edge -> Start measure
ATCuriosity 3:d03eae745223 482
ATCuriosity 4:2de56fc46abb 483 int ready = 0;
ATCuriosity 3:d03eae745223 484 while((ready && 0x10) == 0) // Wait for accelerometer to have new data's
ATCuriosity 3:d03eae745223 485 ready = my8491.Read_Status();
ATCuriosity 3:d03eae745223 486
ATCuriosity 3:d03eae745223 487 data.accX = my8491.getAccAxis(REG_OUT_X_MSB);
ATCuriosity 3:d03eae745223 488 data.accY = my8491.getAccAxis(REG_OUT_Y_MSB);
ATCuriosity 3:d03eae745223 489 data.accZ = my8491.getAccAxis(REG_OUT_Z_MSB);
Salamandre 0:a18d6e69c993 490
ATCuriosity 4:2de56fc46abb 491 Accel_Enable = 0;
Salamandre 0:a18d6e69c993 492
ATCuriosity 3:d03eae745223 493 // Get Piezo Data's
ATCuriosity 4:2de56fc46abb 494 data.Vout_IF = ((float) myPTE20.read_u16() / 0XFFFF) * KL25Z_VDD; // convert in volt
ATCuriosity 4:2de56fc46abb 495 data.Vout_FILT = ((float) myPTE21.read_u16() / 0XFFFF) * KL25Z_VDD;
ATCuriosity 4:2de56fc46abb 496 data.Vout_GAIN = ((float) myPTE22.read_u16() / 0XFFFF) * KL25Z_VDD;
ATCuriosity 3:d03eae745223 497
ATCuriosity 3:d03eae745223 498 return data;
ATCuriosity 3:d03eae745223 499 }
ATCuriosity 3:d03eae745223 500
ATCuriosity 3:d03eae745223 501 // -------------------------------- Log ---------------------------------
ATCuriosity 3:d03eae745223 502 void Log()
ATCuriosity 5:79b8cd191fa8 503 {
ATCuriosity 3:d03eae745223 504 Data currData;
ATCuriosity 3:d03eae745223 505 int16_t AccDataLog [N_PTS*3] = {}; // array to save latest data read
ATCuriosity 4:2de56fc46abb 506 int index_write = 0; // current position to write data in AccDataLog
ATCuriosity 4:2de56fc46abb 507 bool enoughData = 0;
ATCuriosity 4:2de56fc46abb 508 bool shockDetected = 0; // if shock detected
ATCuriosity 4:2de56fc46abb 509 int n_sinceShock = 0; // number of ReadData() since last chock
ATCuriosity 3:d03eae745223 510
ATCuriosity 5:79b8cd191fa8 511 while(Check_Jumper() && !foundError && !flashFull)
ATCuriosity 5:79b8cd191fa8 512 {
ATCuriosity 3:d03eae745223 513 while (bTimer == 0) {} // Wait Acq Tick Timer
Salamandre 0:a18d6e69c993 514 bTimer = 0;
Salamandre 0:a18d6e69c993 515
ATCuriosity 3:d03eae745223 516 currData = ReadData();
ATCuriosity 4:2de56fc46abb 517 //Host_Comm.printf("%d ; %d ; %d ; %f\n\r", currData.accX, currData.accY, currData.accZ, currData.Vout_FILT);
ATCuriosity 3:d03eae745223 518 AccDataLog[index_write*3] = currData.accX;
ATCuriosity 3:d03eae745223 519 AccDataLog[index_write*3+1] = currData.accY;
ATCuriosity 3:d03eae745223 520 AccDataLog[index_write*3+2] = currData.accZ;
Salamandre 0:a18d6e69c993 521
ATCuriosity 4:2de56fc46abb 522 float amplitude = abs(currData.Vout_FILT - KL25Z_VDD/2.0);
ATCuriosity 4:2de56fc46abb 523 //Host_Comm.printf("amplitude = %f\n\r",amplitude);
ATCuriosity 4:2de56fc46abb 524 if (amplitude >= THRESHOLD_SHOCK && enoughData)
Salamandre 0:a18d6e69c993 525 {
ATCuriosity 3:d03eae745223 526 shockDetected = 1;
ATCuriosity 3:d03eae745223 527 n_sinceShock = 0;
Salamandre 0:a18d6e69c993 528 }
ATCuriosity 3:d03eae745223 529 if (n_sinceShock == N_PTS/2 && shockDetected == 1)
Salamandre 0:a18d6e69c993 530 {
ATCuriosity 5:79b8cd191fa8 531 Led_Green = LED_ON;
ATCuriosity 5:79b8cd191fa8 532
ATCuriosity 4:2de56fc46abb 533 MvtSet mvtSet;
ATCuriosity 4:2de56fc46abb 534 Rotate(AccDataLog, N_PTS-1-index_write, mvtSet.inputs);
ATCuriosity 4:2de56fc46abb 535 mvtSet.mvt = SelectMvt(mvtSet.inputs);
ATCuriosity 5:79b8cd191fa8 536
ATCuriosity 4:2de56fc46abb 537 if (mode == CONSOLE)
ATCuriosity 5:79b8cd191fa8 538 PrintSet(mvtSet);
ATCuriosity 4:2de56fc46abb 539 else
ATCuriosity 4:2de56fc46abb 540 WriteFlash(mvtSet);
ATCuriosity 5:79b8cd191fa8 541
ATCuriosity 3:d03eae745223 542 shockDetected = 0;
ATCuriosity 5:79b8cd191fa8 543 wait_ms(100);
ATCuriosity 5:79b8cd191fa8 544 Led_Green = LED_OFF;
Salamandre 0:a18d6e69c993 545 }
ATCuriosity 4:2de56fc46abb 546
ATCuriosity 3:d03eae745223 547 index_write ++;
ATCuriosity 3:d03eae745223 548 n_sinceShock ++;
ATCuriosity 4:2de56fc46abb 549 if (index_write == N_PTS)
ATCuriosity 4:2de56fc46abb 550 {
ATCuriosity 4:2de56fc46abb 551 enoughData = 1;
ATCuriosity 3:d03eae745223 552 index_write = 0;
ATCuriosity 4:2de56fc46abb 553 }
Salamandre 0:a18d6e69c993 554 }
ATCuriosity 3:d03eae745223 555 Clear_Led();
ATCuriosity 3:d03eae745223 556 }
ATCuriosity 3:d03eae745223 557
ATCuriosity 4:2de56fc46abb 558 // ------------------------------- Rotate -------------------------------
ATCuriosity 4:2de56fc46abb 559 void Rotate(int16_t *AccDataLog, int amount, int16_t *inputs)
ATCuriosity 3:d03eae745223 560 {
ATCuriosity 4:2de56fc46abb 561 for(int i = 0; i < N_PTS; i++)
ATCuriosity 4:2de56fc46abb 562 for(int j = 0; j < 3; j++)
ATCuriosity 4:2de56fc46abb 563 inputs[((i+amount)%N_PTS)*3+j] = AccDataLog[i*3+j];
Salamandre 0:a18d6e69c993 564 }
Salamandre 0:a18d6e69c993 565
ATCuriosity 4:2de56fc46abb 566 // ------------------------------ PrintSet ------------------------------
ATCuriosity 5:79b8cd191fa8 567 void PrintSet(MvtSet mvtSet)
ATCuriosity 3:d03eae745223 568 {
ATCuriosity 4:2de56fc46abb 569 Host_Comm.printf("------ Begin Set ------\n\r");
ATCuriosity 4:2de56fc46abb 570 for(int i = 0; i < N_PTS; i++)
ATCuriosity 5:79b8cd191fa8 571 Host_Comm.printf("%d %d %d\n\r",mvtSet.inputs[i*3],mvtSet.inputs[i*3+1],mvtSet.inputs[i*3+2]);
ATCuriosity 5:79b8cd191fa8 572 Host_Comm.printf("Mvt = %d\n\r",mvtSet.mvt);
ATCuriosity 4:2de56fc46abb 573 Host_Comm.printf("------- End Set -------\n\n\r");
ATCuriosity 3:d03eae745223 574 }
Salamandre 0:a18d6e69c993 575
ATCuriosity 6:4ea2ba88338f 576 // -------------------------------------------------------------------------------------------------------
ATCuriosity 6:4ea2ba88338f 577 // -------------------------------------------------------------------------------------------------------
ATCuriosity 6:4ea2ba88338f 578
ATCuriosity 6:4ea2ba88338f 579 // -------------------------------- Map ---------------------------------
ATCuriosity 6:4ea2ba88338f 580 float Map (float x) { return (x+OFFSET)/RANGE; }
ATCuriosity 6:4ea2ba88338f 581
ATCuriosity 6:4ea2ba88338f 582 // ------------------------------ Sigmoid -------------------------------
ATCuriosity 6:4ea2ba88338f 583 float Sigmoid (float x) { return 1/(1+exp(-x)); }
ATCuriosity 6:4ea2ba88338f 584
ATCuriosity 5:79b8cd191fa8 585 // ------------------------------ Softmax -------------------------------
ATCuriosity 5:79b8cd191fa8 586 void Softmax (float *inputs, float *result)
ATCuriosity 5:79b8cd191fa8 587 {
ATCuriosity 5:79b8cd191fa8 588 float exps [N_MVTS];
ATCuriosity 5:79b8cd191fa8 589 float sum = 0;
ATCuriosity 5:79b8cd191fa8 590 for (int i = 0; i < N_MVTS; i++)
ATCuriosity 5:79b8cd191fa8 591 {
ATCuriosity 5:79b8cd191fa8 592 exps[i] = exp(inputs[i]);
ATCuriosity 5:79b8cd191fa8 593 sum += exps[i];
ATCuriosity 5:79b8cd191fa8 594 }
ATCuriosity 5:79b8cd191fa8 595
ATCuriosity 5:79b8cd191fa8 596 for (int i = 0; i < N_MVTS; i++)
ATCuriosity 5:79b8cd191fa8 597 result[i] = exps[i] / sum;
ATCuriosity 5:79b8cd191fa8 598 }
Salamandre 0:a18d6e69c993 599
ATCuriosity 3:d03eae745223 600 // ----------------------------- SelectMvt ------------------------------
ATCuriosity 4:2de56fc46abb 601 Mvt SelectMvt(int16_t *inputs)
ATCuriosity 3:d03eae745223 602 {
ATCuriosity 3:d03eae745223 603 int i, j;
ATCuriosity 6:4ea2ba88338f 604
ATCuriosity 6:4ea2ba88338f 605 float samples [N_PTS];
ATCuriosity 6:4ea2ba88338f 606 for (i = 0; i < N_PTS; i++)
ATCuriosity 6:4ea2ba88338f 607 samples[i] = Sigmoid( Map(inputs[i*3])*w_s_l[i*3] + Map(inputs[i*3+1])*w_s_l[i*3+1] + Map(inputs[i*3+2])*w_s_l[i*3+2] + b_s_l[i] );
ATCuriosity 6:4ea2ba88338f 608
ATCuriosity 6:4ea2ba88338f 609 float probabilities [N_MVTS] = {};
ATCuriosity 3:d03eae745223 610 for (j = 0; j < N_MVTS; j++) {
ATCuriosity 6:4ea2ba88338f 611 for (i = 0; i < N_PTS; i++)
ATCuriosity 6:4ea2ba88338f 612 probabilities[j] += samples[i] * w_o_l[i*N_MVTS+j];
ATCuriosity 6:4ea2ba88338f 613 probabilities[j] = probabilities[j] + b_o_l[j];
ATCuriosity 3:d03eae745223 614 }
ATCuriosity 6:4ea2ba88338f 615 Softmax(probabilities,probabilities);
ATCuriosity 3:d03eae745223 616
ATCuriosity 4:2de56fc46abb 617 Mvt mvt = Undefined;
ATCuriosity 5:79b8cd191fa8 618 Host_Comm.printf("Proba mvt : ");
ATCuriosity 4:2de56fc46abb 619 for (i = 0; i < N_MVTS; i++) {
ATCuriosity 6:4ea2ba88338f 620 if (probabilities[i] > THRESHOLD_MVT)
ATCuriosity 4:2de56fc46abb 621 mvt = static_cast<Mvt>(i+1);
ATCuriosity 6:4ea2ba88338f 622 Host_Comm.printf("%f ",probabilities[i]);
ATCuriosity 4:2de56fc46abb 623 }
ATCuriosity 5:79b8cd191fa8 624 Host_Comm.printf("\n\r");
ATCuriosity 3:d03eae745223 625
ATCuriosity 3:d03eae745223 626 return mvt;
Salamandre 0:a18d6e69c993 627 }