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; }
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; }