/***********************************************************
         /\ /\
       -'<o_o>'-   _
 Nyaw!   () ()\  ,'_\
         ( . ) )/._./
        (_)-(_).--'

	Jesse W.
	Timer3 pulse width modulation C file file
	designed for standard r/c servos and speed controls
	Made: 8-11-06
************************************************************/
#include <avr/io.h>
#include "servoT3.h"

void pwm3Init (unsigned char mask)
{
	/* break down by bits
           before shift xxxxxCBA
	   actual port B positions
           are xxCBAxxx hence the <<5

	   This is the data direction
	   register...
	*/
	DDRE |= (mask<<3);

	
	/* break down by bits
	   AABBCC10
	   10101010
	   need some kind of 
           switch statement on the mask
	*/
	/* EXAMPLE TO USE ALL THREE 	
	TCCR1A = 0xAA;
	*/

	/* break down by bits
    	   before shift xxxxxCBA
	   actual port B positions
           are ABCxxxxx hence the <<5
	   

	   this is the Force Output Compare
	   
	   need another switch on the mask...
	*/ 	
	/* EXAMPLE TO USE ALL THREE
    	   TCCR3C = 0xE0;
	*/

	/* For channel A*/
	if (mask & CH_3A)
	{	
		TCCR3A |= 0x82;
		TCCR3C |= 0x80;
	}
	/* For channel B*/
	if (mask & CH_3B)
	{	
		TCCR3A |= 0x22;
		TCCR3C |= 0x40;	
	}
	/* For channel C*/
	if (mask & CH_3C)
	{	
		TCCR3A |= 0x0A;
		TCCR3C |= 0x20;
	}

	/* break down by bits
	   already set up
	*/ 	
    	TCCR3B = 0x1A;

	/* The Fast PWM mode selected uses   */
    	/* ICR3 as the TOP of the counter.   */
    	/* counting to 40000 gives us a      */
    	/* frequency of 50 Hz, which is what */
    	/* servos need.                      */
	ICR3 = TOP;

		/* For channel A*/
	if (mask & CH_3A)
	{	
		PWM_SET(PWM_3A, PWM_3A_CENTER);
	}
	/* For channel B*/
	if (mask & CH_3B)
	{	
		PWM_SET(PWM_3B, PWM_3B_CENTER);
	}
	/* For channel C*/
	if (mask & CH_3C)
	{	
        PWM_SET(PWM_3C, PWM_3C_CENTER);	
	}
}

void pwm3Off(unsigned char mask)
{
	DDRE &= ~(mask<<3);

	/* For channel A*/
	if (mask & CH_3A)
	{	
		TCCR3A &= ~(0x80);
		TCCR3C &= ~(0x80);
	}
	/* For channel B*/
	if (mask & CH_3B)
	{	
		TCCR3A &= ~(0x20);
		TCCR3C &= ~(0x40);	
	}
	/* For channel C*/
	if (mask & CH_3C)
	{	
		TCCR3A &= ~(0x08);
		TCCR3C &= ~(0x20);	
	}
	
}
										
void pwm3A_SetSafe (signed int pos)
{

	//translation to true_pos
	pos += PWM_3A_CENTER_OA;
	unsigned int true_pos = (unsigned int) pos ;
	
	if (true_pos<PWM_3A_LEFT_OA)
	{
		PWM_SET(PWM_3A, PWM_3A_LEFT_OA);		
	}
	else if (true_pos>PWM_3A_RIGHT_OA)
	{
		PWM_SET(PWM_3A, PWM_3A_RIGHT_OA);
	}
	else PWM_SET(PWM_3A, true_pos);
}

void pwm3B_SetSafe (signed int pos)
{
	
	//translation to true_pos
	pos += PWM_3B_CENTER_OA;
	unsigned int true_pos = (unsigned int) pos ;
	
	if (true_pos<PWM_3B_LEFT)
	{
		PWM_SET(PWM_3B, PWM_3B_LEFT_OA);		
	}
	else if (true_pos>PWM_3B_RIGHT_OA)
	{
		PWM_SET(PWM_3B, PWM_3B_RIGHT_OA);
	}
	else PWM_SET(PWM_3B, true_pos);
}

void pwm3C_SetSafe (signed int pos)
{
	//conversion because e-flight servo is reversed
	pos = (-pos);
	//translation to true_pos
	pos += PWM_3C_CENTER_OA;
	unsigned int true_pos = (unsigned int) pos ;	

	if (true_pos<PWM_3C_LEFT_OA)
	{
		PWM_SET(PWM_3C, PWM_3C_LEFT_OA);
	}
	else if (true_pos>PWM_3C_RIGHT_OA)
	{
		PWM_SET(PWM_3C, PWM_3C_RIGHT_OA);
	}
	else PWM_SET(PWM_3C, true_pos);
}