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);
    }
}