Mari Mishel
/
TAU_Studets_Motion_IKS01A1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 13:f4bb537c9440, committed 2017-08-08
- Comitter:
- HolyMari
- Date:
- Tue Aug 08 16:34:58 2017 +0000
- Parent:
- 12:73fd70c4e857
- Commit message:
- sliding
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun May 21 13:01:32 2017 +0000
+++ b/main.cpp Tue Aug 08 16:34:58 2017 +0000
@@ -36,12 +36,27 @@
******************************************************************************
*/
+/**************************************************************
+*************** THIS HANDLES SLIDING WINDOW *******************
+***************************************************************/
/* Includes */
#include "mbed.h"
#include "x_nucleo_iks01a1.h"
// define sample frequency
-#define SAMPLES_DELAY 5000
+#define SAMPLES_DELAY 5000.f
+#define ARR_LEN 500
+#define MG2MM 100.f
+
+// thresholds
+#define EVENT_THRESHOLD 5.f //5
+#define MOVEMENT_THRESHOLD 5.f
+#define VEL_THRESHOLD 10.f
+#define LOC_THRESHOLD 1.f
+#define STILL_THRESHOLD 2.f
+#define OPEN_THRESHOLD 5.f
+#define CLOSE_THRESHOLD 3.f
+
/* Instantiate the expansion board */
static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
@@ -60,27 +75,92 @@
// send data
bool flagData=0;
+float sum_v = 0, sum_s = 0;
+float bias[3];
+int axis = 0;
// time stamp
int timeStamp_us=0;
int sampleStamp=0;
+
+float integrate(float sample, float prev_sum) {
+ float ret;
+ ret = prev_sum + sample;
+ return ret;
+}
+
+
+bool check_event_start(float* samples, int i) {
+ sum_v = 0;
+ sum_s = 0;
+ float effective_s;
+ for (int j=1; j <= ARR_LEN; j++){
+ sum_v = integrate(samples[ (i + j) % ARR_LEN] - bias[axis], sum_v);
+ pc.printf("sum_vvvvv: %f, \r\n", sum_v);
+ float effective_vel = abs(sum_v) >= VEL_THRESHOLD ? sum_v : 0;
+ pc.printf("effective_vvvvv: %f, \r\n", effective_vel);
+ sum_s = integrate(effective_vel, sum_s);
+ effective_s = abs(sum_s) >= LOC_THRESHOLD ? sum_s : 0;
+ //effective_s = sum_s;
+
+ }
+ effective_s = effective_s / 1000;
+ pc.printf("effective_LOC: %f, \r\n", effective_s);
+ if (abs(effective_s) > MOVEMENT_THRESHOLD) {
+ // pc.printf("effective_LOC: %f, \r\n", effective_s);
+ // (1){}
+ return true;
+ }
+ return false;
+}
+
+
+/*bool check_event_end(float* samples, int i) {
+ float temp_sum_v = 0, temp_sum_s = 0;
+ for (uint8_t j=1; j <= ARR_LEN; j++){
+ temp_sum_v = integrate(samples[ (i + j) % ARR_LEN] - bias[axis], temp_sum_v);
+ temp_sum_s = integrate(temp_sum_v, temp_sum_s);
+ }
+
+ if (temp_sum_s < STILL_THRESHOLD) {
+ return false;
+ }
+ return true;
+}*/
+
/* Simple main function */
int main()
{
pc.baud(921600);
timer.start();
timeStamp_us = timer.read_us();
+ int i = 0,j;
uint8_t id;
int32_t Acc_axes[3];
int32_t Gyro_axes[3];
+ float diff;
+ float samples[3][ARR_LEN];
+ bool event_flag = false;
+ for(j=0; j < ARR_LEN; j++){
+ accelerometer->get_x_axes(Acc_axes);
+ gyroscope->get_g_axes(Gyro_axes);
+ //pc.printf("%7ld,%7ld,%7ld,%7ld,%7ld,%7ld,%7ld\r\n", sampleStamp, Acc_axes[0], Acc_axes[1], Acc_axes[2], Gyro_axes[0], Gyro_axes[1], Gyro_axes[2]);
+ samples[0][j % ARR_LEN] = Acc_axes[0] / MG2MM;
+ samples[1][j % ARR_LEN] = Acc_axes[1] / MG2MM;
+ samples[2][j % ARR_LEN] = Acc_axes[2] / MG2MM;
+ bias[0] += (samples[0][j % ARR_LEN] / ARR_LEN);
+ bias[1] += (samples[1][j % ARR_LEN] / ARR_LEN);
+ bias[2] += (samples[2][j % ARR_LEN] / ARR_LEN);
+ }
pc.printf("\r\n--- Starting new run ---\r\n");
gyroscope->read_id(&id);
pc.printf("LSM6DS0 accelerometer & gyroscope = 0x%X\r\n", id);
+ pc.printf("bias: %f, %f, %f\r\n", bias[0], bias[1], bias[2]);
- wait(0.5);
+ //wait(0.5); //????????????????????????????????????????????????????????????????????????????????????????
while(1) {
// get time stamp
@@ -91,11 +171,51 @@
// get raw data
accelerometer->get_x_axes(Acc_axes);
gyroscope->get_g_axes(Gyro_axes);
- pc.printf("D:%7ld,%7ld,%7ld,%7ld,%7ld,%7ld,%7ld\r\n", sampleStamp, Acc_axes[0], Acc_axes[1], Acc_axes[2], Gyro_axes[0], Gyro_axes[1], Gyro_axes[2]);
- //pc.printf("D:%7ld,%7ld,%7ld,%7ld\r\n", sampleStamp, Acc_axes[0], Acc_axes[1], Acc_axes[2]);
- // stability delay to finish sending data
- //wait_us(10);
+ //pc.printf("%7ld,%7ld,%7ld,%7ld,%7ld,%7ld,%7ld\r\n", sampleStamp, Acc_axes[0], Acc_axes[1], Acc_axes[2], Gyro_axes[0], Gyro_axes[1], Gyro_axes[2]);
+ samples[0][i % ARR_LEN] = Acc_axes[0] / MG2MM;
+ samples[1][i % ARR_LEN] = Acc_axes[1] / MG2MM;
+ samples[2][i % ARR_LEN] = Acc_axes[2] / MG2MM;
+
+ diff = abs(samples[axis][i % ARR_LEN] - samples[axis][(i - (ARR_LEN / 2)) % ARR_LEN]);
+ //pc.printf("diff: %f\r\n", diff);
+ if (diff > EVENT_THRESHOLD && !event_flag) {
+ //pc.printf("suspected event: ");
+ event_flag = check_event_start(samples[axis], i);
+ if(event_flag) {
+ pc.printf("OPENED: sumv= %f sums=%f\r\n", sum_v,sum_s/1000);
+ //while (1){}
+ }
+ /*else {
+ pc.printf("False sum_s: %f\r\n", sum_s);
+ }*/
+ //event_flag = false
+ }
+ else if (event_flag){
+ sum_v = integrate(samples[axis][i % ARR_LEN] - bias[axis], sum_v);
+ // pc.printf("sum_v: %f, \r\n", sum_v);
+ float effective_vel = abs(sum_v) >= VEL_THRESHOLD ? sum_v : 0;
+ //sum_v = abs(sum_v) < VEL_THRESHOLD ? 0 : sum_v;
+ pc.printf("effective_v: %f, \r\n", effective_vel);
+ sum_s = integrate(effective_vel, sum_s);
+ float effective_s = abs(sum_s) >= LOC_THRESHOLD ? sum_s : 0;
+ //effective_s = sum_s;
+ pc.printf("sum_v: %f, sum_s: %f\r\n", sum_v, sum_s);
+ effective_s = effective_s /1000;
+ if (abs(effective_s) <= CLOSE_THRESHOLD){
+ pc.printf("CLOSED: sumv = %f sums= %f \r\n", sum_v,effective_s);
+ event_flag = false;
+ sum_s = 0;
+ sum_v = 0;
+ }
+ }
+ i++;
}// end get & send samples
-
}// end loop
}// end main
+
+
+
+//TODO list:
+// compute event movement
+// find event ending
+// display 0,1
\ No newline at end of file