void vmWishboneCar::simForward(dReal mphSpeed) { dReal dsteer, realSpeed; //dsteer = 0.0 - dJointGetHinge2Angle1(frWheel.joint); // target at zero steer // calculate wheel real speed dReal siSpeed= mphSpeed*1.6/3600.0*1000; // convert ot m/s realSpeed = -siSpeed/frWheel.radius; // steer /*dJointSetHinge2Param(frWheel.joint, dParamVel, steerGain*dsteer+0.01*dsteer/0.01); dJointSetHinge2Param(frWheel.joint, dParamFMax, 1000.0); dJointSetHinge2Param (frWheel.joint,dParamLoStop,-40.0*M_PI/180.0); dJointSetHinge2Param (frWheel.joint,dParamHiStop,40.0*M_PI/180.0); dJointSetHinge2Param (frWheel.joint,dParamFudgeFactor,0.1); dJointSetHinge2Param(flWheel.joint, dParamVel, steerGain*dsteer+0.01*dsteer/0.01); dJointSetHinge2Param(flWheel.joint, dParamFMax, 1000.0); dJointSetHinge2Param (flWheel.joint,dParamLoStop,-40.0*M_PI/180.0); dJointSetHinge2Param (flWheel.joint,dParamHiStop,40.0*M_PI/180.0); dJointSetHinge2Param (flWheel.joint,dParamFudgeFactor,0.1);*/ // speed if (brakeYes) { dReal factor= 0.1; dJointSetHingeParam(rrSuspension.jRotorMid, dParamVel, dJointGetHingeParam(rrSuspension.jRotorMid,dParamVel)*factor); dJointSetHingeParam(rrSuspension.jRotorMid, dParamFMax, 1000.0); dJointSetHingeParam(rlSuspension.jRotorMid, dParamVel, dJointGetHingeParam(rlSuspension.jRotorMid,dParamVel)*factor); dJointSetHingeParam(rlSuspension.jRotorMid, dParamFMax, 1000.0); dJointSetHingeParam(frSuspension.jRotorMid, dParamVel, realSpeed);//dJointGetHinge2Param(frSuspension.jRotorMid,dParamVel2)*factor); dJointSetHingeParam(frSuspension.jRotorMid, dParamFMax, 1000.0); dJointSetHingeParam(flSuspension.jRotorMid, dParamVel, realSpeed);//dJointGetHinge2Param(flSuspension.jRotorMid,dParamVel2)*factor); dJointSetHingeParam(flSuspension.jRotorMid, dParamFMax, 1000.0); } else { // turn off rear wheels dJointSetHingeParam(rrSuspension.jRotorMid, dParamFMax, 0.0); dJointSetHingeParam(rlSuspension.jRotorMid, dParamFMax, 0.0); // set up front wheels dJointSetHingeParam(frSuspension.jRotorMid, dParamVel, realSpeed); dJointSetHingeParam(frSuspension.jRotorMid, dParamFMax, 500.0); dJointSetHingeParam(flSuspension.jRotorMid, dParamVel, realSpeed); dJointSetHingeParam(flSuspension.jRotorMid, dParamFMax, 500.0); } }
static float get_phys_joint_attr(dJointID j, int p) { switch (dJointGetType(j)) { case dJointTypeHinge: return (float) dJointGetHingeParam (j, p); case dJointTypeSlider: return (float) dJointGetSliderParam (j, p); case dJointTypeHinge2: return (float) dJointGetHinge2Param (j, p); case dJointTypeUniversal: return (float) dJointGetUniversalParam(j, p); default: return 0.0f; } }
void vmWishboneCar::simControl() { // auto align steering angle if (manualYes) steerGain= 10; else { steerGain= 10; steer*= 0.98; speed*= 0.999; } dReal dsteer, realSpeed; //dsteer = bounded(steer,-40.0,40.0) - dJointGetHingeAngle(frWheel.joint); realSpeed = -bounded(speed, -14*M_PI, 30*M_PI); // steer /*dJointSetHingeParam(frWheel.joint, dParamVel, steerGain*dsteer+0.01*dsteer/0.01); dJointSetHingeParam(frWheel.joint, dParamFMax, 1000.0); dJointSetHingeParam (frWheel.joint,dParamLoStop,-40.0*M_PI/180.0); dJointSetHingeParam (frWheel.joint,dParamHiStop,40.0*M_PI/180.0); dJointSetHingeParam (frWheel.joint,dParamFudgeFactor,0.1); dJointSetHinge2Param(flWheel.joint, dParamVel, steerGain*dsteer+0.01*dsteer/0.01); dJointSetHinge2Param(flWheel.joint, dParamFMax, 1000.0); dJointSetHinge2Param (flWheel.joint,dParamLoStop,-40.0*M_PI/180.0); dJointSetHinge2Param (flWheel.joint,dParamHiStop,40.0*M_PI/180.0); dJointSetHinge2Param (flWheel.joint,dParamFudgeFactor,0.1);*/ // speed if (brakeYes) { dReal factor= 0.1; dJointSetHingeParam(rrSuspension.jRotorMid, dParamVel ,dJointGetHingeParam(rrSuspension.jRotorMid,dParamVel)*factor); dJointSetHingeParam(rrSuspension.jRotorMid, dParamFMax, 1000.0); dJointSetHingeParam(rlSuspension.jRotorMid, dParamVel ,dJointGetHingeParam(rlSuspension.jRotorMid,dParamVel)*factor); dJointSetHingeParam(rlSuspension.jRotorMid, dParamFMax, 1000.0); dJointSetHingeParam(frSuspension.jRotorMid, dParamVel, realSpeed);//dJointGetHinge2Param(frWheel.joint,dParamVel2)*factor); dJointSetHingeParam(frSuspension.jRotorMid, dParamFMax, 1000.0); dJointSetHingeParam(flSuspension.jRotorMid, dParamVel, realSpeed);//dJointGetHinge2Param(flWheel.joint,dParamVel2)*factor); dJointSetHingeParam(flSuspension.jRotorMid, dParamFMax, 1000.0); } else { // turn off rear wheels dJointSetHingeParam(rrSuspension.jRotorMid, dParamFMax, 0.0); dJointSetHingeParam(rlSuspension.jRotorMid, dParamFMax, 0.0); // set up front wheels dJointSetHingeParam(frSuspension.jRotorMid, dParamVel, realSpeed); dJointSetHingeParam(frSuspension.jRotorMid, dParamFMax, 500.0); dJointSetHingeParam(flSuspension.jRotorMid, dParamVel, realSpeed); dJointSetHingeParam(flSuspension.jRotorMid, dParamFMax, 500.0); //printf("real speed: %f", realSpeed); } // reset manual mode flag manualYes= 0; }
void PhysicsHingeJoint::changed(ConstFieldMaskArg whichField, UInt32 origin, BitVector details) { //Do not respond to changes that have a Sync origin if(origin & ChangedOrigin::Sync) { return; } if(whichField & WorldFieldMask) { if(_JointID) { dJointDestroy(_JointID); _JointID = dJointCreateHinge(getWorld()->getWorldID(), 0); } else { _JointID = dJointCreateHinge(getWorld()->getWorldID(), 0); if(!(whichField & HiStopFieldMask)) { setHiStop(dJointGetHingeParam(_JointID,dParamHiStop)); } if(!(whichField & LoStopFieldMask)) { setLoStop(dJointGetHingeParam(_JointID,dParamLoStop)); } if(!(whichField & BounceFieldMask)) { setBounce(dJointGetHingeParam(_JointID,dParamBounce)); } if(!(whichField & CFMFieldMask)) { setCFM(dJointGetHingeParam(_JointID,dParamCFM)); } if(!(whichField & StopCFMFieldMask)) { setStopCFM(dJointGetHingeParam(_JointID,dParamStopCFM)); } if(!(whichField & StopERPFieldMask)) { setStopERP(dJointGetHingeParam(_JointID,dParamStopERP)); } } } Inherited::changed(whichField, origin, details); if((whichField & AnchorFieldMask) || (whichField & WorldFieldMask)) { dJointSetHingeAnchor(_JointID, getAnchor().x(), getAnchor().y(), getAnchor().z()); } if((whichField & AxisFieldMask) || (whichField & WorldFieldMask)) { dJointSetHingeAxis(_JointID, getAxis().x(), getAxis().y(), getAxis().z()); } if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask)) { dJointSetHingeParam(_JointID, dParamHiStop, getHiStop()); } if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask)) { dJointSetHingeParam(_JointID, dParamLoStop, getLoStop()); } if((whichField & BounceFieldMask) || (whichField & WorldFieldMask)) { dJointSetHingeParam(_JointID, dParamBounce, getBounce()); } if((whichField & CFMFieldMask) || (whichField & WorldFieldMask)) { dJointSetHingeParam(_JointID, dParamCFM, getCFM()); } if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask)) { dJointSetHingeParam(_JointID, dParamStopERP, getStopERP()); } if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask)) { dJointSetHingeParam(_JointID, dParamStopCFM, getStopCFM()); } }
void VelocityMotor::VelocitySensor::updateValue() { data.floatValue = dJointGetType(joint->joint) == dJointTypeHinge ? dJointGetHingeParam(joint->joint, dParamVel) : dJointGetSliderParam(joint->joint, dParamVel); }