test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: can/can.cpp
- Revision:
- 14:d7cb429946f4
- Child:
- 15:3fd1d5698d10
diff -r e7ecdb20665a -r d7cb429946f4 can/can.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/can/can.cpp Tue Feb 12 12:50:45 2019 +0000 @@ -0,0 +1,24 @@ +#include "can.h" +#include "mbed.h" +#include "nhk19_can_info.h" +#include "OneLeg.h" + +CAN can(pin_can_rd,pin_can_td); + +void MakeCanRad(OneLeg leg0, OneLeg leg1, char *candata); +void CanSend(OneLeg leg0, OneLeg leg1) +{ + char candata[kByteEachRad*4] = {}; + MakeCanRad(leg0, leg1, candata); + CANMessage msg(CANID_SERVO_RAD, candata, kByteCanSevoRad); + can.write(msg); +} +void MakeCanRad(OneLeg leg0, OneLeg leg1, char *candata) +{ + float rad[4] = {leg0.GetRad(0), leg0.GetRad(1), leg1.GetRad(0), leg1.GetRad(1), }; + int rad_multi[4]; + for(int i= 0; i < 4; i++){ + rad_multi[i] = rad[i] * kRadToData; + Intobyte(rad_multi[i], kByteEachRad, candata, i*kByteEachRad); + } +} \ No newline at end of file