/****************************************************************************
 Module
 Servos.h

 Description
 Handles Turret, Shooter, and Gate servos
 ****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
#include "GlobalHeader.h"
#include "Servos.h"

/*----------------------------- Module Defines ----------------------------*/
//turret positioning
#define TURRET_STRAIGHT 2220 //calibrated
#define TURRET_MIN 900
#define TURRET_MAX 3600
#define TURRET_SCALE -15 //ticks per degree

#define SERVOS_PERIOD 30000 //20ms at 3MHz

/*---------------------------- Module Variables ---------------------------*/
static unsigned int Servos_TurretOnTime = TURRET_STRAIGHT;
unsigned int Servos_ShooterOnTime = Shooter_Reset;
static unsigned int Servos_GateOnTime = Gate_Reset;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
 ServosInit

 Parameters
 none

 Returns
 none

 Description
 Initialization for servos --  output compare timers and interrupts
 ****************************************************************************/
void ServosInit(void){
    TIM1_TSCR1 |= _S12_TEN;
	TIM1_TSCR2 |= _S12_PR2;//16 = 1.5MHz.  //(_S12_PR1 | _S12_PR0); //Prescale of 8.  3 MHz

	TIM1_TCTL1 |= (_S12_OM4 | _S12_OM5 | _S12_OM6); //clear line on interrupt
	TIM1_TCTL1 &=~(_S12_OL4 | _S12_OL5 | _S12_OL6);
	TIM1_TCTL1 &= ~_S12_OM7; //toggle port 7 every period
	TIM1_TCTL1 |=  _S12_OL7;

	TIM1_OC7M |= (_S12_OC7M4 | _S12_OC7M5 | _S12_OC7M6); //Set Mask registers
	TIM1_OC7D |= (_S12_OC7D4 | _S12_OC7D5 | _S12_OC7D6); //make sure mask clears bits

	TIM1_TIOS |= (_S12_IOS4 | _S12_IOS5 | _S12_IOS6 | _S12_IOS7); //T(4-7) output compare
	TIM1_TIE |= (_S12_C4I | _S12_C5I | _S12_C6I | _S12_C7I); //enable interrupts


	TIM1_TC7 = TIM1_TCNT + SERVOS_PERIOD;		//time next rising edge
	TIM1_TC4 = TIM1_TC7 + Servos_TurretOnTime;	//time next falling edge for each servo
	TIM1_TC5 = TIM1_TC7 + Servos_ShooterOnTime;
	TIM1_TC6 = TIM1_TC7 + Servos_GateOnTime;

	TIM1_TFLG1 = (_S12_C4F | _S12_C5F | _S12_C6F | _S12_C7F);
	printf("Init done\r\n");
}

/****************************************************************************
 Function
 SetTurretServo

 Parameters
 signed char Angle (in degrees, left is +)

 Returns
 none

 Description
 Sets angle of turret servo, bounded by MIN and MAX
 ****************************************************************************/
void SetTurretServo (signed char Angle){
	signed int OnTime;
	OnTime = (signed int)Angle * TURRET_SCALE + TURRET_STRAIGHT;
	OnTime = ((OnTime < TURRET_MIN) ? TURRET_MIN : OnTime);
	OnTime = ((OnTime > TURRET_MAX) ? TURRET_MAX : OnTime);
	Servos_TurretOnTime = (unsigned int)OnTime;
}

/****************************************************************************
 Function
 SetShooterServo / SetGateServo

 Parameters
 unsigned int ...Position - pusle width, in 0.67us's

 Returns
 none

 Description
 sets Servo position based on provided pulse width
 ****************************************************************************/
void SetShooterServo (unsigned int ShooterPosition){
	Servos_ShooterOnTime = ShooterPosition;
}

void SetGateServo (unsigned int GatePosition){
	Servos_GateOnTime = GatePosition;
}


/****************************************************************************
 Interrupt
 TurretInterrupt

 Description
 Creates falling edge for turret servo
 ****************************************************************************/
void interrupt _Vec_tim1ch4 TurretInterrupt(void) //port T4
{
	TIM1_TC4 += SERVOS_PERIOD;
	TIM1_TFLG1 = _S12_C4F;
}

/****************************************************************************
 Interrupt
 ShooterInterrupt

 Description
 Creates falling edge for shooter servo
 ****************************************************************************/
void interrupt _Vec_tim1ch5 ShooterInterrupt(void) //port T5
{
	TIM1_TC5 += SERVOS_PERIOD;
	TIM1_TFLG1 = _S12_C5F;
}

/****************************************************************************
 Interrupt
 GateInterrupt

 Description
 Creates falling edge for gate servo
 ****************************************************************************/
void interrupt _Vec_tim1ch6 GateInterrupt(void) //port T6
{
	TIM1_TC6 += SERVOS_PERIOD;
	TIM1_TFLG1 = _S12_C6F;
}

/****************************************************************************
 Interrupt
 ServoPeriodInterrupt

 Description
 Creates rising edge for all three servos
 ****************************************************************************/
void interrupt _Vec_tim1ch7 ServoPeriodInterrupt(void) //port T6
{
	TIM1_TC4 = TIM1_TC7 + Servos_TurretOnTime;
	TIM1_TC5 = TIM1_TC7 + Servos_ShooterOnTime;
	TIM1_TC6 = TIM1_TC7 + Servos_GateOnTime;
	TIM1_TC7 += SERVOS_PERIOD;
	TIM1_TFLG1 = _S12_C7F;
}