SOES implementation i.c.w. ET1100 (Beckhoff ASIC)
Dependencies: KL25Z_ClockControl MODSERIAL mbed
Fork of EtherCAT-XbusMaster by
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;