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