void ForceTorqueCtrl::SetActiveCalibrationMatrix(int num)
{
std::cout << "\n\n*******Setting Active Calibration Matrix Num to: "<< num <<"********"<< std::endl;
BYTE b = 0;
CanMsg CMsg;
CMsg.setID(0x206);
CMsg.setLength(1);
CMsg.setAt(num,0);
bool ret = m_Can->transmitMsg(CMsg, true);
CanMsg replyMsg;
bool ret2 = m_Can->receiveMsg(&replyMsg);
if(ret2)
{
std::cout<<"reply ID: \t"<<replyMsg.getID()<<std::endl;
std::cout<<"reply Length: \t"<<replyMsg.getLength()<<std::endl;
if(replyMsg.getID() == 0x206)
{
std::cout<<"Setting Calibration Matrix succeed!"<<std::endl;
std::cout<<"Calibration Matrix: "<<replyMsg.getAt(0)<<" is Activ!"<<std::endl;
}
else
std::cout<<"Error: Received wrong opcode!"<<std::endl;
}
else
std::cout<<"Error: Receiving Message failed!"<<std::endl;
}
void ForceTorqueCtrl::ReadFirmwareVersion()
{
std::cout << "\n\n*******Reading Firmware Version: "<< std::endl;
BYTE b = 0;
CanMsg CMsg;
CMsg.setID(0x20F);
CMsg.setLength(0);
bool ret = m_Can->transmitMsg(CMsg, true);
CanMsg replyMsg;
bool ret2 = m_Can->receiveMsg(&replyMsg);
if(ret2)
{
std::cout<<"reply ID: \t"<<replyMsg.getID()<<std::endl;
std::cout<<"reply Length: \t"<<replyMsg.getLength()<<std::endl;
if(replyMsg.getID() == 0x20F)
{
std::cout<<"Reading Firmware Succeed!"<<std::endl;
std::cout << "reply Data: \t" << replyMsg.getAt(0) << " " << replyMsg.getAt(1) << " "
<< replyMsg.getAt(2) << " " << replyMsg.getAt(3) << " "
<< replyMsg.getAt(4) << " " << replyMsg.getAt(5) << " "
<< replyMsg.getAt(6) << " " << replyMsg.getAt(7) << std::endl;
}
else
std::cout<<"Error: Received wrong opcode!"<<std::endl;
}
else
std::cout<<"Error: Receiving Message failed!"<<std::endl;
}
//-------------------------------------------
bool ros_for_can::transmitMsg(CanMsg CMsg, bool bBlocking)
{
TPCANMsg TPCMsg;
bool bRet = true;
if (m_bInitialized == false) return false;
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.getLength();
TPCMsg.ID = CMsg.getID();
TPCMsg.MSGTYPE = CMsg.getType();
for(int i=0; i<8; i++)
TPCMsg.DATA[i] = CMsg.getAt(i);
//TODO Hier stürtzt die Base ab.. verwende libpcan.h pcan.h um Fehler auszulesen, diagnostizieren, ausgeben und CAN_INIT erneut aufzurufen = neustart can-hardware.
int iRet;
//iRet = CAN_Write(m_handle, &TPCMsg);
iRet = LINUX_CAN_Write_Timeout(m_handle, &TPCMsg, 25); //Timeout in micrsoseconds
if(iRet != CAN_ERR_OK) {
#ifdef __DEBUG__
std::cout << "ros_for_can::transmitMsg An error occured while sending..." << iRet << std::endl;
outputDetailedStatus();
#endif
bRet = false;
}
#ifdef __DEBUG__
//is this necessary? try iRet==CAN_Status(m_handle) ?
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
std::cout << "ros_for_can::transmitMsg, system error: " << iRet << std::endl;
bRet = false;
} else if((iRet & CAN_ERR_BUSOFF) != 0) {
std::cout << "ros_for_can::transmitMsg, BUSOFF detected" << std::endl;
//Try to restart CAN-Device
std::cout << "Trying to re-init Hardware..." << std::endl;
bRet = initCAN();
} else if((iRet & CAN_ERR_ANYBUSERR) != 0) {
std::cout << "ros_for_can::transmitMsg, ANYBUSERR" << std::endl;
} else if( (iRet & (~CAN_ERR_QRCVEMPTY)) != 0) {
std::cout << "ros_for_can::transmitMsg, CAN_STATUS: " << iRet << std::endl;
bRet = false;
}
#endif
return bRet;
}
void ForceTorqueCtrl::ReadFTSerialNumber()
{
std::cout << "\n\n*********CheckCalMatrix**********" << std::endl;
CanMsg CMsg;
CMsg.setID(0x205);
CMsg.setLength(0);
bool ret = m_Can->transmitMsg(CMsg, true);
CanMsg replyMsg;
replyMsg.set(0,0);
replyMsg.set(0,1);
replyMsg.set(0,2);
replyMsg.set(0,3);
replyMsg.set(0,4);
bool ret2 = m_Can->receiveMsg(&replyMsg);
int length = replyMsg.getLength();
std::cout << "reply ID: \t" << replyMsg.getID()<<std::endl;
std::cout << "reply Length: \t" << replyMsg.getLength()<<std::endl;
std::cout << "reply Data: \t" << replyMsg.getAt(0) << " " << replyMsg.getAt(1) << " "
<< replyMsg.getAt(2) << " " << replyMsg.getAt(3) << " "
<< replyMsg.getAt(4) << " " << replyMsg.getAt(5) << " "
<< replyMsg.getAt(6) << " " << replyMsg.getAt(7) << std::endl;
}
void ForceTorqueCtrl::ReadMatrix(int axis, Eigen::VectorXf& vec)
{
std::cout << "\n\n*******Read Matrix**********"<<std::endl;
float statusCode = 0, sg0 = 0.0, sg1 = 0.0, sg2 = 0.0, sg3 = 0.0, sg4 = 0.0, sg5 = 0.0;
CanMsg CMsg;
CMsg.setID(0x202);
CMsg.setLength(1);
CMsg.setAt(axis,0);
bool ret = m_Can->transmitMsg(CMsg, true);
if(!ret)
{
std::cout<<"Error: Requesting Calibration Matrix!"<<std::endl;
return;
}
CanMsg replyMsg;
bool ret2 = m_Can->receiveMsg(&replyMsg);
if(ret2)
{
std::cout << "reply ID: \t" << replyMsg.getID()<<std::endl;
std::cout << "reply Length: \t" << replyMsg.getLength()<<std::endl;
std::cout << "reply Data: \t" << replyMsg.getAt(0) << " " << replyMsg.getAt(1) << " "
<< replyMsg.getAt(2) << " " << replyMsg.getAt(3) << " "
<< replyMsg.getAt(4) << " " << replyMsg.getAt(5) << " "
<< replyMsg.getAt(6) << " " << replyMsg.getAt(7) << std::endl;
fbBuf.bytes[0] = replyMsg.getAt(3);
fbBuf.bytes[1] = replyMsg.getAt(2);
fbBuf.bytes[2] = replyMsg.getAt(1);
fbBuf.bytes[3] = replyMsg.getAt(0);
sg0 = fbBuf.value;
fbBuf.bytes[0] = replyMsg.getAt(7);
fbBuf.bytes[1] = replyMsg.getAt(6);
fbBuf.bytes[2] = replyMsg.getAt(5);
fbBuf.bytes[3] = replyMsg.getAt(4);
sg1 = fbBuf.value;
}
else
return;
ret2 = m_Can->receiveMsg(&replyMsg);
if(ret2)
{
std::cout << "reply ID: \t" << replyMsg.getID()<<std::endl;
std::cout << "reply Length: \t" << replyMsg.getLength()<<std::endl;
std::cout << "reply Data: \t" << replyMsg.getAt(0) << " " << replyMsg.getAt(1) << " "
<< replyMsg.getAt(2) << " " << replyMsg.getAt(3) << " "
<< replyMsg.getAt(4) << " " << replyMsg.getAt(5) << " "
<< replyMsg.getAt(6) << " " << replyMsg.getAt(7) << std::endl;
fbBuf.bytes[0] = replyMsg.getAt(3);
fbBuf.bytes[1] = replyMsg.getAt(2);
fbBuf.bytes[2] = replyMsg.getAt(1);
fbBuf.bytes[3] = replyMsg.getAt(0);
sg2 = fbBuf.value;
fbBuf.bytes[0] = replyMsg.getAt(7);
fbBuf.bytes[1] = replyMsg.getAt(6);
fbBuf.bytes[2] = replyMsg.getAt(5);
fbBuf.bytes[3] = replyMsg.getAt(4);
sg3 = fbBuf.value;
}
else
return;
ret2 = m_Can->receiveMsg(&replyMsg);
if(ret2)
{
std::cout << "reply ID: \t" << replyMsg.getID()<<std::endl;
std::cout << "reply Length: \t" << replyMsg.getLength()<<std::endl;
std::cout << "reply Data: \t" << replyMsg.getAt(0) << " " << replyMsg.getAt(1) << " "
<< replyMsg.getAt(2) << " " << replyMsg.getAt(3) << " "
<< replyMsg.getAt(4) << " " << replyMsg.getAt(5) << " "
<< replyMsg.getAt(6) << " " << replyMsg.getAt(7) << std::endl;
fbBuf.bytes[0] = replyMsg.getAt(3);
fbBuf.bytes[1] = replyMsg.getAt(2);
fbBuf.bytes[2] = replyMsg.getAt(1);
fbBuf.bytes[3] = replyMsg.getAt(0);
sg4 = fbBuf.value;
fbBuf.bytes[0] = replyMsg.getAt(7);
fbBuf.bytes[1] = replyMsg.getAt(6);
fbBuf.bytes[2] = replyMsg.getAt(5);
fbBuf.bytes[3] = replyMsg.getAt(4);
sg5 = fbBuf.value;
}
else
return;
vec[0] = sg0; vec[1] = sg1; vec[2] = sg2; vec[3] = sg3; vec[4] = sg4; vec[5] = sg5;
std::cout<<"Matix: SG0: "<<sg0<<" SG1: "<<sg1<<" SG2: "<<sg2<<" SG3: "<<sg3<<" SG4: "<<sg4<<" SG5: "<<sg5<<std::endl;
}
