Diff: ForceRead.cpp

Revision:

4:bf4ad2079096

Child:

6:549177f76f8e

diff -r b79aa7d836fb -r bf4ad2079096 ForceRead.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ForceRead.cpp	Fri Dec 21 20:39:24 2018 +0000
@@ -0,0 +1,107 @@
+#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]);
+
+        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,%1.3f,%1.3f,%1.3f,%1.3f\n", force_L[0],force_L[1],force_L[2],force_L[3],force_R[0],force_R[1],force_R[2],force_R[3]);
+    
+
+    
+}       
\ No newline at end of file