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