// Functions related to HSR Servos

//To use these functions properly, you need to copy and paste the code below 
//to your "main.c"
/*////////////////////////////////////////

#include "hsr_serial.c"

#int_rda
void serial_isr() 
{
  if(kbhit())
 	{
   	RxData[RxCounter]=getc();
    RxCounter++;
	RxCounter%=3;
	   if (RxCounter==0)
	   {
	     	RxLastData[0]=RxData[0];
			RxLastData[1]=RxData[1];
	        RxComplete=1;
	   }
	   else
	   {
	  		RxComplete=0;
	   }
 	}   
}


*/////////////////////////////////////////
#include <regloc.h> // Special Register Locations
#include <hsr_serial.h> // Special commands and memory locations for HSR servos 
void HSR_SerialInitialization(void)
{
   	//Disable Transmission and Receive
   	CREN=0;
   	TXEN=0;
    
  	//Configure both transmission and receive pins as input 
    TRISC |=0b11000000;
   
    enable_interrupts(int_rda);
    enable_interrupts(global);

}

//Release Servo
void HSR_Release(void)
{
   _RxComplete=0; //The data is not available

   _CheckSum=_HSR_ComStart+_HSR_Release;
   _CheckSum=(~_CheckSum)+1;

   CREN = 0;
   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_Release,0x00,0x00,_CheckSum); 
   CREN = 1;
   TXEN = 0;
   
   WaitRxDataReady();
  

}

//Release Servo
void HSR_Release_v2(void)
{
  
   _CheckSum=_HSR_ComStart+_HSR_Release;
   _CheckSum=(~_CheckSum)+1;
   TXEN=1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_Release,0x00,0x00,_CheckSum); 
   TXEN=0;

}

//This function reads the servo position 
uint16 HSR_ReadPosition(void)
{
 	   _RxComplete=0; //The data is not available
       _CheckSum=_HSR_ComStart+_HSR_ReadPosition;
	   _CheckSum=(~_CheckSum)+1;
	   
       CREN=0;
       TXEN=1;
	   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_ReadPosition,0x00,0x00,_CheckSum);
       CREN=1;
       TXEN=0;

	   WaitRxDataReady();
       _ServoPosH = _RxLastData[0];
       _ServoPosL = _RxLastData[1];

       return(make16(_RxLastData[0],_RxLastData[1]));
       
}

//This function only starts to reading current position of servo.
//User need to be check if the data is ready using variable _RxComplete
void HSR_ReadPosition_v2(void)
{
 	   _RxComplete=0; //The data is not available
       _CheckSum=_HSR_ComStart+_HSR_ReadPosition;
	   _CheckSum=(~_CheckSum)+1;
	   
       CREN=0;
       TXEN=1;
	   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_ReadPosition,0x00,0x00,_CheckSum);
       CREN=1;
       TXEN=0;
}

//This function returns if incoming data is ready (1), or not (0). 
short IsRxDataReady(void)
{
       return(_RxComplete);
}

//This function waits until incomming data is ready.

void WaitRxDataReady(void)
{   
	_Start_time =get_timer0();
    while(!_RxComplete)
	{ if (get_timer0()-_Start_time >_Reset_interval)
		{
			output_low(Pin_D5);
			output_low(Pin_D6);
			output_low(Pin_D7);
			break;
		}
	}
}

//Read a Specific EPPROM location
uint8 HSR_ReadEPPROM(uint8 addr)
{
 	   _RxComplete=0; //The data is not available
       _CheckSum=_HSR_ComStart+_HSR_ReadEPPROM+addr;
	   _CheckSum=(~_CheckSum)+1;
	   
       CREN=0;
       TXEN=1;
	   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_ReadEPPROM,addr,0x00,_CheckSum);
       CREN=1;
       TXEN=0;
      
       WaitRxDataReady(); // Wait until receive is completed.
       return(_RxLastData[0]);
}

//This function only starts to reading from EPPROM.
//User need to be check if the data is ready using variable _RxComplete
void HSR_ReadEPPROM_v2(uint8 addr)
{
 	   _RxComplete=0; //The data is not available
       _CheckSum=_HSR_ComStart+_HSR_ReadEPPROM+addr;
	   _CheckSum=(~_CheckSum)+1;
	   
       CREN = 0;
       TXEN = 1;
	   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_ReadEPPROM,addr,0x00,_CheckSum);
       CREN = 1;
       TXEN = 0;
     
}

