dNewtonJointDoubleHinge::dNewtonJointDoubleHinge(const dMatrix pintAndPivotMatrix, void* const body0, void* const body1)
	:dNewtonJoint()
{
	dMatrix bodyMatrix;

	NewtonBody* const netwonBody0 = (NewtonBody*)body0;
	NewtonBody* const netwonBody1 = (NewtonBody*)body1;
	NewtonBodyGetMatrix(netwonBody0, &bodyMatrix[0][0]);

	dMatrix matrix(pintAndPivotMatrix * bodyMatrix);
	dCustomDoubleHinge* const joint = new dCustomDoubleHinge(matrix, netwonBody0, netwonBody1);
	SetJoint(joint);
}
dNewtonJointDoubleHingeActuator::dNewtonJointDoubleHingeActuator(const dMatrix pintAndPivotMatrix, void* const body0, void* const body1)
	:dNewtonJoint()
{
	dMatrix bodyMatrix;
	NewtonBody* const netwonBody0 = (NewtonBody*)body0;
	NewtonBody* const netwonBody1 = (NewtonBody*)body1;
	NewtonBodyGetMatrix(netwonBody0, &bodyMatrix[0][0]);

	dMatrix matrix(pintAndPivotMatrix * bodyMatrix);
	dCustomDoubleHingeActuator* const joint = new dCustomDoubleHingeActuator(matrix, netwonBody0, netwonBody1);
	SetJoint(joint);
	//joint->SetEnableFlag0(true);
	//joint->SetEnableFlag1(true);
}
Esempio n. 3
0
void Stable(void)
{
	static double gval[3];
	static double pre_pitch, pitch, pre_roll, roll;
	int i;
	if(timer_nowtime() - STABLETIMER > FRESHTIME)
	{
		ReadGSensor();
		gval[0] = (double)(G_AXIS_VALUE[0]);
		gval[1] = (double)(G_AXIS_VALUE[1]);
		gval[2] = (double)(G_AXIS_VALUE[2]);
		pitch = (GetPitch(gval)- 5.0*DEG_TO_RAD)*0.5 + pre_pitch*0.5;
		roll = (GetRoll(gval) - 4.5*DEG_TO_RAD)*0.5 + pre_roll*0.5;
	
	
		BODY_PITCH = BODY_PITCH + STABLE_P*pitch + (pitch - pre_pitch)*STABLE_D/FRESHTIME;
		BODY_ROLL = BODY_ROLL + STABLE_P*roll + (roll - pre_roll)*STABLE_D/FRESHTIME;

		if(BODY_PITCH > 60.0*DEG_TO_RAD)
			BODY_PITCH = 60.0*DEG_TO_RAD;
		else if(BODY_PITCH < -60.0*DEG_TO_RAD)
			BODY_PITCH = -60.0*DEG_TO_RAD;
		if(BODY_ROLL > 60.0*DEG_TO_RAD)
			BODY_ROLL = 60.0*DEG_TO_RAD;
		else if(BODY_ROLL < -60.0*DEG_TO_RAD)
			BODY_ROLL = -60.0*DEG_TO_RAD;
			
		
		for(i =0 ; i < 6; i++ )
		{
			LEGPOS[0] = LEGGOAL[3*i] - cos(J3MAP[i])*20.0;
			LEGPOS[1] = LEGGOAL[3*i+1] - sin(J3MAP[i])*20.0;
			LEGPOS[2] = LEGGOAL[3*i+2];
			Transform(BODY_YAW, BODY_PITCH, BODY_ROLL, LEGPOS);
			AdjustPos(POS_GAIN, LEGPOS);
			CenterToJoint3(LEGPOS,i,J3GOAL);
			Joint3ToJoint2(J3GOAL, J2GOAL);
			JointAngle(LEGPOS, J2GOAL,i,JANGLE);
			SetJoint(JANGLE,i);	
			rcservo_SetAction(POSITION, STABLETIME);
			STABLETIMER = timer_nowtime(); 
		}
		pre_pitch = pitch;
		pre_roll = roll;
	}
	rcservo_PlayAction();
}
Esempio n. 4
0
void Idle(void)
{
	int i;
	double temp_p, temp_r;
	if(rcservo_PlayAction() == RCSERVO_PLAYEND)
	{
		for(i =0 ; i < 6; i++ )
		{
			LEGPOS[0] = LEGGOAL[3*i];
			LEGPOS[1] = LEGGOAL[3*i+1];
			LEGPOS[2] = LEGGOAL[3*i+2];
			if(SWING == 1)
			{
				ALPHA = ALPHA + PI/36.0;
				if(ALPHA >= 2*PI - PI/36.0)
					ALPHA = 0.0;
				temp_p = BODY_PITCH + gain_p*sin(ALPHA)*DEG_TO_RAD;
				temp_r = BODY_ROLL+ gain_r*sin(ALPHA+PI/2)*DEG_TO_RAD;
			}
			else
			{
				temp_p = BODY_PITCH;
				temp_r = BODY_ROLL;
			}
			Transform(BODY_YAW, temp_p, temp_r, LEGPOS);
			AdjustPos(POS_GAIN, LEGPOS);
			CenterToJoint3(LEGPOS,i,J3GOAL);
			Joint3ToJoint2(J3GOAL, J2GOAL);
			JointAngle(LEGPOS, J2GOAL,i,JANGLE);
			SetJoint(JANGLE,i);	
			rcservo_SetAction(POSITION, IDLEPLAYTIME);
			
		}
		IDLEFRAME = 0;
	}
	rcservo_PlayAction();
}
Esempio n. 5
0
void SetMotion(int num)
{
	double temp_p, temp_r;
	GetLegMotion(LEGOPPOSITE, LEGGOAL, num, LEGFRAME[num],LEGPOS);
	if(SWING == 1)
	{
		ALPHA = ALPHA + PI/36.0;
		if(ALPHA >= 2*PI - PI/36.0)
			ALPHA = 0.0;
		temp_p = BODY_PITCH + gain_p*sin(ALPHA)*DEG_TO_RAD;
		temp_r = BODY_ROLL+ gain_r*sin(ALPHA+PI/2)*DEG_TO_RAD;
	}
	else
	{
		temp_p = BODY_PITCH;
		temp_r = BODY_ROLL;
	}
	Transform(BODY_YAW, temp_p, temp_r, LEGPOS);
	AdjustPos(POS_GAIN, LEGPOS);
	CenterToJoint3(LEGPOS,num,J3GOAL);
	Joint3ToJoint2(J3GOAL, J2GOAL);
	JointAngle(LEGPOS, J2GOAL,num,JANGLE);
	SetJoint(JANGLE,num);	
}
dNewtonHingeJoint::dNewtonHingeJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1)
	:dNewtonJoint(m_hinge)
{
	SetJoint (new dCustomHinge (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL));
}
dNewtonBallAndSocketJoint::dNewtonBallAndSocketJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1)
    :dNewtonJoint(m_ballAndSocket)
{
    SetJoint (new dCustomBallAndSocket (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL));
}
dNewtonGearAndRackJoint::dNewtonGearAndRackJoint(dFloat ratio, const dFloat* const body0Pin, dNewtonDynamicBody* const body0, const dFloat* const body1Pin, dNewtonDynamicBody* const body1)
    :dNewtonJoint(m_gearAndRack)
{
    SetJoint (new dCustomRackAndPinion (ratio, body0Pin, body1Pin, body0->GetNewtonBody(), body1->GetNewtonBody()));
}
dNewtonPulleyJoint::dNewtonPulleyJoint(dFloat ratio, const dFloat* const body0Pin, dNewtonDynamicBody* const body0, const dFloat* const body1Pin, dNewtonDynamicBody* const body1)
    :dNewtonJoint(m_pulley)
{
    SetJoint (new dCustomPulley (ratio, body0Pin, body1Pin, body0->GetNewtonBody(), body1->GetNewtonBody()));
}
Esempio n. 10
0
dNewtonGearJoint::dNewtonGearJoint(dFloat ratio, const dFloat* const body0Pin, dNewtonDynamicBody* const body0, const dFloat* const body1Pin, dNewtonDynamicBody* const body1)
    :dNewtonJoint(m_gear)
{
    SetJoint (new dCustomGear (ratio, body0Pin, body1Pin, body0->GetNewtonBody(), body1->GetNewtonBody()));
}
Esempio n. 11
0
dNewtonCylindricalJoint::dNewtonCylindricalJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1)
    :dNewtonJoint(m_cylindrical)
{
    SetJoint (new dCustomCorkScrew (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL));
}
Esempio n. 12
0
dNewtonDoubleHinge::dNewtonDoubleHinge(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1)
	:dNewtonJoint(m_universal)
{
	SetJoint (new dCustomDoubleHinge (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL));
}
Esempio n. 13
0
dNewtonSliderJoint::dNewtonSliderJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1)
	:dNewtonJoint(m_slider)
{
	SetJoint (new dCustomSlider (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL));
}