//////////////////////////////////////////////////
// servo_estimate.thd
//  
// This file estimates motor-parameters
// from data obtained from a video of a
//////////////////////////////////////////////////

 

var gTarget = -2*0x40;  // swing to make , this was the input to the servo controller
var aScale = 1.4; // aScale scales the input unit to actual degrees

var gJ =  7;   // * .001 motor inertia
var gRa = 0.10; // internal motor resistance

var gKb = 3.5;    // * .001 back
var gDamper = 14 ;  // * .001  motor/gearset friction
var N =  250 ; // gear ratio of servo


var SIM_DT = .0001; // simulation stepsize

  
// set background color
scene.backColor=RGB(128,128,192); 
scene.gravity = {0,-9.810,0};  //mm/s2
scene.scale=2000;
scene.defaultLights(1,1);
// use degrees as metric for angles
factory.useDegrees = true;  
 
// choose some default material
var mat = Material(650, .2, .9, 0.1);  
factory.material = mat;    
var virtual_load_mat = Material(650, .2, .9, 0.1);  
 
var sim_data = array(); 

// measured data of the angle of the lever @ 30 fps = 1/30 sec
var data = array(-45.83031549,-45.83031549,
-40.85537626,
-34.9920202,
-27.64597536,
-18.43494882,
-9.865806943,
-1.193489424,
8.130102354,
17.28149837,
26.02959219,
34.01934999,
41.63353934,
48.36646066,
55.30484647,
56.97613244,
55.30484647,
50.9644871,
48.36646066,
44.16968451,
43.31531568,
44.16968451,
45,
46.68468432,
48.36646066,
50.04245107,
50.04245107,
49.2678933,
48.36646066,
48.36646066,
46.68468432,
45.881404); 

var startAngle = data[0];
class Load
{
	var sz = sqrt ( N ) * gJ/1000.0; 

	var a = Cylinder({sz, sz, .01},{0,.025,0.01} ); 	// motor inertia * N
	a.visible=false;

 	var b = Box({.030,.170, .006},{0,.100,0} );			// load itself

 	b.color = RGB(128,128,0,128);
};

var deltaThreshold =16; // motor control 
var deltaFactor = 256/deltaThreshold ;

class motor(load)
{
	var Vin= 0;				// motor input voltage
	var Kb = gKb /1000.0;//5.0/837.0;  // 8000 rpm @ 5 V
	var Ra = gRa ;//.6;			// armature resistance
	var Km = Kb ;			
	ivar motor_load = load;
 	var PWM_switch = false;
	var motor_block = Box({.020,.070,.020},{0,0,-.020}); 
	motor_block.color = RGB(0,128,0); 

	var motor_joint = FixedAxisJoint({.010, .010, .025},{0, .025, -.010});
	motor_joint.color = RGB(0,0,255);
	motor_joint.damperConstant = gDamper /1000.0;  // motor friction
	 
 	motor_joint.attach(motor_block, motor_load);

	var KbN = Kb*N;
	var KmRa = Km/Ra;
	function set_torque()  // typical dc motor model
	{ 
		if (PWM_switch)
			return (  (Vin   - KbN  * motor_load.rotVelocity[2] )*KmRa    ) ; 
		else 
			return 0; 
	};
	motor_joint.torque =  set_torque;
	motor_joint.strength = 10;	


// actual servo controller function, as implemented for the PIC16f872 controller
// runs every 1/150 second
	function do_target(target )
	{	 
 		
		KbN = Kb*N;
		KmRa = Km/Ra;

		var iStable = 0;
		while(iStable <20)
		{
 
	 		//WaitFrame( );	
 			
			sim_data.append(startAngle - motor_load.orientation[5] );
			var Pulse_width=0;
			for (var i=0;i<5;i++)
			{ 		

				var actual = floor( aScale*motor_load.orientation[5]+0.5);
 			 	// this is the motor control algoritm used
				if (actual  > target)
				{				
					iStable=0;
					var delta = actual  - target;
					if (delta < deltaThreshold )
						 Pulse_width =  (delta *deltaFactor );
					else			
						Pulse_width = 255;
					Vin = -5.0;
				
				}
				else if (actual  < target)		
				{	
					iStable=0;
					var delta = target - actual;
					if (delta < deltaThreshold )
						Pulse_width = (delta *deltaFactor );
					else			
						Pulse_width = 255;					
					Vin = 5.0;
				}
				else 
				{
					iStable++;
					 Vin = 0;
					Pulse_width = 0;
				}
				// end of control algoritm
				// simulate pulse-width modulation
				var step_size = 1.0/150.0;
				var pwm_step_size = step_size/255;				
				PWM_switch = true; // on
				// simulate on state
				simulate( pwm_step_size*Pulse_width, SIM_DT, SIM_RK4 ); 
				PWM_switch = false; // off
				// simulate off state
				simulate( pwm_step_size*(255-Pulse_width), SIM_DT, SIM_RK4 );	
			 //	WaitFrame(false );
			}

 
			if (sim_data.count()>= data.count() ) break; // .. no more data needed
		} 

	}; 
};
  


 
// simulation data may be shifted in time by Toffset, range [-1..1]
// The fit is minimized for Toffset, and this result is used for further parameter optimization
 
var Toffset=0;
 
function f_fit()
{
	if (sim_data.count()<data.count())
		return 1e9;
	var sum=0;
	var Toff = max(-1, min(1,Toffset)); 
	if (Toff >=0)
	{
 		for (var i=1;i < data.count()-1;i++)
		{
			var y = sim_data[i]*(1.0-Toff )+sim_data[i+1]*Toff ; // interpolation
			sum +=  sqr(y - data[i] );		 
 		}
 	}
	else
	{
		Toff = 0.0 - Toff;
		for (var i=1;i < data.count()-1;i++)
		{
			var y = sim_data[i]*(1.0-Toff )+sim_data[i-1]*Toff ;// interpolation
			sum +=  sqr(y - data[i] );		 
 		}
	}
	return sum +1000*sqr(Toff - Toffset);// plus penalty for Toffset exceeding bounds
};

// function that performs a simulation of a full swing
// and returns the fit of simulation result to the given dataset
function Run()
{
	if (gJ<=0 || gRa<=0 || gDamper  < 0 || gKb<=0)
		return 1e10;

	sim_data.clear();
	var  lever = Load(); 
	var  m = motor(lever); 
	m.static = true; 
	lever.static = false;	
  	
	print ("do target with Kb = ",gKb ,", Ra = ",gRa, ", gJ = ", gJ,  ", gDamper = ", gDamper , ", aScale = ", aScale );
 	
// initial position of lever
	lever.rotateAround(m.motor_joint, -startAngle ); 
	
	// goto new position
	m.do_target( gTarget );  	 
	Toffset = 0.5;
 	var s = solve(SOLVE_MIN, f_fit, 1e-6, 100, Toffset); 
 	print (", Xoffset = ", Toffset, " fit = ", s, newline); 	 
	return s;
};


var dt={.001,   .0001 };

for (var q = 0; q< dt.count(); q++)
{
	 SIM_DT=dt[q]; // simulation stepsize
// and here we estimate the parameters we're interested in 
	var result = solve(SOLVE_MIN, Run, 1e-6,100,  gKb , gRa, gJ, gDamper, aScale ); // estimate motor parameters
  	print ("dt=", SIM_DT, ": Result = ", result ,", Kb = ",gKb ,", Ra = ",gRa, ", gJ = ", gJ,  ", gDamper = ", gDamper   , newline ); 
};