File content as of revision 17:9a060d2daca8:

#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];         // not used    
//    force_L[0] = force_L1.read();         // not used
//    force_L[1] = force_L2.read();         // not used
//    force_L[2] = force_L3.read();         // not used
//    force_L[3] = force_L4.read();         // not used

    

//    bt.printf("%1.3f,%1.3f,%1.3f,%1.3f\n",force_R[0],force_R[1],force_R[2],force_R[3]);           //debug purpose
// ======================================== BH 190704 reomoved
//    float force_R[4];
//    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.putc('<');
//    bt.putc('F');
//    bt.putc('O');
//    bt.putc('T');
//
//    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');
// ======================================== BH 190704 added
    int force_R[4];
    force_R[0] = force_R1.read();
    force_R[1] = force_R2.read();
    force_R[2] = force_R3.read();
    force_R[3] = force_R4.read();           // 0 ~ 1
    for(int i=0; i<4 ; i++){
        force_R[i]*=100;                    //forcr_R range = 0~1    -> changed 0~99
        if(force_R[i] > 100) force_R[i] = 99;   // for fixed space 2
    }
    bt.printf("<FOT%d%d%d%d>\n", force_R[0],
                                force_R[1],
                                force_R[2],
                                force_R[3]);
    
}