#ifndef _TRAPSERVO
#define _TRAPSERVO
#include "dwarf.h"
#define VEL_LIMIT	120	
#define INT_LIMIT	2500

		
typedef struct
{
	byte channel;	 		//servo channel
	byte optionMask;
	byte servoStatus;		//status of move, limits, current
	
	int32 position;  		//absolute position (not encoder reading)
	int32 lastPosition; 	//previous value of intermediate
	int32 goalPosition; 	//desired final position
	int32 relative;			//size of relative move in encoder counts
	int32 intermediate;	 	//desired position for next servo cycle
	int32 maxPositionErr;

	uint16 maxSpeed; 		//plateau speed
	uint16 minSpeed;
	int16 cmdVel;	 		//current velocity
	int32 accel;	 		//constant accel rate 
	int8 stopFlag;			//deceleration flag
	
	int16PID gain;	
	int32 sum;		 		//sum of error for integral term
	int32 lastError;		//previous error for derivative term
	
	int16 motorCurrent;		//motor current in counts
	uint8 maxForce;
	int8  maxPressureDelta;	//max difference between pressures
	uint8 maxPressure[2];	//max acceptable absolute pressure
	uint8 minPressure[2];	//min acceptable absolute pressure
	
}MotorChannel;

typedef struct
{
	byte	ServoConfigure;
	byte	optionMask;
	int16PID	gain;
	uint16	maxSpeed;	//does this need to be 24bits??
	uint16	minSpeed;
	int32	maxPositionErr;
	uint8	maxForce;
	int8	maxPressureDelta;
	uint8	maxPressure[2];
	uint8	minPressure[2];	
}ServoConfigureReply;
typedef struct
{
	byte	ServoConfigure;
	byte	optionMask;
	int16PID	gain;
	uint16	maxSpeed;	//does this need to be 24bits??
	uint16	minSpeed;
	int32	maxPositionErr;
	uint8	maxForce;
	int8	maxPressureDelta;
	uint8	maxPressure[2];
	uint8	minPressure[2];		
}ServoConfigureMsg;

void getNewPos(MotorChannel* mc);
void haltMotor(MotorChannel* mc,unsigned char gMISCout);
void trapServoLoop(MotorChannel* mc);
void configMotorChannel(MotorChannel* mc);
void startmove(long int relative,MotorChannel* mc);
void getMotorConfig(MotorChannel* mc,ServoConfigureReply* scr);
void configureMotorChannel(MotorChannel* mc, ServoConfigureReply* scr);


#endif