main.cpp

00001 #include "mbed.h"
00002 #include "FXOS8700.h"
00003 #include "Hexi_OLED_SSD1351.h"
00004 #include "images.h"
00005 //  Check out the full featured example application for interfacing to the 
00006 //  Accelerometer/Magnetometer device at the following URL
00007 //  https://developer.mbed.org/users/trm/code/fxos8700cq_example/
00008 
00009 Timer acc_timer;
00010 PwmOut rled(LED_RED);
00011 PwmOut gled(LED_GREEN);
00012 PwmOut bled(LED_BLUE);
00013 
00014 #define AVG_ARRAY_SZ 32
00015 //TWM added stuff
00016 int last_image = 4;
00017 int update_image;
00018 float avg_array[AVG_ARRAY_SZ];
00019 int read_from = 0;
00020 int read;
00021 int write_to = 0;
00022 int items_stored_in_array = 0;
00023 float avg;
00024 #define ACC_DEADBAND  ((float)0.2)
00025 #define FILTER_MIN_STEPS 7
00026 int filter_count=0;
00027 unsigned char going_up = 0;
00028 unsigned char going_down = 0;
00029 //rep counting stuff
00030 #define MAX_REP_DURATION 15000000 //tenths of a second 15,000,000 = 15 sec
00031 #define TARGET_REP       ((float)(2.0))
00032 #define TOO_FAST         1.5
00033 #define TOO_SLOW         2.5
00034  float total_rep;
00035  float below_g_rep;
00036  float above_g_rep;
00037  unsigned int time_reg=0;
00038  unsigned int stop_time_reg = 0; // for calc delta time
00039  unsigned int delta_time_reg;
00040  unsigned int time_below_g_inst = 0;
00041  unsigned int time_above_g_inst = 0;
00042  unsigned int time_below_g=0; //UP green led below 1 G
00043  unsigned int time_above_g=0; // DOWN red led above 1 G 
00044  unsigned int time_below_g_cycles=0;
00045  unsigned int time_above_g_cycles=0;
00046  //total_rep moving average
00047 #define REP_AVG_ARRAY_SZ 10
00048 //TWM added stuff
00049 float rep_avg_array[REP_AVG_ARRAY_SZ];
00050 int rep_read_from = 0;
00051 int rep_read;
00052 int rep_write_to = 0;
00053 int rep_items_stored_in_array = 0;
00054 float rep_avg;
00055 static const float greenx=sqrt(3.0f)/2.0f;
00056 static const float bluex=-sqrt(3.0f)/2.0f;
00057 
00058 void paint_leds(void){
00059     float led_temp;
00060     float OLED_temp;
00061     // 1 LED off 0 LED ON
00062     led_temp =  (TARGET_REP - rep_avg)*(float)0.5 + (float)0.5;
00063     if (led_temp > (float)1.0) {
00064         led_temp = 1.0;
00065     }else if(led_temp<0){
00066         led_temp = 0;
00067     }
00068     gled = led_temp;
00069     led_temp = (float)1.0 - led_temp;
00070     rled = led_temp;
00071     OLED_temp = led_temp*(float)9.0;
00072     update_image = (int) OLED_temp;
00073     if (update_image > 8) {
00074         update_image = 8;
00075     }else if(update_image<0){
00076         update_image = 0;
00077     }
00078 }
00079 void average_total_reps(void){
00080     int i;      
00081     rep_avg_array[rep_write_to++] = ((float)(time_above_g_inst+time_below_g_inst))/(float)1000.0;
00082     if(rep_write_to >= REP_AVG_ARRAY_SZ){
00083         rep_write_to = 0;
00084     }
00085     if(rep_items_stored_in_array < REP_AVG_ARRAY_SZ){
00086         rep_items_stored_in_array++;
00087     }else{
00088         rep_read_from++;
00089         if(rep_read_from >= REP_AVG_ARRAY_SZ){
00090             rep_read_from = 0;
00091         }
00092     }
00093    
00094     rep_avg = 0;
00095     rep_read = rep_read_from;
00096 
00097     for(i=0; i<rep_items_stored_in_array; i++){
00098         rep_avg += rep_avg_array[rep_read++];
00099         if(rep_read >= REP_AVG_ARRAY_SZ){
00100             rep_read = 0;
00101         }
00102     }
00103     rep_avg = rep_avg/rep_items_stored_in_array;
00104 }
00105 
00106 //DigitalOut led1(LED_GREEN);
00107 
00108 // Initialize Serial port
00109 Serial pc(USBTX, USBRX);
00110 
00111 // Pin connections & address for Hexiwear
00112 FXOS8700 accel(PTC11, PTC10);
00113 FXOS8700 mag(PTC11, PTC10);
00114 
00115 // main() runs in its own thread in the OS
00116 // (note the calls to Thread::wait below for delays)
00117 int main() {
00118     /* Pointer for the images to be displayed  */  
00119     const uint8_t *image[9];
00120 
00121     /* Setting pointer location of the 96 by 96 pixel bitmap */
00122     image[0] = red0_bmp;
00123     image[1] = orn9_bmp;
00124     image[2] = ltorn18_bmp;
00125     image[3] = egg26_bmp;
00126     image[4] = yel36_bmp;
00127     image[5] = ltyel44_bmp;
00128     image[6] = ltgrn53_bmp;
00129     image[7] = lime71_bmp;
00130     image[8] = grn80_bmp;
00131     
00132     
00133     /* Instantiate the SSD1351 OLED Driver */
00134     SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15); // (MOSI,SCLK,POWER,CS,RST,DC)
00135     
00136     /* Turn on the backlight of the OLED Display */
00137     oled.DimScreenON();
00138     oled.DrawImage(image[last_image],0,0); //last_image = 4 at start
00139     
00140     //TWM REP Hexiwear Stuff
00141     // 1ms pwm frequency
00142     rled.period(0.001f);
00143     gled.period(0.001f);
00144     bled.period(0.001f);
00145     acc_timer.start();
00146     
00147     // Configure Accelerometer FXOS8700, Magnetometer FXOS8700
00148     accel.accel_config();
00149     mag.mag_config();
00150 
00151     float accel_data[3];
00152 #ifdef USE_MAG_DATA
00153     float mag_data[3];   float mag_rms=0.0;
00154 #endif    
00155 
00156     printf("Begin Data Acquisition from FXOS8700CQ sensor....\r\n\r\n");
00157     wait(0.5);
00158     
00159     while (1) {
00160     int i;      
00161     float xaccln,yaccln,zaccln;
00162         
00163     accel.acquire_accel_data_g(accel_data);
00164     xaccln = accel_data[0];  // using the same named variables from the old REP software
00165     yaccln = accel_data[1];
00166     zaccln = accel_data[2];
00167     time_reg = acc_timer.read_us();
00168     float norm=sqrt(xaccln*xaccln + yaccln*yaccln +zaccln*zaccln);
00169     avg_array[write_to++] = norm;
00170     if(write_to>= AVG_ARRAY_SZ){
00171         write_to = 0;
00172     }
00173     if(items_stored_in_array < AVG_ARRAY_SZ){
00174         items_stored_in_array++;
00175     }else{
00176         read_from++;
00177         if(read_from >= AVG_ARRAY_SZ){
00178             read_from = 0;
00179         }
00180     }
00181    
00182     avg = 0;
00183     read = read_from;
00184 
00185     for(i=0; i<items_stored_in_array; i++){
00186         avg += avg_array[read++];
00187         if(read >= AVG_ARRAY_SZ){
00188             read = 0;
00189         }
00190     }
00191     
00192     avg = avg/items_stored_in_array;
00193     
00194     bled = 1;
00195     if (fabs(avg-norm) > ACC_DEADBAND){
00196     if((avg-norm)>0){
00197 // avg-norm a positive number
00198 //  going downward from the top of a rep
00199         if(!going_down)
00200         { //if there is a change in direction calc rep duration
00201             delta_time_reg = time_reg - stop_time_reg;            
00202             stop_time_reg = time_reg; // store away the next stop time
00203             if(delta_time_reg >MAX_REP_DURATION){
00204                 total_rep  = 0.0;
00205                 below_g_rep = 0.0;
00206                 above_g_rep = 0.0;
00207                 delta_time_reg = 0;
00208                 time_below_g_inst = 0;
00209                 time_above_g_inst = 0;
00210                 time_below_g=0; //UP green led below 1 G
00211                 time_above_g=0; // DOWN red led above 1 G 
00212                 time_below_g_cycles=0;
00213                 time_above_g_cycles=0;
00214             }else{
00215                 time_above_g_inst = delta_time_reg/1000;                
00216                 time_above_g += time_above_g_inst;
00217                 time_above_g_cycles++;
00218                 above_g_rep = (float)(time_above_g/time_above_g_cycles);//track in seconds 
00219                 above_g_rep /= 1000;
00220                 total_rep = above_g_rep + below_g_rep;
00221                 average_total_reps();
00222                 paint_leds();
00223                 if(last_image != update_image){
00224                     last_image = update_image;
00225                     oled.DrawImage(image[last_image],0,0);
00226                 }  
00227                 
00228 //              bt.printf("U");
00229                 //if(bt.writeable()){
00230                     //printf("Bluetooth writeable! \r\n");
00231                     //bt.putc('U');
00232                     //printf("Back from bluetooth comm! \r\n");
00233                 //}
00234     //          bt.printf("\x1B[JREP #%d\r\n",time_above_g_cycles);
00235                 printf("\x1B[KREP #%d\r\n",time_above_g_cycles);
00236       //        bt.printf("Raising rep: %d\r\n",delta_time_reg/1000);
00237     //          bt.printf("AVG Raising rep: %1.2f \r\n",above_g_rep);
00238                 printf("\x1B[KAccumulated Average Raising Rep:  %1.2f \r\n",above_g_rep);
00239        //           bt.printf("rep_avg: %1.2f \r\n",rep_avg);           
00240             }       
00241             going_down = 1;
00242             going_up = 0;
00243             //rled = 0;
00244             //gled = 1;                                                                     
00245         }
00246     }else{
00247 //avg-norm a negative number
00248 //accelerating upward from the bottom of a rep      
00249         if(!going_up)
00250         { //if there is a change in direction calc rep duration
00251             delta_time_reg = time_reg - stop_time_reg;
00252             stop_time_reg = time_reg; // store away the next stop time              
00253             if(delta_time_reg >MAX_REP_DURATION){       
00254                 total_rep  = 0.0;
00255                 below_g_rep = 0.0;
00256                 above_g_rep = 0.0;
00257                 delta_time_reg = 0;
00258                 time_below_g_inst = 0;
00259                 time_above_g_inst = 0;          
00260                 time_below_g=0; //UP green led below 1 G
00261                 time_above_g=0; // DOWN red led above 1 G 
00262                 time_below_g_cycles=0;
00263                 time_above_g_cycles=0;      
00264             }else{
00265                 time_below_g_inst = delta_time_reg/1000;
00266                 time_below_g += time_below_g_inst; // track in mS
00267                 time_below_g_cycles++;
00268                 below_g_rep = (float)(time_below_g/time_below_g_cycles); //track in seconds
00269                 below_g_rep /= 1000;
00270                 total_rep = above_g_rep + below_g_rep;
00271                 average_total_reps();
00272                 paint_leds();
00273                 if(last_image != update_image){
00274                     last_image = update_image;
00275                     oled.DrawImage(image[last_image],0,0);
00276                 }  
00277                 
00278         //        bt.printf("Lowering rep: %d\r\n",delta_time_reg/1000);            
00279         //      bt.printf("AVG Lowering rep: %1.2f\x1B[H",below_g_rep);
00280         
00281                 printf("\x1B[KAccumulated Average Lowering Rep: %1.2f\r\n",below_g_rep);
00282                 printf("\x1B[KRecent Activity Rep Average:      %1.2f Target is 2.00\x1B[J\x1B[H",rep_avg);                 
00283             }
00284             going_down = 0;
00285             going_up = 1;
00286             //rled = 1;
00287             //gled = 0;         
00288         }                                                           
00289     }// endif direction up or down
00290     }// endif deadband
00291  
00292 //  printf("X: %1.4f, Y: %1.4f, Z: %1.4f,avg-norm %1.4f Norm: %1.4f \r\n",
00293     //   xaccln, yaccln, zaccln,(avg-norm),
00294     //   norm);
00295 //  printf("%d\r\n",time_reg);
00296  // TWM using thread wait below instead of this wait function!!!
00297  //   wait(0.1f);
00298  
00299 
00300         
00301 //     led1 = !led1;
00302         // Example data printing          
00303 //      accel_rms = sqrt(((accel_data[0]*accel_data[0])+(accel_data[1]*accel_data[1])+(accel_data[2]*accel_data[2]))/3);
00304  //     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);
00305  //     wait(0.01);
00306 #ifdef USE_MAG_DATA     
00307       mag.acquire_mag_data_uT(mag_data);
00308       mag_rms = sqrt(((mag_data[0]*mag_data[0])+(mag_data[1]*mag_data[1])+(mag_data[2]*mag_data[2]))/3);
00309       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);
00310       wait(0.01);
00311 #endif
00312         Thread::wait(100);        
00313  //       Thread::wait(500);
00314     }
00315 }