//------------------------------------------------------------------
// control_struct.h:  contains the complete structure for low-level
// open-loop motions through full force feedback.  Values are
// reinitialized from file whenever the user leaves quiescent
// state.
//
// fsh & aab 4 Feb 1998
//------------------------------------------------------------------

#define MAX_OPTIONS 3

//-----------------------------------------------------------------
// 1. Open-loop motions always precede posing and position control.
//-----------------------------------------------------------------

struct ol_st {
	int STEP ;		// if 1, then three Voltage steps occur
				// in .._t; otherwise, a sinusoid is applied.
				// else, use sinusoids at _freq
	double amp[3] ;		// amplitude, Volts
	double t[3]  ;		// time of the open-loop period, s	
	double freq[3]  ;	// frequency, rad/s
} ;

//---------------------------------------------------------------------------
// 2. Posing always follows open-loop motions, and precedes position control.
//---------------------------------------------------------------------------

struct pose_st {
	double enc[MAX_OPTIONS][3] ;		
	double t  ;		// time to pose
} ;

//------------------------------------------------------------------
// 3. Position control always follows open-loop motions, and posing.
//------------------------------------------------------------------

struct move_st {
	int CARTESIAN[MAX_OPTIONS] ; 	// if 1, amp(0,1) is interpreted
						//     in meters (x,y), and (2) not used.  
	double amp[MAX_OPTIONS][3] ;		// used in both control.c and t2.c
	double freq[MAX_OPTIONS][3] ;	// rad/s of sinusoids 
	double env_tau  ;	// tau of low-pass used in buildup	
	double phase[MAX_OPTIONS][3] ;	// phase in each channel, radians
} ;

//------------------------------
// Lower level control settings.
//------------------------------

struct servo_st {
	double dither_amp[3] ;
	double dither_freq[3] ;
	double kp[3] ;	
	double kd[3] ;
	double kdd[3] ;
	double panto[MAX_OPTIONS] ;			// force feedback: rad to m
	double pantod ;
	double vel_ffwd[3] ;		// takes rad/s (traj_dot) to volts
	double max_speed[3] ;		// max speed in trajgen, rad/s
	double tau[MAX_OPTIONS][3] ;			// time constant of trajectory, s 
	double max_spool ;	// maximum output volts
} ;

//------------
// Robot stuff
//------------
struct robot_st {
	double l[3] ;		// link lengths, m
	double sign_s2 ;	// which inverse kinematic solution:
				// controls the sign of the distal angle.
				// use plus or minus 1.0
	int ii ;	// either 0 (hip) or 1 (knee); index of
				// the inboard link for inverse kinematics.
} ;

//------------------------------------------------------------------
// Pass the structure p, or parts of it, to the various subroutines.
//------------------------------------------------------------------

struct p_st {
	int DEBUG ;		// some error messages are printed if 1
	struct ol_st ol ;
	struct pose_st pose ;
	struct move_st move ;
	struct servo_st servo ;
	struct robot_st robot ;
} ;

//???? static double xygoal[2] ;	// dummy variable used in cartesian control 
//???? double pose_enc[2] = {0.,.1} ;	// t2.c writes onto this; then used in control.c

#include <math.h>
 
//-----------------------
// function prototypes 
//-----------------------

double unwrap(double) ;  
double sign(double) ;
double ssat(double,double) ;
void get_goal(double, double[],double[],struct p_st,double,double[],int,
	double);
void get_traj(double[], double[], double[], double, double[], 
		double[],double,struct p_st, int) ;
void open_loop(double,double[], struct ol_st);
void get_spool(double[], double[], double[], double[], 
	       struct servo_st, double[], double) ;
