Is this test code correct ?
Code: Select all
#include <unistd.h>
#include <iostream>
#include <iterator>
#include "PCANBasic.h"
// Pcanbasic params
TPCANMsgFD msg;
TPCANStatus Status;
int HandleReadError(TPCANStatus status);
char strMsg[256];
TPCANBitrateFD BitrateFD = (char*) "f_clock_mhz=80,nom_brp=1,nom_tseg1=50,nom_tseg2=29,nom_sjw=10,data_brp=1,data_tseg1=8,data_tseg2=7,data_sjw=12";
unsigned int moteus_id(0x8002);
bool initialize_port()
{
Status = CAN_InitializeFD(PCAN_PCIBUS1, BitrateFD); // Bus1 for motor 02 (8002). the 80 byte is needed to ask a query
if (Status != PCAN_ERROR_OK)
{
// if error message, show it
CAN_GetErrorText(Status, 0, strMsg);
std::cout<<(strMsg); // TODO add a rospy.loginfo or error, later...
return false;
}
else {return true;}
}
void send()
{
//send frame to controller
msg.MSGTYPE = PCAN_MESSAGE_FD;
msg.DLC = 3; // DLC (data length code, for canFD message) value. -> 11 is 20 bytes, 12 is 24, 13 is 32, 14 is 48, 15 is 64
msg.ID = moteus_id;
msg.DATA[0] = 42;
msg.DATA[1] = 01;
msg.DATA[2] = 20;
Status = CAN_WriteFD(PCAN_PCIBUS1, &msg);
if (Status != PCAN_ERROR_OK)
{
CAN_GetErrorText(Status, 0, strMsg);
std::cout<<(strMsg);
}
else
{
std::cout<<"\nmessage correctly sent : ";
std::copy(std::begin(msg.DATA), std::end(msg.DATA), std::ostream_iterator<int>(std::cout, " "));
// it should respond with 41 01 00
}
}
void receive()
{
//send frame to controller
msg.MSGTYPE = PCAN_MESSAGE_FD;
msg.ID = moteus_id;
Status = CAN_ReadFD(PCAN_PCIBUS1, &msg, NULL);
if (Status != PCAN_ERROR_OK)
{
CAN_GetErrorText(Status, 0, strMsg);
std::cout<<(strMsg);
}
else
{
std::cout<<"\nmessage correctly received : ";
std::copy(std::begin(msg.DATA), std::end(msg.DATA), std::ostream_iterator<int>(std::cout, " "));
std::cout<<"\n";
// it should respond with 41 01 00
}
}
int main()
{
initialize_port();
usleep(50);
send();
usleep(50);
receive();
}
Code: Select all
message correctly sent : 42 1 20 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
message correctly received : 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
Could the problem come from the ID ? I use msg.ID=8002;
Without the recognized 80 byte on id, the controller doesn't send any informations, even if I query it !