void move(geometry_msgs::Vector3 poserobot, geometry_msgs::Vector3 a, geometry_msgs::Vector3 n)
{
float t[5]={0,0,0,0,0};
std_msgs::Float64 degrees1;
std_msgs::Float64 degrees2;
std_msgs::Float64 degrees3;
std_msgs::Float64 degrees4;
std_msgs::Float64 degrees5;
ros::Rate loop_rate(10);
mci(t,poserobot,a,n); //obtain the theta for this pose
ros::Duration(1).sleep();
degrees1.data=t[0];
degrees2.data=t[1];
degrees3.data=t[2];
degrees4.data=t[3];
degrees5.data=t[4];
ROS_INFO("t1:%f, t2=%f, t3=%f, t4=%f, t5=%f", t[0],t[1],t[2],t[3],t[4]);
joint1.publish(degrees1);
joint2.publish(degrees2);
joint3.publish(degrees3);
ros::Duration(2).sleep();
joint4.publish(degrees4);
joint5.publish(degrees5);
}
void home()
{
float t[5]={0,0,0,0,0};
geometry_msgs::Vector3 poserobot,a,n;
std_msgs::Float64 degrees1;
std_msgs::Float64 degrees2;
std_msgs::Float64 degrees3;
std_msgs::Float64 degrees4;
std_msgs::Float64 degrees5;
poserobot.x= 0.3125;
poserobot.y= 0;
poserobot.z= 0.1450;
a.x= 1;
a.y= 0;
a.z= 0;
n.x= 0;
n.y= 0;
n.z= 1;
mci(t,poserobot,a,n); //obtain the theta for this pose
ros::Duration(1).sleep();
degrees1.data=t[0];
degrees2.data=t[1];
degrees3.data=t[2];
degrees4.data=t[3];
degrees5.data=t[4];
ROS_INFO("t1:%f, t2=%f, t3=%f, t4=%f, t5=%f", t[0],t[1],t[2],t[3],t[4]);
joint1.publish(degrees1);
joint2.publish(degrees2);
joint3.publish(degrees3);
joint4.publish(degrees4);
joint5.publish(degrees5);
}
stringbuf& operator <<(stringbuf& sb, const TransOpBase& op) {
static const char* size_names[4] = {"b", "w", "d", ""};
// e.g. addfp, addfv, addfd, xxx
static const char* fptype_names[4] = {".s", ".vs", ".d", ".d"};
bool ld = isload(op.opcode);
bool st = isstore(op.opcode);
bool fp = (isclass(op.opcode, OPCLASS_FP_ALU));
bool br = isbranch(op.opcode);
stringbuf sbname;
sbname << nameof(op.opcode);
if (!(opinfo[op.opcode].flagops & opNOSIZE)) sbname << (fp ? fptype_names[op.size] : size_names[op.size]);
if (isclass(op.opcode, OPCLASS_USECOND)) sbname << ".", cond_code_names[op.cond];
if (ld|st) {
if (op.opcode == OP_mf) {
static const char* mf_names[4] = {"none", "st", "ld", "all"};
sbname << '.', mf_names[op.extshift];
}
sbname << ((op.cond == LDST_ALIGN_LO) ? ".lo" : (op.cond == LDST_ALIGN_HI) ? ".hi" : "");
} else if ((op.opcode == OP_mask) || (op.opcode == OP_maskb)) {
sbname << ((op.cond == 0) ? "" : (op.cond == 1) ? ".z" : (op.cond == 2) ? ".x" : ".???");
}
if ((ld|st) && (op.cachelevel > 0)) sbname << ".L", (char)('1' + op.cachelevel);
if ((ld|st) && (op.locked)) sbname << ((ld) ? ".acq" : ".rel");
if (op.internal) sbname << ".p";
if (op.eom) sbname << ".", (op.any_flags_in_insn ? "+" : "-");
sb << padstring((char*)sbname, -12), " ", arch_reg_names[op.rd];
if ((op.rd < ARCHREG_COUNT) & (!op.final_arch_in_insn)) sb << ".t";
sb << " = ";
if (ld|st) sb << "[";
sb << arch_reg_names[op.ra];
if (op.rb == REG_imm) {
if (abs(op.rbimm) <= 32768) sb << ",", op.rbimm; else sb << ",", (void*)op.rbimm;
} else {
sb << ",", arch_reg_names[op.rb];
}
if (ld|st) sb << "]";
if ((op.opcode == OP_mask) | (op.opcode == OP_maskb)) {
MaskControlInfo mci(op.rcimm);
int sh = (op.opcode == OP_maskb) ? 3 : 0;
sb << ",[ms=", (mci.info.ms >> sh), " mc=", (mci.info.mc >> sh), " ds=", (mci.info.ds >> sh), "]";
} else {
/*! Dumps the internal structures. For debugging only! */
void TagFileParser::dump()
{
msg("Result:\n");
QListIterator<TagClassInfo> lci(m_tagFileClasses);
//============== CLASSES
TagClassInfo *cd;
for (;(cd=lci.current());++lci)
{
msg("class `%s'\n",cd->name.data());
msg(" filename `%s'\n",cd->filename.data());
if (cd->bases)
{
QListIterator<BaseInfo> bii(*cd->bases);
BaseInfo *bi;
for ( bii.toFirst() ; (bi=bii.current()) ; ++bii)
{
msg( " base: %s \n", bi->name.data() );
}
}
QListIterator<TagMemberInfo> mci(cd->members);
TagMemberInfo *md;
for (;(md=mci.current());++mci)
{
msg(" member:\n");
msg(" kind: `%s'\n",md->kind.data());
msg(" name: `%s'\n",md->name.data());
msg(" anchor: `%s'\n",md->anchor.data());
msg(" arglist: `%s'\n",md->arglist.data());
}
}
//============== NAMESPACES
QListIterator<TagNamespaceInfo> lni(m_tagFileNamespaces);
TagNamespaceInfo *nd;
for (;(nd=lni.current());++lni)
{
msg("namespace `%s'\n",nd->name.data());
msg(" filename `%s'\n",nd->filename.data());
QStringList::Iterator it;
for ( it = nd->classList.begin();
it != nd->classList.end(); ++it )
{
msg( " class: %s \n", (*it).latin1() );
}
QListIterator<TagMemberInfo> mci(nd->members);
TagMemberInfo *md;
for (;(md=mci.current());++mci)
{
msg(" member:\n");
msg(" kind: `%s'\n",md->kind.data());
msg(" name: `%s'\n",md->name.data());
msg(" anchor: `%s'\n",md->anchor.data());
msg(" arglist: `%s'\n",md->arglist.data());
}
}
//============== FILES
QListIterator<TagFileInfo> lfi(m_tagFileFiles);
TagFileInfo *fd;
for (;(fd=lfi.current());++lfi)
{
msg("file `%s'\n",fd->name.data());
msg(" filename `%s'\n",fd->filename.data());
QStringList::Iterator it;
for ( it = fd->namespaceList.begin();
it != fd->namespaceList.end(); ++it )
{
msg( " namespace: %s \n", (*it).latin1() );
}
for ( it = fd->classList.begin();
it != fd->classList.end(); ++it )
{
msg( " class: %s \n", (*it).latin1() );
}
QListIterator<TagMemberInfo> mci(fd->members);
TagMemberInfo *md;
for (;(md=mci.current());++mci)
{
msg(" member:\n");
msg(" kind: `%s'\n",md->kind.data());
msg(" name: `%s'\n",md->name.data());
msg(" anchor: `%s'\n",md->anchor.data());
msg(" arglist: `%s'\n",md->arglist.data());
}
QListIterator<TagIncludeInfo> mii(fd->includes);
TagIncludeInfo *ii;
for (;(ii=mii.current());++mii)
{
msg(" includes id: %s name: %s\n",ii->id.data(),ii->name.data());
}
}
//============== GROUPS
QListIterator<TagGroupInfo> lgi(m_tagFileGroups);
TagGroupInfo *gd;
for (;(gd=lgi.current());++lgi)
{
msg("group `%s'\n",gd->name.data());
msg(" filename `%s'\n",gd->filename.data());
QStringList::Iterator it;
for ( it = gd->namespaceList.begin();
it != gd->namespaceList.end(); ++it )
{
msg( " namespace: %s \n", (*it).latin1() );
}
for ( it = gd->classList.begin();
it != gd->classList.end(); ++it )
{
msg( " class: %s \n", (*it).latin1() );
}
for ( it = gd->fileList.begin();
it != gd->fileList.end(); ++it )
{
msg( " file: %s \n", (*it).latin1() );
}
for ( it = gd->subgroupList.begin();
it != gd->subgroupList.end(); ++it )
{
msg( " subgroup: %s \n", (*it).latin1() );
}
for ( it = gd->pageList.begin();
it != gd->pageList.end(); ++it )
{
msg( " page: %s \n", (*it).latin1() );
}
QListIterator<TagMemberInfo> mci(gd->members);
TagMemberInfo *md;
for (;(md=mci.current());++mci)
{
msg(" member:\n");
msg(" kind: `%s'\n",md->kind.data());
msg(" name: `%s'\n",md->name.data());
msg(" anchor: `%s'\n",md->anchor.data());
msg(" arglist: `%s'\n",md->arglist.data());
}
}
//============== PAGES
QListIterator<TagPageInfo> lpi(m_tagFilePages);
TagPageInfo *pd;
for (;(pd=lpi.current());++lpi)
{
msg("page `%s'\n",pd->name.data());
msg(" title `%s'\n",pd->title.data());
msg(" filename `%s'\n",pd->filename.data());
}
//============== DIRS
QListIterator<TagDirInfo> ldi(m_tagFileDirs);
TagDirInfo *dd;
for (;(dd=ldi.current());++ldi)
{
msg("dir `%s'\n",dd->name.data());
msg(" path `%s'\n",dd->path.data());
QStringList::Iterator it;
for ( it = dd->fileList.begin();
it != dd->fileList.end(); ++it )
{
msg( " file: %s \n", (*it).latin1() );
}
for ( it = dd->subdirList.begin();
it != dd->subdirList.end(); ++it )
{
msg( " subdir: %s \n", (*it).latin1() );
}
}
}
bool Sensor2DeviceImpl::setMagCalibrationReport(const MagCalibrationReport& data)
{
MagCalibrationImpl mci(data);
return GetInternalDevice()->SetFeatureReport(mci.Buffer, MagCalibrationImpl::PacketSize);
}