//Write EPPROM
void HSR_WriteEPPROM(uint8 addr, uint8 data)
{
   _RxComplete=0; //The data is not available

   _CheckSum = _HSR_ComStart + _HSR_WriteEPPROM + addr + data;
   _CheckSum = (~_CheckSum) + 1;

   CREN = 0;
   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_WriteEPPROM,addr,data,_CheckSum); 
   CREN = 1;
   TXEN = 0;

   WaitRxDataReady();

}

//Write EPPROM
void HSR_WriteEPPROM_v2(uint8 addr, uint8 data)
{
  
   _CheckSum = _HSR_ComStart + _HSR_WriteEPPROM + addr + data;
   _CheckSum = (~_CheckSum) + 1;
   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_WriteEPPROM,addr,data,_CheckSum); 
   TXEN = 0;

}

//Read a Specific Memory location
uint8 HSR_ReadMemory(uint8 addr)
{
 	   _RxComplete=0; //The data is not available
       _CheckSum=_HSR_ComStart+_HSR_ReadMemory+addr;
	   _CheckSum=(~_CheckSum)+1;
	   
       CREN=0;
       TXEN=1;
	   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_ReadMemory,addr,0x00,_CheckSum);
       CREN=1;
       TXEN=0;
      
       WaitRxDataReady(); // Wait until receive is completed.
       return(_RxLastData[0]);
}

//This function only starts to reading memory.
//User need to be check if the data is ready using variable _RxComplete
void HSR_ReadMemory_v2(uint8 addr)
{
 	   _RxComplete=0; //The data is not available
       _CheckSum=_HSR_ComStart+_HSR_ReadMemory+addr;
	   _CheckSum=(~_CheckSum)+1;
	   
       CREN=0;
       TXEN=1;
	   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_ReadMemory,addr,0x00,_CheckSum);
       CREN=1;
       TXEN=0;
     
}

//Write Memory
void HSR_WriteMemory(uint8 addr, uint8 data)
{
   _RxComplete=0; //The data is not available

   _CheckSum=_HSR_ComStart+_HSR_WriteMemory+addr+data;
   _CheckSum=(~_CheckSum)+1;

   CREN=0;
   TXEN=1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_WriteMemory,addr,data,_CheckSum); 
   CREN=1;
   TXEN=0;

   WaitRxDataReady(); // Wait until receive is completed.

}

//Write Memory
void HSR_WriteMemory_v2(uint8 addr, uint8 data)
{
   _CheckSum=_HSR_ComStart+_HSR_WriteMemory+addr+data;
   _CheckSum=(~_CheckSum)+1;

   TXEN=1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_WriteMemory,addr,data,_CheckSum); 
   TXEN=0;

}

//Set target position
void HSR_SetPosition(uint8 PosH, uint8 PosL)
{
   
   _CheckSum = _HSR_ComStart + _HSR_SetPosition + PosH + PosL;
   _CheckSum=(~_CheckSum)+1;

   TXEN=1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_SetPosition,PosH,PosL,_CheckSum); 
   TXEN=0;

}


//Overloaded Function
//Set target position
void HSR_SetPosition(uint8 ServoID, uint8 PosH, uint8 PosL)
{
   
   _CheckSum = _HSR_ComStart + ServoID + PosH + PosL;
   _CheckSum=(~_CheckSum)+1;

   TXEN=1;
   printf("%c%c%c%c%c",_HSR_ComStart,ServoID,PosH,PosL,_CheckSum); 
   TXEN=0;

}

//This function is used to smoothly go to a target position 
//from limp position
void HSR_SetInitialPosition(uint8 PosH, uint8 PosL)
{

   HSR_ReadPosition_v2();//Read Current Position
   WaitRxDataReady(); // Wait until data is receieved
   HSR_WriteMemory(_HSR_MEM_CurTargetPosL, _ServoPosL);
   HSR_WriteMemory(_HSR_MEM_CurTargetPosH, _ServoPosH);
   HSR_SetPosition(PosH, PosL);

}

//Read Servo Version and ID
void HSR_ReadVerID(void)
{

   _RxComplete=0; //The data is not available

   _CheckSum=_HSR_ComStart+_HSR_ReadVerID;
   _CheckSum=(~_CheckSum)+1;

   CREN = 0;
   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_ReadVerID,0x00,0x00,_CheckSum); 
   CREN = 1;
   TXEN = 0;

   WaitRxDataReady(); // Wait until receive is completed.
   _ServoID = _RxLastData[0];
   _ServoVersion = _RxLastData[1];
 
}

