can.cpp

00001 #include "can.h"
00002 #include "mbed.h"
00003 #include "nhk19mr2_can_info.h"
00004 #include "pinnames.h"
00005 #include "splitData.h"
00006 CAN can(pin_can_rd, pin_can_td);
00007 
00008 void MakeCanRad(OneLeg leg0, OneLeg leg1, char *candata);
00009 void SendRad(OneLeg leg0, OneLeg leg1)
00010 {
00011     char candata[kByteEachRad * 4] = {};
00012     MakeCanRad(leg0, leg1, candata);
00013     CANMessage msg(CANID_SERVO_RAD, candata, kByteCanSevoRad);
00014     can.write(msg);
00015 }
00016 void MakeCanRad(OneLeg leg0, OneLeg leg1, char *candata)
00017 {
00018     float rad[4] = {
00019         leg0.GetRad(0),
00020         leg0.GetRad(1),
00021         leg1.GetRad(0),
00022         leg1.GetRad(1),
00023     };
00024     int rad_multi[4];
00025     for (int i = 0; i < 4; i++)
00026     {
00027         rad_multi[i] = rad[i] * kRadToData;
00028         Intobyte(rad_multi[i], kByteEachRad, candata, i * kByteEachRad);
00029     }
00030 }