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 CanCtrlPltfCOb3::sendNetStartCanOpen()
{
CanMsg msg;
msg.m_iID = 0;
msg.m_iLen = 2;
msg.set(1,0,0,0,0,0,0,0);
m_pCanCtrl->transmitMsg(msg, false);
usleep(100000);
}
//-------------------------------------------
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;
}
bool ElmoCtrl::sendNetStartCanOpen(CanItf* canCtrl) {
bool ret = false;
CanMsg msg;
msg.m_iID = 0;
msg.m_iLen = 2;
msg.set(1,0,0,0,0,0,0,0);
ret = canCtrl->transmitMsg(msg, false);
usleep(100000);
return ret;
}
//-------------------------------------------
bool CanPeakSys::transmitMsg(CanMsg CMsg, bool bBlocking)
{
TPCANMsg TPCMsg;
bool bRet = true;
if (m_bInitialized == false) return false;
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.m_iLen;
TPCMsg.ID = CMsg.m_iID;
TPCMsg.MSGTYPE = CMsg.m_iType;
for(int i=0; i<8; i++)
TPCMsg.DATA[i] = CMsg.getAt(i);
// write msg
int iRet;
iRet = CAN_Write(m_handle, &TPCMsg);
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
std::cout << "CanPeakSys::transmitMsg, errorcode= " << nGetLastError() << std::endl;
bRet = false;
}
return bRet;
}
//-------------------------------------------
bool CANPeakSys2PCI::transmitMsg(CanMsg& CMsg)
{
TPCANMsg TPCMsg;
bool bRet = true;
if (m_bInitialized == false) return false;
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.m_iLen;
TPCMsg.ID = CMsg.m_iID;
TPCMsg.MSGTYPE = CMsg.m_iType;
for(int i=0; i<8; i++)
TPCMsg.DATA[i] = CMsg.getAt(i);
// write msg
int iRet;
iRet = CAN_Write(m_handle, &TPCMsg);
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
LOGERROR( "transmitMsg() : errorcode= " << nGetLastError() );
bRet = false;
}
return bRet;
}
//-----------------------------------------------
bool CanESD::transmitMsg(CanMsg &CMsg)
{
CMSG NTCANMsg;
NTCANMsg.id = CMsg.m_iID;
NTCANMsg.len = CMsg.m_iLen;
for(int i=0; i<8; i++)
NTCANMsg.data[i] = CMsg.getAt(i);
NTCAN_RESULT ret;
int32_t len;
bool bRet = true;
len = 1;
//ret = canWrite(m_Handle, canmsg, &len, NULL);
ret = canSend(m_Handle, &NTCANMsg, &len);
if( ret != NTCAN_SUCCESS)
{
LOGERROR("canSend() failed: " << getErrorMessage(ret));
bRet = false;
}
m_LastID = (int)NTCANMsg.data[0];
//ret auswerten
return bRet;
}
//-------------------------------------------
bool CANPeakSysDongle::transmitMsg(CanMsg& CMsg)
{
bool bRet;
int i, iRet;
TPCANMsg TPCMsg;
if(!m_bInitialized) { return false; }
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.m_iLen;
TPCMsg.ID = CMsg.m_iID;
TPCMsg.MSGTYPE = CMsg.m_iType;
for(i = 0; i < 8; i++)
{
TPCMsg.DATA[i] = CMsg.getAt(i);
}
// write msg
bRet = true;
// std::cout<<"sending command: "<<std::endl;
CAN_Write(m_handle, &TPCMsg);
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
std::cout<<"transmitMsg() : errorcode= " << nGetLastError()<<std::endl;
bRet = false;
}
return bRet;
}
/**
* Transmit a message via the CAN bus.
* Additionally, an error flag is set internally if the transmission does not succeed.
* @param CMsg Structure containing the CAN message.
* @return true on success, false on failure.
*/
bool CanESD::transmitMsg(CanMsg CMsg, bool bBlocking)
{
CMSG NTCANMsg;
NTCANMsg.id = CMsg.m_iID;
NTCANMsg.len = CMsg.m_iLen;
for(int i=0; i<8; i++)
{
NTCANMsg.data[i] = CMsg.getAt(i);
//std::cout << "Msg: "std::cout << CMsg.getAt(i) << std::endl;
}
int ret;
//long len;
int32_t len;
bool bRet = true;
len = 1;
//m_Mutex.lock();
if (bBlocking)
ret = canWrite(m_Handle, &NTCANMsg, &len,+ NULL);
else
ret = canSend(m_Handle, &NTCANMsg, &len);
//m_Mutex.unlock();
if( ret != NTCAN_SUCCESS)
{
std::cout << "error in CANESD::transmitMsg: " << GetErrorStr(ret) << std::endl;
bRet = false;
}
m_LastID = (int)NTCANMsg.data[0];
//readEvent();
//ret auswerten
m_bIsTXError = !bRet;
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::ReadSGData(double &Fx, double &Fy, double &Fz, double &Tx, double &Ty, double &Tz)
{
int statusCode = 0, sg0 = 0, sg1 = 0, sg2 = 0, sg3 = 0, sg4 = 0, sg5 = 0;
CanMsg CMsg;
CMsg.setID(0x200);
CMsg.setLength(0);
bool ret = m_Can->transmitMsg(CMsg, true);
CanMsg replyMsg;
bool ret2 = m_Can->receiveMsg(&replyMsg);
unsigned char c[2];
if(ret2)
{
int length = replyMsg.getLength();
c[0] = replyMsg.getAt(0); //status code
c[1] = replyMsg.getAt(1);
//statusCode = (((char)c[0] << 8) | c[1]);
c[0] = replyMsg.getAt(2); //sg0
c[1] = replyMsg.getAt(3);
//sg0 = (((char)c[0] << 8) | c[1]);
sg0 = (short)((c[0] << 8) | c[1]);
c[0] = replyMsg.getAt(4); //sg1
c[1] = replyMsg.getAt(5);
sg1 = (short)((c[0] << 8) | c[1]);
c[0] = replyMsg.getAt(6); //sg2
c[1] = replyMsg.getAt(7);
sg2 = (short)((c[0] << 8) | c[1]);
}
else
return;
ret2 = m_Can->receiveMsg(&replyMsg);
if(ret2)
{
int length = replyMsg.getLength();
c[0] = replyMsg.getAt(0); //sg3
c[1] = replyMsg.getAt(1);
sg3 = (short)((c[0] << 8) | c[1]);
c[0] = replyMsg.getAt(2); //sg4
c[1] = replyMsg.getAt(3);
sg4 = (short)((c[0] << 8) | c[1]);
c[0] = replyMsg.getAt(4); //sg5
c[1] = replyMsg.getAt(5);
sg5 = (short)((c[0] << 8) | c[1]);
}
else
return;
//std::cout<<"\nsg0: "<<sg0<<" sg1: "<<sg1<<" sg2: "<<sg2<<" sg3: "<<sg3<<" sg4: "<<sg4<<" sg5: "<<sg5<<std::endl;
//out<<"sg0: "<<sg0<<" sg1: "<<sg1<<" sg2: "<<sg2<<" sg3: "<<sg3<<" sg4: "<<sg4<<" sg5: "<<sg5<<std::endl;
StrainGaugeToForce(sg0, sg1, sg2, sg3, sg4, sg5);
Fx = m_vForceData[0]; Fy = m_vForceData[1]; Fz = m_vForceData[2];
Tx = m_vForceData[3]; Ty= m_vForceData[4]; Tz = m_vForceData[5];
//out<<"Fx: "<<Fx<<" Fy: "<<Fy<<" Fz: "<<Fz<<" Tx: "<<Tx<<" Ty: "<<Ty<<" Tz: "<<Tz<<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;
}
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;
}
//-----------------------------------------------
bool CanDriveHarmonica::evalReceivedMsg(CanMsg& msg)
{
bool bRet = false;
int iDigIn;
int iFailure;
int iPara;
int iHomeDigIn = 0x0001; // 0x0001 for CoB3 steering drive homing input; 0x0400 for Scara
int iTemp1, iTemp2;
m_CanMsgLast = msg;
//-----------------------
// eval answers from PDO1 - transmitted on SYNC msg
if (msg.m_iID == m_ParamCanOpen.iTxPDO1)
{
iTemp1 = (msg.getAt(3) << 24) | (msg.getAt(2) << 16)
| (msg.getAt(1) << 8) | (msg.getAt(0) );
m_dPosGearMeasRad = m_DriveParam.getSign() * m_DriveParam.
PosMotIncrToPosGearRad(iTemp1);
iTemp2 = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4) );
m_dVelGearMeasRadS = m_DriveParam.getSign() * m_DriveParam.
VelMotIncrPeriodToVelGearRadS(iTemp2);
m_WatchdogTime.SetNow();
bRet = true;
}
//-----------------------
// eval answers from binary interpreter
if (msg.m_iID == m_ParamCanOpen.iTxPDO2)
{
if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'X') ) // current pos
{
}
else if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'A') ) // position absolute
{
}
else if( (msg.getAt(0) == 'J') && (msg.getAt(1) == 'V') ) // current velocity
{
}
else if( (msg.getAt(0) == 'B') && (msg.getAt(1) == 'G') ) // begin motion
{
}
else if( (msg.getAt(0) == 'U') && (msg.getAt(1) == 'M') ) // user mode
{
iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4)) );
}
else if( (msg.getAt(0) == 'I') && (msg.getAt(1) == 'P') ) // digital in == limit switches
{
iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4)) );
iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4)) );
if( (iDigIn & iHomeDigIn) != 0x0000 )
{
m_bLimSwRight = true;
}
}
else if( (msg.getAt(0) == 'S') && (msg.getAt(1) == 'R') ) // status
{
m_iStatusCtrl = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4) );
evalStatusRegister(m_iStatusCtrl);
ElmoRec->readoutRecorderTryStatus(m_iStatusCtrl);
}
else if( (msg.getAt(0) == 'M') && (msg.getAt(1) == 'F') ) // motor failure
{
iFailure = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4) );
evalMotorFailure(iFailure);
}
// debug eval
else if( (msg.getAt(0) == 'U') && (msg.getAt(1) == 'M') )
{
iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4) );
std::cout << "um " << iPara << std::endl;
}
else if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'M') )
{
iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4) );
std::cout << "pm " << iPara << std::endl;
}
else if( (msg.getAt(0) == 'A') && (msg.getAt(1) == 'C') )
{
iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4) );
std::cout << "ac " << iPara << std::endl;
}
else if( (msg.getAt(0) == 'D') && (msg.getAt(1) == 'C') )
{
iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4) );
std::cout << "dc " << iPara << std::endl;
}
else if( (msg.getAt(0) == 'H') && (msg.getAt(1) == 'M') )
{
// status message (homing armed = 1 / disarmed = 0) is encoded in 5th byte
if(msg.getAt(4) == 0)
{
// if 0 received: elmo disarmed homing after receiving the defined event
m_bLimSwRight = true;
}
}
else if( (msg.getAt(0) == 'I') && (msg.getAt(1) == 'Q') )
{
int iVal=0;
iVal = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
| (msg.getAt(5) << 8) | (msg.getAt(4) );
float* pfVal;
pfVal=(float*)&iVal;
m_dMotorCurr = *pfVal;
}
else
{
}
m_WatchdogTime.SetNow();
bRet = true;
}
//-----------------------
// eval answer from SDO
if (msg.m_iID == m_ParamCanOpen.iTxSDO)
{
m_WatchdogTime.SetNow();
if( (msg.getAt(0) >> 5) == 0) { //Received Upload SDO Segment (scs = 0)
//std::cout << "SDO Upload Segment received" << std::endl;
receivedSDODataSegment(msg);
} else if( (msg.getAt(0) & 0xE2) == 0x40) { //Received Initiate SDO Upload, that is not expedited -> start segmented upload (scs = 2 AND expedited flag = 0)
