Diff: can/can.cpp

Revision:

14:d7cb429946f4

Child:

15:3fd1d5698d10

--- /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