Diff: ForceRead.cpp

Revision:

19:67584cb64b9c

Parent:

18:25281ee3a517

--- a/ForceRead.cpp	Thu Jul 04 08:08:18 2019 +0000
+++ b/ForceRead.cpp	Thu Aug 22 08:36:15 2019 +0000
@@ -1,18 +1,12 @@
 #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;
+//extern              Serial pc;
 
 float sum_L[4]={0,};
 float sum_R[4]={0,};
@@ -20,120 +14,11 @@
 float avg_L[4]={0,};
 float avg_R[4]={0,};
 
-
-/*----------------------- Callback Functions ------------------------*/
-
-void CalForce()
+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();
-   
-    
-    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');
+    int force_R[4];
 
-        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[i]*=100;                    //forcr_R range = 0~1    -> changed 0~99
     force_R[0] = force_R1.read()*99;
     force_R[1] = force_R2.read()*99;
     force_R[2] = force_R3.read()*99;
@@ -143,5 +28,4 @@
                                 force_R[1],
                                 force_R[2],
                                 force_R[3]);
-    
 }       
\ No newline at end of file