First Last
/
EtherCAT
EtherCAT slave based on SOES
Diff: soes.cpp
- Revision:
- 9:33673e05639f
- Parent:
- 8:09dcef3ed467
- Child:
- 10:4e9069e5d698
diff -r 09dcef3ed467 -r 33673e05639f soes.cpp --- a/soes.cpp Fri Dec 12 00:31:22 2014 +0000 +++ b/soes.cpp Fri Dec 12 11:28:20 2014 +0000 @@ -68,6 +68,7 @@ float rawFrontRight; float rawBackLeft; float rawBackRight; +float grf; //volatile uint8 diginput; //Serial shoe_serial(SHOE_SERIAL_TX,SHOE_SERIAL_RX); @@ -181,8 +182,10 @@ 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)); Rb.CoPx = copx; Rb.CoPy = copy; + Rb.grf = grf; Rb.FrontRight = rawFrontRight; Rb.FrontLeft = rawFrontLeft; Rb.BackRight = rawBackRight;