//Read Servo Version and ID. 
//The servo version and id will be available as the first and 
//second elements of _RxLastData at the end of communication. 
void HSR_ReadVerID_v2(void)
{

   _CheckSum=_HSR_ComStart+_HSR_ReadVerID;
   _CheckSum=(~_CheckSum)+1;

   TXEN=1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_ReadVerID,0x00,0x00,_CheckSum); 
   TXEN=0;

}

//Read pulsewidth and voltage
void HSR_ReadPWM(void)
{

   _RxComplete = 0; //The data is not available

   _CheckSum = _HSR_ComStart + _HSR_ReadPWM;
   _CheckSum = (~_CheckSum) + 1;

   CREN = 0;
   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_ReadPWM,0x00,0x00,_CheckSum); 
   CREN = 1;
   TXEN = 0;

   WaitRxDataReady(); // Wait until receive is completed.
   _ServoPW = _RxLastData[0];
   _ServoVoltage = _RxLastData[1];

}

//Read pulsewidth and voltage
//At te end of communication, pulsewidth and voltage will be 
//available as the first and second elements of _RxLastData,
//respectively.
void HSR_ReadPWM_v2(void)
{

   _CheckSum = _HSR_ComStart + _HSR_ReadPWM;
   _CheckSum = (~_CheckSum) + 1;

   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_ReadPWM,0x00,0x00,_CheckSum); 
   TXEN = 0;

}

//Set the speed of a single servo using its ID 
//and read its current position
void HSR_SetSpeed(uint8 ServoID, uint8 ServoSpeed)
{

   _RxComplete = 0; //The data is not available

   _CheckSum = _HSR_ComStart + _HSR_SetSpeed + ServoID + ServoSpeed;
   _CheckSum = (~_CheckSum) + 1;

   CREN = 0;
   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_SetSpeed,ServoID,ServoSpeed,_CheckSum); 
   CREN = 1;
   TXEN = 0;

   WaitRxDataReady(); // Wait until receive is completed.
   _ServoPosH = _RxLastData[0];
   _ServoPosL = _RxLastData[1];

}

//Set the speed of a single servo using its ID 
//and read its current position.
//This function only starts the communication, 
//you need to wait communication to end successfully.
//Current position will be available as _RxLastData
void HSR_SetSpeed_v2(uint8 ServoID, uint8 ServoSpeed)
{

   _CheckSum = _HSR_ComStart + _HSR_SetSpeed + ServoID + ServoSpeed;
   _CheckSum = (~_CheckSum) + 1;

   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_SetSpeed,ServoID,ServoSpeed,_CheckSum); 
   TXEN = 0;

}

//Select Control Parameter Set
void HSR_SelectControl(uint8 ControlSet)
{
// Note: ControlSet must be 1-3, otherwise it will be processed as 3.

   _RxComplete = 0; //The data is not available

   _CheckSum = _HSR_ComStart + _HSR_SelectControl + ControlSet;
   _CheckSum = (~_CheckSum) + 1;

   CREN = 0;
   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_SelectControl,0x00,ControlSet,_CheckSum); 
   CREN = 1;
   TXEN = 0;

   WaitRxDataReady(); // Wait until receive is completed.

}

//Select Control Parameter Set
//This function only starts to communication. 
//To make sure that communication ends succesfully, 
//wait 3ms or call WaitRxDataReady();
void HSR_SelectControl_v2(uint8 ControlSet)
{
// Note: ControlSet must be 1-3, otherwise it will be processed as 3.


   _CheckSum = _HSR_ComStart + _HSR_SelectControl + ControlSet;
   _CheckSum = (~_CheckSum) + 1;

   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_SelectControl,0x00,ControlSet,_CheckSum); 
   TXEN = 0;

}

//Set Go/Stop
void HSR_SetGoStop(uint8 GoStop)
{

   _RxComplete = 0; //The data is not available

   _CheckSum = _HSR_ComStart + _HSR_SetGoStop + GoStop;
   _CheckSum = (~_CheckSum) + 1;

   CREN = 0;
   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_SetGoStop,GoStop,0x00,_CheckSum); 
   CREN = 1;
   TXEN = 0;

   WaitRxDataReady(); // Wait until receive is completed.	

}

//Set Go/Stop
//This function only starts to communication. 
//To make sure that communication ends succesfully, 
//wait 3ms or call WaitRxDataReady();
void HSR_SetGoStop_v2(uint8 GoStop)
{

   _CheckSum = _HSR_ComStart + _HSR_SetGoStop + GoStop;
   _CheckSum = (~_CheckSum) + 1;

   TXEN = 1;
   printf("%c%c%c%c%c",_HSR_ComStart,_HSR_SetGoStop,GoStop,0x00,_CheckSum); 
   TXEN = 0;	

}

