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