#include <stdio.h>
#include <stdlib.h>
#include <iostream.h>
#include <math.h>
#include <termios.h>

#include "driver/PrTimer.h"
#include "driver/ServoToGo.h"
#include "GripperControl.h"

#define HZ	500

//ServoToGo *stg = new ServoToGo(3, 0x2A0);

GripperControl::GripperControl(ServoToGo *stg_card)
{
	stg = stg_card;

	stg->RawDAC(PGRIPPER_CHANNEL,STOP);
	stg->DioDirection(PORTA_DIR,1);	//set PORTA to output
	stg->RawDO(PGRIPPER_CHANNEL,1,CONTROL_PORT);	//set control bit to 1 to activate
													//PIC18F state machine
}

GripperControl::~GripperControl()
{
	stg->RawDAC(PGRIPPER_CHANNEL,0);
	stg->RawDO(PGRIPPER_CHANNEL,0,CONTROL_PORT);	//clear control bit to suspend 
													//PIC18F state machine
	printf("gripper destructor \n");

}

void GripperControl::gripperOpen(void)
{
	stg->RawDAC(PGRIPPER_CHANNEL,OPEN);
}
	
void GripperControl::gripperClose(void)
{
	stg->RawDAC(PGRIPPER_CHANNEL,CLOSE);
}

void GripperControl::gripperStop(void)
{
	stg->RawDAC(PGRIPPER_CHANNEL,STOP);
}

void GripperControl::gripperCommand(int goal)
{
	float temp;
	temp = ((float)(goal)/76.0)*2048.0;
//	printf("goal: %d, temp: %f \n", goal, temp);
	stg->RawDAC(PGRIPPER_CHANNEL,(short)(temp));

}


