File content as of revision 10:3fcaf50f528f:

#include "mbed.h"

AnalogIn            force_L1(PB_0);            // Force Sensor
AnalogIn            force_L2(PB_1);
AnalogIn            force_L3(PC_0);           
AnalogIn            force_L4(PC_1);
AnalogIn            force_R1(PC_2);            
AnalogIn            force_R2(PC_3);
AnalogIn            force_R3(PC_4);            
AnalogIn            force_R4(PC_5);

extern              Serial bt;
extern              Serial pc;

char                readCount=0;

float sum_L[4]={0,};
float sum_R[4]={0,};

float avg_L[4]={0,};
float avg_R[4]={0,};


/*----------------------- Callback Functions ------------------------*/

void CalForce()
{

    float force_L[4];
    float force_R[4];
    
    force_L[0] = force_L1.read();
    force_L[1] = force_L2.read();
    force_L[2] = force_L3.read();
    force_L[3] = force_L4.read();
    force_R[0] = force_R1.read();
    force_R[1] = force_R2.read();
    force_R[2] = force_R3.read();
    force_R[3] = force_R4.read();
   
    
    if ( readCount < 6)
    {
        sum_L[0] = sum_L[0] + force_L[0];
        sum_L[1] = sum_L[1] + force_L[1];    
        sum_L[2] = sum_L[2] + force_L[2];
        sum_L[3] = sum_L[3] + force_L[3];
        
        sum_R[0] = sum_R[0] + force_R[0];
        sum_R[1] = sum_R[1] + force_R[1];    
        sum_R[2] = sum_R[2] + force_R[2];
        sum_R[3] = sum_R[3] + force_R[3];
        
        readCount++;
            
    }
     
    else if (readCount == 6 | readCount>6)
    {
        
        avg_L[0] = sum_L[0]/6;
        avg_L[1] = sum_L[1]/6;
        avg_L[2] = sum_L[2]/6;
        avg_L[3] = sum_L[3]/6;
        
        avg_R[0] = sum_R[0]/6;
        avg_R[1] = sum_R[1]/6;
        avg_R[2] = sum_R[2]/6;
        avg_R[3] = sum_R[3]/6;
        
//        bt.printf("%1.3f,%1.3f,%1.3f,%1.3f,%1.3f,%1.3f,%1.3f,%1.3f\n", avg_L[0],avg_L[1],avg_L[2],avg_L[3],avg_R[0],avg_R[1],avg_R[2],avg_R[3]);
    bt.putc('<');
    bt.putc('F');
    bt.putc('O');
    bt.putc('T');
    bt.putc((unsigned char)((int)avg_L[0]*127));
    bt.putc((unsigned char)((int)avg_L[1]*127));
    bt.putc((unsigned char)((int)avg_L[2]*127));
    bt.putc((unsigned char)((int)avg_L[3]*127));
    bt.putc((unsigned char)((int)avg_R[0]*127));
    bt.putc((unsigned char)((int)avg_R[1]*127));
    bt.putc((unsigned char)((int)avg_R[2]*127));
    bt.putc((unsigned char)((int)avg_R[3]*127));
    bt.putc('>');
    bt.putc('\r');
    bt.putc('\n');

        readCount=0;
        
        for(int i =0; i<4 ; i++)
        {
            sum_L[i] = 0;
            sum_R[i] = 0;
            avg_L[i] = 0;
            avg_R[i] = 0;
        }
        
        
    } 
     
    
}

void ReadForce()
{
    float force_L[4];
    float force_R[4];
    
    force_L[0] = force_L1.read();
    force_L[1] = force_L2.read();
    force_L[2] = force_L3.read();
    force_L[3] = force_L4.read();
    force_R[0] = force_R1.read();
    force_R[1] = force_R2.read();
    force_R[2] = force_R3.read();
    force_R[3] = force_R4.read();
    
//    bt.printf("%1.3f,%1.3f,%1.3f,%1.3f\n",force_R[0],force_R[1],force_R[2],force_R[3]);
    bt.putc('<');
    bt.putc('F');
    bt.putc('O');
    bt.putc('T');
//    bt.putc((char)((unsigned int)(force_L[0]*127)));
//    bt.putc((char)((unsigned int)(force_L[1]*127)));
//    bt.putc((char)((unsigned int)(force_L[2]*127)));
//    bt.putc((char)((unsigned int)(force_L[3]*127)));
    bt.putc((char)((unsigned int)(force_R[0]*127)));
    bt.putc((char)((unsigned int)(force_R[1]*127)));
    bt.putc((char)((unsigned int)(force_R[2]*127)));
    bt.putc((char)((unsigned int)(force_R[3]*127)));
    bt.putc('>');
    bt.putc('\r');
    bt.putc('\n');

    
    
    

    
}