void execute() { //printInstructionTable(); int pc = 0; char opcode[OPCODE_SIZE]; char operand[OPERAND_SIZE]; while(strcmp(opcode, "halt")){ fetchInstruction(pc, opcode, operand); // execution of current instruction if(strcmp(opcode, "nop") == 0){pc = nop(pc);} else if(strcmp(opcode, "add") == 0){pc = add(pc);} else if(strcmp(opcode, "sub") == 0){pc = sub(pc);} else if(strcmp(opcode, "mul") == 0){pc = mul(pc);} else if(strcmp(opcode, "divide") == 0){pc = divide(pc);} else if(strcmp(opcode, "get") == 0){pc = get(pc, operand);} else if(strcmp(opcode, "put") == 0){pc = put(pc, operand);} else if(strcmp(opcode, "push") == 0){pc = push(pc, operand);} else if(strcmp(opcode, "pop") == 0){pc = pop(pc, operand);} else if(strcmp(opcode, "not") == 0){pc = not(pc);} else if(strcmp(opcode, "and") == 0){pc = and(pc);} else if(strcmp(opcode, "or") == 0){pc = or(pc);} else if(strcmp(opcode, "testeq") == 0){pc = testeq(pc);} else if(strcmp(opcode, "testne") == 0){pc = testne(pc);} else if(strcmp(opcode, "testlt") == 0){pc = testlt(pc);} else if(strcmp(opcode, "testle") == 0){pc = testle(pc);} else if(strcmp(opcode, "testgt") == 0){pc = testgt(pc);} else if(strcmp(opcode, "testge") == 0){pc = testge(pc);} else if(strcmp(opcode, "jump") == 0){pc = jump(pc, operand);} else if(strcmp(opcode, "jf") == 0){pc = jf(pc, operand);} } }
extern "C" jboolean JVM_IterateJarEntries(const JvmPathChar* jar_file_name, JvmNameFilterProc entry_filter_proc, JvmDoJarEntryProc do_jar_entry_proc, JvmAllocProc alloc_proc, JvmFreeProc free_proc, void* caller_data) { SETUP_ERROR_CHECKER_ARG; if (alloc_proc == NULL) { if (free_proc != NULL) { return KNI_FALSE; } else { alloc_proc = (JvmAllocProc)&OsMemory_allocate; free_proc = (JvmFreeProc)&OsMemory_free; } } if (free_proc == NULL) { return KNI_FALSE; } JarFile jf(alloc_proc, free_proc); jf.open(jar_file_name, false, false JVM_CHECK_0); if (!jf.is_open()) { return KNI_FALSE; } bool success = jf.filtered_do_entries(entry_filter_proc, do_jar_entry_proc, caller_data JVM_CHECK_0); return CAST_TO_JBOOLEAN(success); }
void Foam::jumpCyclicAMIFvPatchField<Type>::updateInterfaceMatrix ( gpuField<Type>& result, const gpuField<Type>& psiInternal, const scalargpuField& coeffs, const Pstream::commsTypes ) const { const labelgpuList& nbrFaceCells = this->cyclicAMIPatch().cyclicAMIPatch().neighbPatch().getFaceCells(); gpuField<Type> pnf(psiInternal, nbrFaceCells); if (this->cyclicAMIPatch().applyLowWeightCorrection()) { pnf = this->cyclicAMIPatch().interpolate ( pnf, this->patchInternalField()() ); } else { pnf = this->cyclicAMIPatch().interpolate(pnf); } // only apply jump to original field if (&psiInternal == &this->internalField()) { gpuField<Type> jf(this->jump()); if (!this->cyclicAMIPatch().owner()) { jf *= -1.0; } pnf -= jf; } // Transform according to the transformation tensors this->transformCoupleField(pnf); // Multiply the field by coefficients and add into the result matrixPatchOperation ( this->patch().index(), result, this->patch().boundaryMesh().mesh().lduAddr(), matrixInterfaceFunctor<Type> ( coeffs.data(), pnf.data() ) ); }
void Foam::jumpCyclicAMIFvPatchField<Type>::updateInterfaceMatrix ( Field<Type>& result, const Field<Type>& psiInternal, const scalarField& coeffs, const Pstream::commsTypes ) const { const labelUList& nbrFaceCells = this->cyclicAMIPatch().cyclicAMIPatch().neighbPatch().faceCells(); Field<Type> pnf(psiInternal, nbrFaceCells); if (this->cyclicAMIPatch().applyLowWeightCorrection()) { pnf = this->cyclicAMIPatch().interpolate ( pnf, this->patchInternalField()() ); } else { pnf = this->cyclicAMIPatch().interpolate(pnf); } // only apply jump to original field if (&psiInternal == &this->internalField()) { Field<Type> jf(this->jump()); if (!this->cyclicAMIPatch().owner()) { jf *= -1.0; } pnf -= jf; } // Transform according to the transformation tensors this->transformCoupleField(pnf); // Multiply the field by coefficients and add into the result const labelUList& faceCells = this->cyclicAMIPatch().faceCells(); forAll(faceCells, elemI) { result[faceCells[elemI]] -= coeffs[elemI]*pnf[elemI]; }
bool Cartridge::load_file(const char *fn, uint8 *&data, unsigned &size, CompressionMode compression) const { if(file::exists(fn) == false) return false; Reader::Type filetype = Reader::Normal; if(compression == CompressionInspect) filetype = Reader::detect(fn, true); if(compression == CompressionAuto) filetype = Reader::detect(fn, snes.config.file.autodetect_type); switch(filetype) { default: case Reader::Normal: { FileReader ff(fn); if(!ff.ready()) return false; size = ff.size(); data = ff.read(); } break; #ifdef GZIP_SUPPORT case Reader::GZIP: { GZReader gf(fn); if(!gf.ready()) return false; size = gf.size(); data = gf.read(); } break; case Reader::ZIP: { ZipReader zf(fn); if(!zf.ready()) return false; size = zf.size(); data = zf.read(); } break; #endif #ifdef JMA_SUPPORT case Reader::JMA: { try { JMAReader jf(fn); size = jf.size(); data = jf.read(); } catch(JMA::jma_errors jma_error) { return false; } } break; #endif } return true; }
void execALU(){ switch(controle_alu.op_code){ case 1: add(); break; case 2: addinc(); break; case 3: and(); break; case 4: andnota(); break; case 5: asl(); break; case 6: asr(); break; case 7: deca(); break; case 8: inca(); break; case 9: j(); break; case 10: jal(); break; case 11: jf(); break; case 12: jr(); break; case 13: jt(); break; case 14: lch(); break; case 15: lcl(); break; case 16: load(); break; case 17: loadlit(); break; case 18: lsl(); break; case 19: lsr(); break; case 20: nand(); break; case 21: nor(); break; case 22: ones(); break; case 23: or(); break; case 24: ornotb(); break; case 25: passa(); break; case 26: passnota(); break; case 27: store(); break; case 28: sub(); break; case 29: subdec(); break; case 30: xnor(); break; case 31: xor(); break; case 32: zeros(); break; } }
std::pair<bool, std::shared_ptr<file_md5_skel::skel_frame> > file_md5_skel::process_skel_frame( file_md5_anim::frame const & f, file_md5_anim::bound const & bbox, std::list<file_md5_anim::hierarchy> const & hier, std::list<file_md5_anim::baseframe> const & base ){ if( hier.size() != base.size() ){ assert( false && "hierarchy and baseframe size not equal" ); return { false, {} }; } std::shared_ptr<skel_frame> sf( new skel_frame ); sf->_joints.resize( hier.size() ); auto it_hier = hier.begin(); auto it_base = base.begin(); int index_current_joint = 0; while( it_hier != hier.end() ){ int frame_data_start_index = it_hier->_start_index; int flag = it_hier->_flags; //obtain rotation and position from base frame float sf_pos[3]; float sf_rot[3]; for( int i = 0; i < 3; ++i ){ sf_pos[i] = it_base->_pos[i]; sf_rot[i] = it_base->_orient[i]; } int offset = 0; //update rotation and position from frame data if neccessary for( int i = 0; i < 3; ++i ){ if( flag & (1<<i) ){ if( frame_data_start_index + offset >= f._data.size() ){ assert( false && "index access out of range" ); return { false, {} }; } sf_pos[i] = f._data[ frame_data_start_index + offset ]; ++offset; } } for( int i = 0; i < 3; ++i ){ if( flag & (8<<i) ){ if( frame_data_start_index + offset >= f._data.size() ){ assert( false && "index access out of range" ); return { false, {} }; } sf_rot[i] = f._data[ frame_data_start_index + offset ]; ++offset; } } //compute rotation quaternion Quat sf_orient( sf_rot[0], sf_rot[1], sf_rot[2] ); sf_orient.NormalizeQuatCurrent(); int parent_joint_index = it_hier->_parent; std::shared_ptr<joint_frame> jf( new joint_frame ); jf->_parent = parent_joint_index; jf->_name = it_hier->_name; if( parent_joint_index >= 0 ){ if( parent_joint_index >= sf->_joints.size() ){ assert( false && "parent joint index out of range." ); return { false, {} }; } std::shared_ptr<joint_frame> parent_joint_frame = sf->_joints[ parent_joint_index ]; //chain transformation from parent joint //update positions Quat qpos( sf_pos[0], sf_pos[1], sf_pos[2], 0.0f ); Quat orient_inv = parent_joint_frame->_orient.Inverse(); orient_inv.NormalizeQuatCurrent(); Quat res = parent_joint_frame->_orient * qpos * orient_inv; for( int i = 0; i < 3; ++i ){ jf->_pos[i] = parent_joint_frame->_pos[i] + res._quat[i]; } //update orientation jf->_orient = parent_joint_frame->_orient * sf_orient; jf->_orient.NormalizeQuatCurrent(); }else{ //no parent, root joint for( int i = 0; i < 3; ++i ){ jf->_pos[i] = sf_pos[i]; } jf->_orient = sf_orient; jf->_orient.NormalizeQuatCurrent(); } // sf->_joints.push_back( jf ); sf->_joints[ index_current_joint ] = jf; ++it_hier; ++it_base; ++index_current_joint; } for( int i = 0; i < 3; ++i ){ sf->_bbox_lower[i] = bbox._min[i]; sf->_bbox_upper[i] = bbox._max[i]; } return { true, sf }; }
bool Jets(Cluster_Amplitude *ampl,int mode) { DEBUG_FUNC("mode = "<<mode); msg_Debugging()<<*ampl<<"\n"; PHASIC::Jet_Finder *jf(ampl->JF<PHASIC::Jet_Finder>()); double q2cut(jf->Ycut()*sqr(rpa->gen.Ecms())); double q2min(std::numeric_limits<double>::max()); size_t imin(0), jmin(0), kmin(0); Flavour mofl; for (size_t i(0);i<ampl->Legs().size();++i) { Cluster_Leg *li(ampl->Leg(i)); for (size_t j(Max(i+1,ampl->NIn()));j<ampl->Legs().size();++j) { Cluster_Leg *lj(ampl->Leg(j)); if (j<ampl->NIn()) continue; for (size_t k(0);k<ampl->Legs().size();++k) { if (k==i || k==j) continue; Cluster_Leg *lk(ampl->Leg(k)); if (i<ampl->NIn() && k>=ampl->NIn()) continue; if (lk->Flav().Strong() && li->Flav().Strong() && lj->Flav().Strong()) { if (i<ampl->NIn()) li->SetMom(-li->Mom()); if (k<ampl->NIn()) lk->SetMom(-lk->Mom()); double q2ijk(pT2pythia(ampl,*li,*lj,*lk,i<ampl->NIn()?-1:1)); msg_Debugging()<<"Q_{"<<ID(li->Id())<<ID(lj->Id()) <<","<<ID(lk->Id())<<"} = "<<sqrt(q2ijk)<<"\n"; if (i<ampl->NIn()) li->SetMom(-li->Mom()); if (k<ampl->NIn()) lk->SetMom(-lk->Mom()); if (mode==0) { if (q2ijk<q2cut) return false; } else { if (q2ijk<q2min) { q2min=q2ijk; mofl=Flavour(kf_gluon); if (li->Flav().IsGluon()) mofl=lj->Flav(); if (lj->Flav().IsGluon()) mofl=li->Flav(); imin=i; jmin=j; kmin=k; } } } } } } if (mode!=0 && imin!=jmin) { Vec4D_Vector p=Combine(*ampl,imin,jmin,kmin,mofl); if (p.empty()) { msg_Error()<<METHOD<<"(): Combine failed. Use R configuration."<<std::endl; return Jets(ampl,0); } Cluster_Amplitude *bampl(Cluster_Amplitude::New()); bampl->SetProc(ampl->Proc<void>()); bampl->SetMS(ampl->MS()); bampl->SetNIn(ampl->NIn()); bampl->SetJF(ampl->JF<void>()); for (int i(0), j(0);i<ampl->Legs().size();++i) { if (i==jmin) continue; if (i==imin) { bampl->CreateLeg(p[j],mofl,ampl->Leg(i)->Col()); bampl->Legs().back()->SetId(ampl->Leg(imin)->Id()|ampl->Leg(jmin)->Id()); bampl->Legs().back()->SetK(ampl->Leg(kmin)->Id()); } else { bampl->CreateLeg(p[j],ampl->Leg(i)->Flav(),ampl->Leg(i)->Col()); } ++j; } bool res=Jets(bampl,0); bampl->Delete(); return res; } msg_Debugging()<<"--- Jet veto ---\n"; return true; }
JointFrame Solver::ToJoint(Point ptaim, JointFrame jointframe) { JointFrame PreJointFrame(jointframe); double t1,t2; bool flag[2]={false}; bool flag1=false; double theta[2][2]; double temptheta1[2], temptheta2[2]; int cnt=0; double jt_end_x=ptaim.getx(); double jt_end_y=ptaim.gety(); double a=((pow(jt_end_x,2)+pow(jt_end_y,2))+(pow(length1,2)-pow(length2,2)))/(2*length1*sqrt(pow(jt_end_x,2)+pow(jt_end_y,2))); if(a<-1|a>1) flag1=false; else{ //多解性 t1=acos(a);//acos默认取值 0~PI t2=atan2(jt_end_y,jt_end_x); theta[0][0]=t1+t2; if(theta[0][0]*180/PI<theta1max&theta[0][0]*180/PI>theta1min){ theta[0][1]=atan2(jt_end_y-length1*sin(theta[0][0]),jt_end_x-length1*cos(theta[0][0])); if(theta[0][1]*180/PI<theta2max&theta[0][1]*180/PI>theta2min){ if(abs(jt_end_x-length1*cos(theta[0][0])-length2*cos(theta[0][1]))<0.01&abs(jt_end_y-length1*sin(theta[0][0])-length2*sin(theta[0][1]))<0.01) { flag[0]=true; } } } theta[1][0]=-t1+t2; if(theta[1][0]*180/PI<theta1max&theta[1][0]*180/PI>theta1min){ theta[1][1]=atan2(jt_end_y-length1*sin(theta[1][0]),jt_end_x-length1*cos(theta[1][0])); if(theta[1][1]*180/PI<theta2max&theta[1][1]*180/PI>theta2min){ if(abs(jt_end_x-length1*cos(theta[1][0])-length2*cos(theta[1][1]))<0.01&abs(jt_end_y-length1*sin(theta[1][0])-length2*sin(theta[1][1]))<0.01) { flag[1]=true; } } } if (flag[0] == true&&flag[1]==false) { flag1 = true; JointFrame jf(theta[0][0], theta[0][1]); return jf; } else if (flag[0] == false && flag[1] == true) { flag1 = true; JointFrame jf(theta[1][0], theta[1][1]); return jf; } else if (flag[0] == true && flag[1] == true) //选择转动较小的方案进行控制 { flag1 = true; double delta1 = abs(theta[0][0] - PreJointFrame.gettheta1()) + abs(theta[0][1] - PreJointFrame.gettheta2()); double delta2 = abs(theta[1][0] - PreJointFrame.gettheta1()) + abs(theta[1][1] - PreJointFrame.gettheta2()); if (delta1 <= delta2) { JointFrame jf(theta[0][0], theta[0][1]); return jf; } else { JointFrame jf(theta[1][0], theta[1][1]); return jf; } } else flag1 = false; } if(flag1==false) cout<<"无法达到指定位置"<<endl; }
void runProgram(){ int pc = 0; char lastOp[OPCODE_SIZE]; int haltHit = 0; while(!haltHit){ if(strcmp(fetchOpcode(pc), "nop") == 0){ pc = nop(pc); } if(strcmp(fetchOpcode(pc), "add") == 0){ pc = add(pc); } if(strcmp(fetchOpcode(pc), "sub") == 0){ pc = sub(pc); } if(strcmp(fetchOpcode(pc), "mult") == 0){ pc = mult(pc); } if(strcmp(fetchOpcode(pc), "div") == 0){ pc = divide(pc); } if(strcmp(fetchOpcode(pc), "get") == 0){ pc = get(pc); } if(strcmp(fetchOpcode(pc), "put") == 0){ pc = put(pc); } if(strcmp(fetchOpcode(pc), "push") == 0){ pc = push(pc); } if(strcmp(fetchOpcode(pc), "pop") == 0){ pc = pop(pc); } if(strcmp(fetchOpcode(pc), "not") == 0){ pc = not(pc); } if(strcmp(fetchOpcode(pc), "and") == 0){ pc = and(pc); } if(strcmp(fetchOpcode(pc), "or") == 0){ pc = or(pc); } if(strcmp(fetchOpcode(pc), "tsteq") == 0){ pc = testeq(pc); } if(strcmp(fetchOpcode(pc), "tstne") == 0){ pc = testne(pc); } if(strcmp(fetchOpcode(pc), "tstlt") == 0){ pc = testlt(pc); } if(strcmp(fetchOpcode(pc), "tstle") == 0){ pc = testle(pc); } if(strcmp(fetchOpcode(pc), "tstgt") == 0){ pc = testgt(pc); } if(strcmp(fetchOpcode(pc), "tstge") == 0){ pc = testge(pc); } if(strcmp(fetchOpcode(pc), "j") == 0){ pc = jump(pc); } if(strcmp(fetchOpcode(pc), "jf") == 0){ pc = jf(pc); } if(strcmp(fetchOpcode(pc), "halt") == 0){ haltHit = 1; pc = halt(pc); } } }
std::uint16_t VirtualCPU::executeInstructionAtAddress(std::uint16_t address) { debugger_.pc_ = address; //HACK FOR NOW switch (memoryController_.readAtAddress(address)) { case 0: return address + halt(); case 1: return address + set(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2)); case 2: return address + push(memoryController_.readAtAddress(address + 1)); case 3: return address + pop(memoryController_.readAtAddress(address + 1)); case 4: return address + eq(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 5: return address + gt(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address +2), memoryController_.readAtAddress(address + 3)); case 6: return jump(memoryController_.readAtAddress(address + 1), address); case 7: return jt(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), address); case 8: return jf(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), address); case 9: return address + add(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 10: return address + mult(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 11: return address + mod(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 12: return address + and_i(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 13: return address + or_i(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 14: return address + not_i(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2)); case 15: return address + rmem(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2)); case 16: return address + wmem(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2)); case 17: return call(memoryController_.readAtAddress(address + 1), address); case 18: return ret(); case 19: return address + out(memoryController_.readAtAddress(address + 1)); case 21: return address + noop(); case 20: return address + in(memoryController_.readAtAddress(address + 1)); default: std::cout << "CPU ERROR illegal op code: " << memoryController_.readAtAddress(address) << std::endl; return address + 1; } }
/** *@brief サインスマート製4自由度ロボットアーム制御クラスのコンストラクタ */ RobotArm::RobotArm() { jl = new Vector3d[4]; pl = new Vector3d[4]; l[0] = ArmLength0; l[1] = ArmLength1; l[2] = ArmLength2; l[3] = ArmLength3; lh = HandLength; lf = FingerLength; m[0] = ArmMath0; m[1] = ArmMath1; m[2] = ArmMath2; m[3] = ArmMath3; mh = HandMath; mf = FingerMath; wi = ArmWidth; wf = FingerWidth; hi = ArmHeight; hf = FingerHeight; rh = HandRadius; jl[0](0) = 0; jl[0](1) = 0; jl[0](2) = 0; pl[0](0) = jl[0](0); pl[0](1) = jl[0](1); pl[0](2) = jl[0](2)+l[0]/2; jl[1](0) = pl[0](0); jl[1](1) = pl[0](1); jl[1](2) = pl[0](2)+l[0]/2; pl[1](0) = jl[1](0); pl[1](1) = jl[1](1); pl[1](2) = jl[1](2)+l[1]/2; jl[2](0) = pl[1](0); jl[2](1) = pl[1](1); jl[2](2) = pl[1](2)+l[1]/2; pl[2](0) = jl[2](0); pl[2](1) = jl[2](1); pl[2](2) = jl[2](2)+l[2]/2; jl[3](0) = pl[2](0); jl[3](1) = pl[2](1); jl[3](2) = pl[2](2)+l[2]/2; pl[3](0) = jl[3](0); pl[3](1) = jl[3](1)+l[3]/2; pl[3](2) = jl[3](2); jh(0) = pl[3](0); jh(1) = pl[3](1)+l[3]/2; jh(2) = pl[3](2); ph(0) = jh(0); //pyh = jyh+lh/2; ph(1) = jh(1); ph(2) = jh(2); jf(0) = ph(0); //jyf = pyh+lh/2; jf(1) = ph(1); jf(2) = ph(2); pf(0) = jf(0); pf(1) = jf(1); pf(2) = jf(2)-lf/2; hw = 0.02; setAngle(0, 0, 0, 0); dt = 0.01; //endTime = -1; //time = 0; /*targetPoint(0) = 0; targetPoint(1) = 0; targetPoint(2) = 0; startPoint(0) = 0; startPoint(1) = 0; startPoint(2) = 0;*/ setOffset(0,0,0,0); Kp = 10; Kjp = 10; openGripper(); maxSpeedCartesian = Vector3d(1000, 1000, 1000); maxSpeedJoint[0] = 1000; maxSpeedJoint[1] = 1000; maxSpeedJoint[2] = 1000; maxSpeedJoint[3] = 1000; softUpperLimitCartesian = Vector3d(1000, 1000, 1000); softLowerLimitCartesian = Vector3d(-1000, -1000, -1000); pauseFalg = false; stopFalg = false; //homePosition = Vector3d(0, 0, 0); softUpperLimitJoint[0] = MOTOR_UPPER__LIMIT_0; softUpperLimitJoint[1] = MOTOR_UPPER__LIMIT_1; softUpperLimitJoint[2] = MOTOR_UPPER__LIMIT_2; softUpperLimitJoint[3] = MOTOR_UPPER__LIMIT_3; softLowerLimitJoint[0] = MOTOR_LOWER__LIMIT_0; softLowerLimitJoint[1] = MOTOR_LOWER__LIMIT_1; softLowerLimitJoint[2] = MOTOR_LOWER__LIMIT_2; softLowerLimitJoint[3] = MOTOR_LOWER__LIMIT_3; serbo = true; manifactur = "SainSmart"; type = "DIY 4-Axis Servos Control Palletizing Robot Arm"; axisNum = 4; cmdCycle = 50; isGripper = false; //speedPoint = 10; //speedJointPos = 1; MaxSpeedJoint[0] = 2; MaxSpeedJoint[1] = 2; MaxSpeedJoint[2] = 2; MaxSpeedJoint[3] = 2; MaxSpeedCartesianTrans = 0.5; MaxSpeedCartesianRot = 2; MinTime = dt; jointOffset[0] = MOTOR_OFFSET_0; jointOffset[1] = MOTOR_OFFSET_1; jointOffset[2] = MOTOR_OFFSET_2; jointOffset[3] = MOTOR_OFFSET_3; }