//Set Action
void HSR_SetAction(sint16 TargetAngle, uint16 ActionTime)
{
    uint16 	CurServoPos;
    uint16 	TargetPos;
    uint8 	TargetPosH;
    uint8	TargetPosL;
    uint16  ActionSpeed;
    uint8   ActionSpeedByte;
	CurServoPos = HSR_ReadPosition();
    TargetPos = TargetAngle+14500;
    TargetPos = TargetPos/10;
    TargetPosH = make8(TargetPos,1);
	TargetPosL = make8(TargetPos,0);
    ActionTime=ActionTime/10;

    if (TargetPos>CurServoPos)
	{
        ActionSpeed = (TargetPos-CurServoPos)/10;
        ActionSpeed*=39;
        ActionSpeed/=ActionTime;
 	} 	
    else
	{
		ActionSpeed = (CurServoPos-TargetPos)/10;
        ActionSpeed*=39;
        ActionSpeed/=ActionTime;
 	} 	
    ActionSpeedByte=make8(ActionSpeed,0);
    output_b(ActionSpeedByte);
    HSR_WriteMemory(_HSR_MEM_Speed, ActionSpeedByte);
    HSR_WriteMemory(_HSR_MEM_CurSpeed, ActionSpeedByte);
    HSR_SetPosition(TargetPosH,TargetPosL);
        
}

//Set Action
void HSR_SetAction_v2(sint16 TargetAngle, uint16 ActionTime)
{
    uint16 	CurServoPos;
    uint16 	TargetPos;
    uint8 	TargetPosH;
    uint8	TargetPosL;
    uint32  ActionSpeed;
    uint8   ActionSpeedByte;
	CurServoPos = HSR_ReadPosition();
    TargetPos = TargetAngle+14500;
    TargetPos = TargetPos/10;
    TargetPosH = make8(TargetPos,1);
	TargetPosL = make8(TargetPos,0);

    if (TargetPos>CurServoPos)
	{
        ActionSpeed = (TargetPos-CurServoPos);
        ActionSpeed*=2000;
        ActionSpeed/=ActionTime;
		ActionSpeed/=79;
 	} 	
    else
	{
		ActionSpeed = (CurServoPos-TargetPos);
        ActionSpeed*= 2000;
        ActionSpeed/=ActionTime;
		ActionSpeed/=79;
 	} 	
    ActionSpeedByte=make8(ActionSpeed,0);
    output_b(ActionSpeedByte);
    HSR_WriteMemory(_HSR_MEM_Speed, ActionSpeedByte);
    HSR_WriteMemory(_HSR_MEM_CurSpeed, ActionSpeedByte);
    HSR_SetPosition(TargetPosH,TargetPosL);
        
}

//Set Action and estimaate time of motion
uint32 HSR_SetAction_v3(sint16 TargetAngle, uint16 ActionTime)
{
    uint16 	CurServoPos;
    uint16 	TargetPos;
    uint8 	TargetPosH;
    uint8	TargetPosL;
    uint32  ActionSpeed;
    uint32  EstimatedTime;
    uint8   ActionSpeedByte;
	CurServoPos = HSR_ReadPosition();
    TargetPos = TargetAngle+14500;
    TargetPos = TargetPos/10;
    TargetPosH = make8(TargetPos,1);
	TargetPosL = make8(TargetPos,0);

    if (TargetPos>CurServoPos)
	{
        ActionSpeed = (TargetPos-CurServoPos);
        ActionSpeed*=2000;
        ActionSpeed/=79;
        EstimatedTime=ActionSpeed;
        ActionSpeed/=ActionTime;
		
 	} 	
    else
	{
		ActionSpeed = (CurServoPos-TargetPos);
        ActionSpeed*= 2000;
        ActionSpeed/=79;        
 		EstimatedTime=ActionSpeed;
		ActionSpeed/=ActionTime;
 	} 	
    ActionSpeedByte=make8(ActionSpeed,0);
    EstimatedTime/=ActionSpeedByte;
    output_b(ActionSpeedByte);
    HSR_WriteMemory(_HSR_MEM_Speed, ActionSpeedByte);
    HSR_WriteMemory(_HSR_MEM_CurSpeed, ActionSpeedByte);
    HSR_SetPosition(TargetPosH,TargetPosL);
    return(EstimatedTime);
        
}