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