First Last / Mbed 2 deprecated EtherCAT-XbusMaster

EtherCAT slave that reads 3 Xsens IMU's connected to a Xbus Master

Dependencies:   MODSERIAL mbed KL25Z_ClockControl

Fork of EtherCAT by First Last

Diff: soes.cpp

Revision:
10:4e9069e5d698
Parent:
9:33673e05639f
Child:
11:166353137b95
--- a/soes.cpp	Fri Dec 12 11:28:20 2014 +0000
+++ b/soes.cpp	Fri Dec 12 12:41:38 2014 +0000
@@ -170,6 +170,7 @@
   if (APPstate) //input or output enabled
     {	
     	float fl,fr,br,bl,copx,copy;
+    	float total_f;
         Rb.timestamp = ESCvar.Time;
         //just some dummy data to test
         //Rb.counter++;
@@ -180,9 +181,10 @@
         fl = rawFrontLeft - FrontLeft_offset;
         br = rawBackRight - BackRight_offset;
         bl = rawBackLeft - BackLeft_offset;
-        copx = ((fl+bl)-(fr+br))*BALANCE_WIDTH;
-        copy = ((fr+fl)-(br+bl))*BALANCE_HEIGHT;
-        grf = (fl+fr+br+bl)*(3.3*(1<<4)*(1./11.0));
+        total_f = fr+fl+br+bl;
+        copx = (((fl+bl)-(fr+br))/total_f)*BALANCE_WIDTH;
+        copy = (((fr+fl)-(br+bl))/total_f)*BALANCE_HEIGHT;
+        grf = (total_f)*(4096*(1<<4)*(1./1100.0));
         Rb.CoPx = copx;
         Rb.CoPy = copy;
         Rb.grf = grf;