Hexiwear based rate of exercise coach
Dependencies: FXOS8700 Hexi_OLED_SSD1351
Fork of Hexi_Accelero_Magneto_Example by
main.cpp
- Committer:
- tom_minnich
- Date:
- 2016-10-02
- Revision:
- 4:adea7bcbd995
- Parent:
- 3:9f60cb7455c4
File content as of revision 4:adea7bcbd995:
#include "mbed.h" #include "FXOS8700.h" #include "Hexi_OLED_SSD1351.h" #include "images.h" // Check out the full featured example application for interfacing to the // Accelerometer/Magnetometer device at the following URL // https://developer.mbed.org/users/trm/code/fxos8700cq_example/ Timer acc_timer; PwmOut rled(LED_RED); PwmOut gled(LED_GREEN); PwmOut bled(LED_BLUE); #define AVG_ARRAY_SZ 32 //TWM added stuff int last_image = 4; int update_image; float avg_array[AVG_ARRAY_SZ]; int read_from = 0; int read; int write_to = 0; int items_stored_in_array = 0; float avg; #define ACC_DEADBAND ((float)0.2) #define FILTER_MIN_STEPS 7 int filter_count=0; unsigned char going_up = 0; unsigned char going_down = 0; //rep counting stuff #define MAX_REP_DURATION 15000000 //tenths of a second 15,000,000 = 15 sec #define TARGET_REP ((float)(2.0)) #define TOO_FAST 1.5 #define TOO_SLOW 2.5 float total_rep; float below_g_rep; float above_g_rep; unsigned int time_reg=0; unsigned int stop_time_reg = 0; // for calc delta time unsigned int delta_time_reg; unsigned int time_below_g_inst = 0; unsigned int time_above_g_inst = 0; unsigned int time_below_g=0; //UP green led below 1 G unsigned int time_above_g=0; // DOWN red led above 1 G unsigned int time_below_g_cycles=0; unsigned int time_above_g_cycles=0; //total_rep moving average #define REP_AVG_ARRAY_SZ 10 //TWM added stuff float rep_avg_array[REP_AVG_ARRAY_SZ]; int rep_read_from = 0; int rep_read; int rep_write_to = 0; int rep_items_stored_in_array = 0; float rep_avg; static const float greenx=sqrt(3.0f)/2.0f; static const float bluex=-sqrt(3.0f)/2.0f; void paint_leds(void){ float led_temp; float OLED_temp; // 1 LED off 0 LED ON led_temp = (TARGET_REP - rep_avg)*(float)0.5 + (float)0.5; if (led_temp > (float)1.0) { led_temp = 1.0; }else if(led_temp<0){ led_temp = 0; } gled = led_temp; led_temp = (float)1.0 - led_temp; rled = led_temp; OLED_temp = led_temp*(float)9.0; update_image = (int) OLED_temp; if (update_image > 8) { update_image = 8; }else if(update_image<0){ update_image = 0; } } void average_total_reps(void){ int i; rep_avg_array[rep_write_to++] = ((float)(time_above_g_inst+time_below_g_inst))/(float)1000.0; if(rep_write_to >= REP_AVG_ARRAY_SZ){ rep_write_to = 0; } if(rep_items_stored_in_array < REP_AVG_ARRAY_SZ){ rep_items_stored_in_array++; }else{ rep_read_from++; if(rep_read_from >= REP_AVG_ARRAY_SZ){ rep_read_from = 0; } } rep_avg = 0; rep_read = rep_read_from; for(i=0; i<rep_items_stored_in_array; i++){ rep_avg += rep_avg_array[rep_read++]; if(rep_read >= REP_AVG_ARRAY_SZ){ rep_read = 0; } } rep_avg = rep_avg/rep_items_stored_in_array; } //DigitalOut led1(LED_GREEN); // Initialize Serial port Serial pc(USBTX, USBRX); // Pin connections & address for Hexiwear FXOS8700 accel(PTC11, PTC10); FXOS8700 mag(PTC11, PTC10); // main() runs in its own thread in the OS // (note the calls to Thread::wait below for delays) int main() { /* Pointer for the images to be displayed */ const uint8_t *image[9]; /* Setting pointer location of the 96 by 96 pixel bitmap */ image[0] = red0_bmp; image[1] = orn9_bmp; image[2] = ltorn18_bmp; image[3] = egg26_bmp; image[4] = yel36_bmp; image[5] = ltyel44_bmp; image[6] = ltgrn53_bmp; image[7] = lime71_bmp; image[8] = grn80_bmp; /* Instantiate the SSD1351 OLED Driver */ SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15); // (MOSI,SCLK,POWER,CS,RST,DC) /* Turn on the backlight of the OLED Display */ oled.DimScreenON(); oled.DrawImage(image[last_image],0,0); //last_image = 4 at start //TWM REP Hexiwear Stuff // 1ms pwm frequency rled.period(0.001f); gled.period(0.001f); bled.period(0.001f); acc_timer.start(); // Configure Accelerometer FXOS8700, Magnetometer FXOS8700 accel.accel_config(); mag.mag_config(); float accel_data[3]; #ifdef USE_MAG_DATA float mag_data[3]; float mag_rms=0.0; #endif printf("Begin Data Acquisition from FXOS8700CQ sensor....\r\n\r\n"); wait(0.5); while (1) { int i; float xaccln,yaccln,zaccln; accel.acquire_accel_data_g(accel_data); xaccln = accel_data[0]; // using the same named variables from the old REP software yaccln = accel_data[1]; zaccln = accel_data[2]; time_reg = acc_timer.read_us(); float norm=sqrt(xaccln*xaccln + yaccln*yaccln +zaccln*zaccln); avg_array[write_to++] = norm; if(write_to>= AVG_ARRAY_SZ){ write_to = 0; } if(items_stored_in_array < AVG_ARRAY_SZ){ items_stored_in_array++; }else{ read_from++; if(read_from >= AVG_ARRAY_SZ){ read_from = 0; } } avg = 0; read = read_from; for(i=0; i<items_stored_in_array; i++){ avg += avg_array[read++]; if(read >= AVG_ARRAY_SZ){ read = 0; } } avg = avg/items_stored_in_array; bled = 1; if (fabs(avg-norm) > ACC_DEADBAND){ if((avg-norm)>0){ // avg-norm a positive number // going downward from the top of a rep if(!going_down) { //if there is a change in direction calc rep duration delta_time_reg = time_reg - stop_time_reg; stop_time_reg = time_reg; // store away the next stop time if(delta_time_reg >MAX_REP_DURATION){ total_rep = 0.0; below_g_rep = 0.0; above_g_rep = 0.0; delta_time_reg = 0; time_below_g_inst = 0; time_above_g_inst = 0; time_below_g=0; //UP green led below 1 G time_above_g=0; // DOWN red led above 1 G time_below_g_cycles=0; time_above_g_cycles=0; }else{ time_above_g_inst = delta_time_reg/1000; time_above_g += time_above_g_inst; time_above_g_cycles++; above_g_rep = (float)(time_above_g/time_above_g_cycles);//track in seconds above_g_rep /= 1000; total_rep = above_g_rep + below_g_rep; average_total_reps(); paint_leds(); if(last_image != update_image){ last_image = update_image; oled.DrawImage(image[last_image],0,0); } // bt.printf("U"); //if(bt.writeable()){ //printf("Bluetooth writeable! \r\n"); //bt.putc('U'); //printf("Back from bluetooth comm! \r\n"); //} // bt.printf("\x1B[JREP #%d\r\n",time_above_g_cycles); printf("\x1B[KREP #%d\r\n",time_above_g_cycles); // bt.printf("Raising rep: %d\r\n",delta_time_reg/1000); // bt.printf("AVG Raising rep: %1.2f \r\n",above_g_rep); printf("\x1B[KAccumulated Average Raising Rep: %1.2f \r\n",above_g_rep); // bt.printf("rep_avg: %1.2f \r\n",rep_avg); } going_down = 1; going_up = 0; //rled = 0; //gled = 1; } }else{ //avg-norm a negative number //accelerating upward from the bottom of a rep if(!going_up) { //if there is a change in direction calc rep duration delta_time_reg = time_reg - stop_time_reg; stop_time_reg = time_reg; // store away the next stop time if(delta_time_reg >MAX_REP_DURATION){ total_rep = 0.0; below_g_rep = 0.0; above_g_rep = 0.0; delta_time_reg = 0; time_below_g_inst = 0; time_above_g_inst = 0; time_below_g=0; //UP green led below 1 G time_above_g=0; // DOWN red led above 1 G time_below_g_cycles=0; time_above_g_cycles=0; }else{ time_below_g_inst = delta_time_reg/1000; time_below_g += time_below_g_inst; // track in mS time_below_g_cycles++; below_g_rep = (float)(time_below_g/time_below_g_cycles); //track in seconds below_g_rep /= 1000; total_rep = above_g_rep + below_g_rep; average_total_reps(); paint_leds(); if(last_image != update_image){ last_image = update_image; oled.DrawImage(image[last_image],0,0); } // bt.printf("Lowering rep: %d\r\n",delta_time_reg/1000); // bt.printf("AVG Lowering rep: %1.2f\x1B[H",below_g_rep); printf("\x1B[KAccumulated Average Lowering Rep: %1.2f\r\n",below_g_rep); printf("\x1B[KRecent Activity Rep Average: %1.2f Target is 2.00\x1B[J\x1B[H",rep_avg); } going_down = 0; going_up = 1; //rled = 1; //gled = 0; } }// endif direction up or down }// endif deadband // printf("X: %1.4f, Y: %1.4f, Z: %1.4f,avg-norm %1.4f Norm: %1.4f \r\n", // xaccln, yaccln, zaccln,(avg-norm), // norm); // printf("%d\r\n",time_reg); // TWM using thread wait below instead of this wait function!!! // wait(0.1f); // led1 = !led1; // Example data printing // accel_rms = sqrt(((accel_data[0]*accel_data[0])+(accel_data[1]*accel_data[1])+(accel_data[2]*accel_data[2]))/3); // printf("Accelerometer \tX-Axis %4.2f \tY-Axis %4.2f \tZ-Axis %4.2f \tRMS %4.2f\n\r",accel_data[0],accel_data[1],accel_data[2],accel_rms); // wait(0.01); #ifdef USE_MAG_DATA mag.acquire_mag_data_uT(mag_data); mag_rms = sqrt(((mag_data[0]*mag_data[0])+(mag_data[1]*mag_data[1])+(mag_data[2]*mag_data[2]))/3); printf("Magnetometer \tX-Axis %4.2f \tY-Axis %4.2f \tZ-Axis %4.2f \tRMS %4.2f\n\n\r",mag_data[0],mag_data[1],mag_data[2],mag_rms); wait(0.01); #endif Thread::wait(100); // Thread::wait(500); } }