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