//------------------------------------------- bool CANPeakSysUSB::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 << "CANPeakSysUSB::transmitMsg, errorcode= " << nGetLastError() << std::endl; 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; }
//------------------------------------------- bool CANPeakSysUSB::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); //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 << "CANPeakSysUSB::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 << "CANPeakSysUSB::transmitMsg, system error: " << iRet << std::endl; bRet = false; } else if((iRet & CAN_ERR_BUSOFF) != 0) { std::cout << "CANPeakSysUSB::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 << "CANPeakSysUSB::transmitMsg, ANYBUSERR" << std::endl; } else if( (iRet & (~CAN_ERR_QRCVEMPTY)) != 0) { std::cout << "CANPeakSysUSB::transmitMsg, CAN_STATUS: " << iRet << std::endl; bRet = false; } #endif return bRet; }
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; }
/** * 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::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)