//////////////////////////////////////////////////
// jmp.thd 
// This script simulates a jumping biped
// The simulation will consume quite some computing power, so be patient.
//////////////////////////////////////////////////
 
scene.backColor=RGB(192,192,128); 
scene.scale=100; 
scene.defaultLights(1);

// use degrees as metric for angles
factory.useDegrees = true;
 
// choose some default material
var mat = Material(650, 2e6, .999, 0);  
factory.material = mat;     
factory.strength = 1e4;   
// create a floor for the scene 
//var texfloor  = Texture("floor.jpg"); 

var flr = Box({1,.01,1},{0,-.005,0}); 
flr.static = true;
flr.color = RGB(255, 255, 0);
//flr.useTexture(texfloor  );
  
      
   


simulator.stepsize = 5e-4; 
var sc = 100; // spring constanten
 


// see case 1

class motor(load, dir)
{
	var N =  250 ; // gear ratio of servo 
	var Vin= 0;				// motor input voltage
	var Kb = 3.09607e-3;//5.0/837.0;  // 8000 rpm @ 5 V
	var Ra = 0.0715337; 	// armature resistance
	var Km = Kb ;	 	
	var aScale = 1.37706; // hoek naar 0.255 encoding
	var PT = 0;
	var PL = 5;	 
 	ivar ld = load;
	var motor_block = Box({.020,.070,.020},{0,.05,-.013}); 
	motor_block.color = RGB(0,128,0); 

	var motor_joint = FixedAxisJoint({.010, .010, .035},{0, .03, -.003});
	motor_joint.color = RGB(0,0,255);
	motor_joint.damperConstant = 13.0404e-3;  // motor friction 
 

 
	function set_torque()  // typical dc motor model
	{   
		var Vb = Kb*N *  load.rotVelocity[2]; 
		return   dir*(Vin  - Vb)*Km/Ra ;  
	};
 	motor_joint.torque =  set_torque;

	
	var pulse_clock =0;
	var target =0;
	var Pulse_width = 0;
	var actual = 0;
	var actual_load = 0;
// actual servo controller function, as implemented for the PIC16f872 controller
// runs every 1/150 second
	function do_target(t )
	{	  
		target = t;
	};
	function Update(a, b)
	{
		actual =  aScale*(  a.orientation[5] - b.orientation[5]) ;			
		actual_load=  aScale*(   load.orientation[5] - b.orientation[5]) ;
		var eT = target - actual;
		var eL = target - actual_load; 
		Vin  = PT *eT+ PL*eL ;
		if (Vin>5) Vin = 5; else if (Vin<-5) Vin=-5; 
		
	}; 
};
  
 
class Foot(z)
{
	factory.color=RGB(64,64,0);
	var h = Box({.10, .002, .02},{0.02,0,z}); 
 
	var v = Box({.03, .03, .002},{0,.015, 0.01+z}); 
};

class Lever
{ 
	var b = Box({.06,.02,.005},{0,.03,0.020});
	b.color = RGB(255,0,0);  
	var a = Cylinder({.01, .01, .001},{0,.03,0.02} ); 	// motor inertia * N
	a.rotMass = {2.6e-4, 2.6e-4, 2.6e-4};
	a.mass = 0;
 	a.visible = false;
};

class  Leg(ft,   y, z, mirr, bUp)
{
 	var bone = Box({.030,.100, .006},{0,.070+y,z} );			// load itself
	
	ivar lev = Lever(); 

	var m = motor(lev,mirr );  	
	m.move(0,y,z);
	if (bUp)
		m.N*=0.5;

	var ankle_joint = FixedAxisJoint({.010, .010, .025},{0, .02+y, z});
 
	ankle_joint.color = RGB(0,0,255);  
	ankle_joint.damperConstant = .005;
	
	if (mirr<0) // The current version of ThreeDImSim does not mirror joints, so we mirror it manually
	{
		self.mirror({0,0,1},{0,0,0});
		lev .mirror({0,0,1},{0,0,0});
 		ankle_joint.rotate(0,180,0); 
 		m.motor_joint.rotate(0,180,0); 
 
	}
 	m.motor_joint.attach(m.motor_block, m.load);
	ankle_joint.attach(bone, ft); 
 
	
	
 
	var s1 = Spring(lev.b, {0.03,0, 0}, ft, {0.03,y+.03, lev.b.position[2]});
	s1.springConstant = sc;
	s1.defaultLength = s1.length ;

	var s2 = Spring( lev.b, {-.03,0, 0}, ft, {-.03,y+.03, lev.b.position[2]});
	s2.springConstant = sc;
	s2.defaultLength = s2.length ;
 	if (bUp)
	{
		s1.springConstant *=3;
		s2.springConstant *=3;
	}
};
 

class Body(L, R)
{
	ivar left = L;		
	ivar right = R;	
 
	var b= Box({.05,.035,.09},{0,.2,0});
	var hipLeft =  FixedAxisJoint({.010, .010, .025},{0, .2 ,  -0.045});
	var hipRight =  FixedAxisJoint({.010, .010, .025},{0, .2 ,  0.045});
	hipLeft .color = RGB(0,0,255); 
	hipRight.color = RGB(0,0,255); 
	hipLeft .rotate(0,180,0);
	hipRight.attach(b, R);
	hipLeft .attach(b, L);  
};

 
flr.move(0,-.001,0 ); 

var footL = Foot(-.05);
var lowerlegL =  Leg(footL,0,-.05, 1,false);
var upperlegL =  Leg(lowerlegL,  .09, -.01-.05, 1, true);

var footR = Foot(-.05); 
footR .mirror({0,0,1},{0,0,0}); 
var lowerlegR =  Leg(footR,0, -.05, -1, false);
var upperlegR =  Leg(lowerlegR,  .09, -.01-.05, -1, true); 

var b = Body(upperlegL, upperlegR);
 
   
 
var iter_per_dt = 1000;
var dt = 1.0/60.0 ;
var step_size = dt /iter_per_dt; 

  
function Sim(t)
{
	var nSteps = t/step_size;
	while(nSteps>0)
	{
		lowerlegL.m.Update(footL, lowerlegL);	
		upperlegL.m.Update(lowerlegL, upperlegL);		
		lowerlegR.m.Update(footR, lowerlegR);	
		upperlegR.m.Update(lowerlegR, upperlegR);
		simulator.run(step_size );
	 	if ((nSteps % iter_per_dt)==0) 
			WaitFrame( );
		nSteps--;
	};
};

print ("Bending knees...", newline);
for (var k=1;k<45;k++)
{
	print ("k = ", k, newline);
	upperlegL.m.do_target(-1.95*k);   
	upperlegR.m.do_target(-1.95*k);   
 
	lowerlegL.m.do_target(k); 	
	lowerlegR.m.do_target(k); 
	Sim(2*dt ); 
};
  
print ("Wait...", newline);
	Sim(4.0*dt ); 	
print ("Jump!", newline);
for (k=45;k>=0;k-=15)
{
	print ("k = ", k, newline);
	upperlegL.m.do_target(-1.95*k);   
	upperlegR.m.do_target(-1.95*k);   
 
	lowerlegL.m.do_target(k); 	
	lowerlegR.m.do_target(k); 
	Sim(dt ); 
};	

Sim(.5);