116 std::cout <<
"Can not read Serial Number from FTS!" << std::endl;
121 std::cout <<
"Can not read Firmware version from FTS!" << std::endl;
126 std::cout <<
"Can not read Counts Per Unit from FTS!" << std::endl;
131 std::cout <<
"Can not read Unit Codes from FTS!" << std::endl;
140 std::cout <<
"CAN initialisation unsuccessful!" << std::endl;
170 std::cout <<
"\n\n*********FTSerialNumber**********" << std::endl;
180 replyMsg.
set(0, 0, 0, 0, 0, 0, 0, 0);
186 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
187 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
192 std::cout <<
"Reading Serial Number Succeeded!" << std::endl;
193 std::cout <<
"reply Data: \t" << (char)replyMsg.
getAt(0) <<
" " << (char)replyMsg.
getAt(1) <<
" " 194 << (char)replyMsg.
getAt(2) <<
" " << (char)replyMsg.
getAt(3) <<
" " << (char)replyMsg.
getAt(4) <<
" " 195 << (char)replyMsg.
getAt(5) <<
" " << (char)replyMsg.
getAt(6) <<
" " << (char)replyMsg.
getAt(7)
202 std::cout <<
"Error: Received wrong opcode!" << std::endl;
210 std::cout <<
"ATIForceTorqueSensorHWCan::ReadFTSerialNumber(): Can not read message!" << std::endl;
217 std::cout <<
"ATIForceTorqueSensorHWCan::ReadFTSerialNumber(): Can not transmit message!" << std::endl;
227 std::cout <<
"\n\n*********Read Counts Per Unit**********" << std::endl;
230 float countsPerForce = 0, countsPerTorque = 0;
245 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
246 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
251 std::cout <<
"Reading Counts Per Unit Succeeded!" << std::endl;
252 std::cout <<
"reply Data: \t" << (char)replyMsg.
getAt(0) <<
" " << (char)replyMsg.
getAt(1) <<
" " 253 << (char)replyMsg.
getAt(2) <<
" " << (char)replyMsg.
getAt(3) <<
" " << (char)replyMsg.
getAt(4) <<
" " 254 << (char)replyMsg.
getAt(5) <<
" " << (char)replyMsg.
getAt(6) <<
" " << (char)replyMsg.
getAt(7)
261 std::cout <<
"Error: Received wrong opcode!" << std::endl;
270 countsPerForce =
intbBuf.value;
276 countsPerTorque =
intbBuf.value;
280 std::cout <<
"ATIForceTorqueSensorHWCan::ReadCountsPerUnit(): Can not read message!" << std::endl;
285 std::cout <<
"ATIForceTorqueSensorHWCan::ReadCountsPerUnit(): Can not transmit message!" << std::endl;
288 std::cout <<
"CountsPerforce: " << countsPerForce <<
" CountsPerTorque: " << countsPerTorque << std::endl;
296 std::cout <<
"\n\n*********Read Unit Codes**********" << std::endl;
313 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
314 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
319 std::cout <<
"Reading Unit codes Succeed!" << std::endl;
320 std::cout <<
"reply Data: \t" << replyMsg.
getAt(0) <<
" " << replyMsg.
getAt(1) << std::endl;
326 std::cout <<
"ATIForceTorqueSensorHWCan::ReadUnitCodes(): Can not read message!" << std::endl;
331 std::cout <<
"ATIForceTorqueSensorHWCan::ReadUnitCodes(): Can not transmit message!" << std::endl;
341 std::cout <<
"\n\n*******Read Diagnostic ADC Voltages on index: " << index <<
"********" << std::endl;
348 CMsg.
setAt(index, 0);
359 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
360 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
365 std::cout <<
"Reading Diagnostic ADC Voltage succeed!" << std::endl;
366 std::cout <<
"ADC Voltage of diagnostic value " << index <<
" : " << replyMsg.
getAt(0) <<
" " 367 << replyMsg.
getAt(1) << std::endl;
375 std::cout <<
"Error: Received wrong opcode!" << std::endl;
378 std::cout <<
"Error: Receiving Message failed!" << std::endl;
382 std::cout <<
"ATIForceTorqueSensorHWCan::readDiagnosticADCVoltages(byte index): Can not transmit message!" << std::endl;
391 std::cout <<
"\n\n*******Setting Active Calibration Matrix Num to: " << num <<
"********" << std::endl;
408 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
409 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
414 std::cout <<
"Setting Calibration Matrix succeed!" << std::endl;
415 std::cout <<
"Calibration Matrix: " << replyMsg.
getAt(0) <<
" is Activ!" << std::endl;
421 std::cout <<
"Error: Received wrong opcode!" << std::endl;
428 std::cout <<
"Error: Receiving Message failed!" << std::endl;
434 std::cout <<
"ATIForceTorqueSensorHWCan::SetActiveCalibrationMatrix(int num): Can not transmit message!" << std::endl;
444 std::cout <<
"\n\n*******Setting Baud Rate value to: " << value <<
"********" << std::endl;
450 CMsg.
setAt(value, 0);
461 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
462 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
467 std::cout <<
"Send Baud Rate value: " << CMsg.
getAt(0) <<
"!" << std::endl;
468 std::cout <<
"Setting Baud Rate succeed!" << std::endl;
473 std::cout <<
"Error: Received wrong opcode!" << std::endl;
479 std::cout <<
"Error: Receiving Message failed!" << std::endl;
485 std::cout <<
"ATIForceTorqueSensorHWCan::SetBaudRate(int value): Can not transmit message!" << std::endl;
494 std::cout <<
"\n\n******* Reseting the NETCANOEM ********" << std::endl;
504 std::cout <<
"ATIForceTorqueSensorHWCan::Reset(): Can not transmit message!" << std::endl;
516 std::cout <<
"\n\n*******Setting Base Identifier value to HEX : " << std::hex << identifier <<
" ********" 523 CMsg.
setAt(identifier, 0);
534 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
535 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
540 std::cout <<
"Setting Base Identifier succeed!" << std::endl;
541 std::cout <<
"Send Base Identifier value: " << std::hex << CMsg.
getAt(0) <<
"!" << std::endl;
547 std::cout <<
"Error: Received wrong opcode!" << std::endl;
553 std::cout <<
"Error: Receiving Message failed!" << std::endl;
559 std::cout <<
"ATIForceTorqueSensorHWCan::SetBaseIdentifier(int identifier): Can not transmit message!" << std::endl;
568 Eigen::VectorXf vCoef(6);
599 std::cout <<
"\n\n*******Read Matrix**********" << std::endl;
601 float statusCode = 0, sg0 = 0.0, sg1 = 0.0, sg2 = 0.0, sg3 = 0.0, sg4 = 0.0, sg5 = 0.0;
611 std::cout <<
"Error: Requesting Calibration Matrix!" << std::endl;
620 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
621 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
626 std::cout <<
"Reading Matrix Succeeded!" << std::endl;
627 std::cout <<
"reply Data: \t" << replyMsg.
getAt(0) <<
" " << replyMsg.
getAt(1) <<
" " << replyMsg.
getAt(2) <<
" " 628 << replyMsg.
getAt(3) <<
" " << replyMsg.
getAt(4) <<
" " << replyMsg.
getAt(5) <<
" " << replyMsg.
getAt(6)
629 <<
" " << replyMsg.
getAt(7) << std::endl;
634 std::cout <<
"Error: Received wrong opcode!" << std::endl;
657 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
658 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
659 std::cout <<
"reply Data: \t" << replyMsg.
getAt(0) <<
" " << replyMsg.
getAt(1) <<
" " << replyMsg.
getAt(2) <<
" " 660 << replyMsg.
getAt(3) <<
" " << replyMsg.
getAt(4) <<
" " << replyMsg.
getAt(5) <<
" " << replyMsg.
getAt(6)
661 <<
" " << replyMsg.
getAt(7) << std::endl;
683 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
684 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
685 std::cout <<
"reply Data: \t" << replyMsg.
getAt(0) <<
" " << replyMsg.
getAt(1) <<
" " << replyMsg.
getAt(2) <<
" " 686 << replyMsg.
getAt(3) <<
" " << replyMsg.
getAt(4) <<
" " << replyMsg.
getAt(5) <<
" " << replyMsg.
getAt(6)
687 <<
" " << replyMsg.
getAt(7) << std::endl;
712 std::cout <<
"Matix: SG0: " << sg0 <<
" SG1: " << sg1 <<
" SG2: " << sg2 <<
" SG3: " << sg3 <<
" SG4: " << sg4
713 <<
" SG5: " << sg5 << std::endl;
720 std::cout <<
"\n\n*******Reading Firmware Version*******" << std::endl;
736 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
737 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
742 std::cout <<
"Reading Firmware Succeed!" << std::endl;
743 std::cout <<
"reply Data: \t" << replyMsg.
getAt(0) <<
" " << replyMsg.
getAt(1) <<
" " << replyMsg.
getAt(2)
744 <<
" " << replyMsg.
getAt(3) << std::endl;
749 std::cout <<
"Error: Received wrong opcode!" << std::endl;
755 std::cout <<
"Error: Receiving Message failed!" << std::endl;
761 std::cout <<
"Error: Transmiting Message failed!" << std::endl;
770 int sg0 = 0, sg1 = 0, sg2 = 0, sg3 = 0, sg4 = 0, sg5 = 0;
779 std::cout <<
"ATIForceTorqueSensorHWCan::ReadSGData: Error: Transmiting message failed!" << std::endl;
790 std::cout <<
"ATIForceTorqueSensorHWCan::ReadSGData: Error: Received wrong opcode (Should be 0x200)!" << std::endl;
791 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
792 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
793 std::cout <<
"reply Data: \t" << replyMsg.
getAt(0) <<
" " << replyMsg.
getAt(1) <<
" " << replyMsg.
getAt(2) <<
" " 794 << replyMsg.
getAt(3) <<
" " << replyMsg.
getAt(4) <<
" " << replyMsg.
getAt(5) <<
" " << replyMsg.
getAt(6)
795 <<
" " << replyMsg.
getAt(7) << std::endl;
799 c[0] = replyMsg.
getAt(0);
800 c[1] = replyMsg.
getAt(1);
801 statusCode = (short)((c[0] << 8) | c[1]);
805 if (statusCode & 0x4000)
807 std::cout <<
"ATIForceTorqueSensorHWCan::ReadSGData: CAN bus error detected!" << std::endl;
809 std::cout <<
"ATIForceTorqueSensorHWCan::ReadSGData: FTS reseted!" << std::endl;
813 std::cout <<
"ATIForceTorqueSensorHWCan::ReadSGData: Error: Something is wrong with sensor!" << std::endl;
814 std::cout << std::hex << statusCode << std::endl;
818 c[0] = replyMsg.
getAt(2);
819 c[1] = replyMsg.
getAt(3);
820 sg0 = (short)((c[0] << 8) | c[1]);
822 c[0] = replyMsg.
getAt(4);
823 c[1] = replyMsg.
getAt(5);
824 sg2 = (short)((c[0] << 8) | c[1]);
826 c[0] = replyMsg.
getAt(6);
827 c[1] = replyMsg.
getAt(7);
828 sg4 = (short)((c[0] << 8) | c[1]);
838 std::cout <<
"ATIForceTorqueSensorHWCan::ReadSGData: Error: Received wrong opcode (Should be 0x201)!" << std::endl;
839 std::cout <<
"reply ID: \t" << std::hex << replyMsg.
getID() << std::endl;
840 std::cout <<
"reply Length: \t" << replyMsg.
getLength() << std::endl;
841 std::cout <<
"reply Data: \t" << replyMsg.
getAt(0) <<
" " << replyMsg.
getAt(1) <<
" " << replyMsg.
getAt(2) <<
" " 842 << replyMsg.
getAt(3) <<
" " << replyMsg.
getAt(4) <<
" " << replyMsg.
getAt(5) <<
" " << replyMsg.
getAt(6)
843 <<
" " << replyMsg.
getAt(7) << std::endl;
847 c[0] = replyMsg.
getAt(0);
848 c[1] = replyMsg.
getAt(1);
849 sg1 = (short)((c[0] << 8) | c[1]);
851 c[0] = replyMsg.
getAt(2);
852 c[1] = replyMsg.
getAt(3);
853 sg3 = (short)((c[0] << 8) | c[1]);
855 c[0] = replyMsg.
getAt(4);
856 c[1] = replyMsg.
getAt(5);
857 sg5 = (short)((c[0] << 8) | c[1]);
874 Eigen::VectorXf v6SG(6);
875 Eigen::VectorXf v6tmp(6);
876 Eigen::VectorXf
test(6);
890 Eigen::VectorXf tmp(6);
901 Eigen::VectorXf tmp(6);
914 Eigen::VectorXf tmp(6);
926 Eigen::VectorXf tmp(6);
938 Eigen::VectorXf tmp(6);
950 Eigen::VectorXf tmp(6);
962 Eigen::VectorXf tmp(6);
974 Eigen::VectorXf tmp(6);
987 Eigen::MatrixXf tmp(6, 6);
1035 Eigen::MatrixXf tmp(6, 6);
void StrainGaugeToForce(int &sg0, int &sg1, int &sg2, int &sg3, int &sg4, int &sg5)
bool SetActiveCalibrationMatrix(int num)
union ATIForceTorqueSensorHWCan::@1 intbBuf
bool SetBaseIdentifier(int identifier)
void SetFYGain(float fyg0, float fyg1, float fyg2, float fyg3, float fyg4, float fyg5)
bool readFTData(int statusCode, double &Fx, double &Fy, double &Fz, double &Tx, double &Ty, double &Tz)
Eigen::VectorXf m_v3FYGain
void SetTXGain(float txg0, float txg1, float txg2, float txg3, float txg4, float txg5)
void SetFXGain(float fxg0, float fxg1, float fxg2, float fxg3, float fxg4, float fxg5)
Eigen::VectorXf m_v3TZGain
bool ReadFTSerialNumber()
void ReadCalibrationMatrix()
void setAt(BYTE data, int iNr)
union ATIForceTorqueSensorHWCan::@2 fbBuf
Eigen::VectorXf m_v3FXGain
bool SetBaudRate(int value)
bool ReadFirmwareVersion()
void SetGaugeGain(float gg0, float gg1, float gg2, float gg3, float gg4, float gg5)
~ATIForceTorqueSensorHWCan()
bool readDiagnosticADCVoltages(int index, short int &value)
void SetGaugeOffset(float sg0Off, float sg1Off, float sg2Off, float sg3Off, float sg4Off, float sg5Off)
void SetTYGain(float tyg0, float tyg1, float tyg2, float tyg3, float tyg4, float tyg5)
union ATIForceTorqueSensorHWCan::@0 ibBuf
virtual bool init_ret()=0
virtual bool receiveMsgRetry(CanMsg *pCMsg, int iNrOfRetry)=0
Eigen::VectorXf m_v3GaugeGain
bool initCommunication(int can_type, std::string can_path, int can_baudrate, int base_identifier)
void SetFZGain(float fzg0, float fzg1, float fzg2, float fzg3, float fzg4, float fzg5)
Eigen::MatrixXf m_vForceData
void set(BYTE Data0=0, BYTE Data1=0, BYTE Data2=0, BYTE Data3=0, BYTE Data4=0, BYTE Data5=0, BYTE Data6=0, BYTE Data7=0)
Eigen::VectorXf m_v3StrainGaigeOffset
#define CANITFTYPE_SOCKET_CAN
Eigen::VectorXf m_v3TYGain
Eigen::VectorXf m_v3FZGain
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void ReadMatrix(int axis, Eigen::VectorXf &vec)
Eigen::VectorXf m_v3TXGain
ATIForceTorqueSensorHWCan()
virtual bool transmitMsg(CanMsg CMsg, bool bBlocking=true)=0
Eigen::MatrixXf m_mXCalibMatrix
void SetTZGain(float tzg0, float tzg1, float tzg2, float tzg3, float tzg4, float tzg5)
#define CANITFTYPE_CAN_PEAK_USB