/* pwm_direct_t0.c
** PWM routines for ORANGUTAN motor controller from Pololu
** atmega168/LB1836M motor controller, WinAVR
** these routines use TIMER0 in phase correct PWM mode, no interrupts
** by Jim Remington (sjames_remington _at_ yahoo dot com)
** for more information, see http://www.uoxray.uoregon.edu/orangutan
*/

#include <avr/io.h>
#define F_CPU 8000000UL
#include <util/delay.h>

/*
** pwm_init - Set up the pwm ports (PORT B1=IN1, B2=IN3, D5=IN2, D6=IN4)
** Initialize Timer 0 to phase correct PWM mode 1
*/

void	pwm_init( void ) 
{
	DDRB |= 6;				//set PORTB.1 and PORTB.2 to output
	PORTB &= ~6;			//clear outputs
	DDRD |= (3<<5);			//set PORTD.5 and PORTD.6 to output
	PORTD &= ~(3<<5);		//clear outputs (motor driver is now off)
	TCCR0A = (1<<WGM00);	//8 bit pwm phase correct, timer outputs disconnected from PORTD
	TCCR0B = 2; 			//Timer increments at CPU clock/8, overflow frequency = 3.9 KHz
							//TCCR0B = 3 for Baby Orangutan (clock/64)  
	OCR0A = 0;				//clear compare match registers
	OCR0B = 0;
}

/*

pwm_set(signed int left, signed int right) - Set the two PWM speeds.  

Each speed ranges from -255 to 255, NO CHECK is performed for |speed| > 255

right motor = PB.1(IN1) and PD.5 (IN2, OC0B)
left  motor = PB.2(IN3) and PD.6 (IN4, OC0A) 

If direction of travel is wrong, switch motor leads, but note asymmetry of PWM modes
in forward and reverse directions. Brake mode is more power-hungry but more power at low speed.

Truth Table for LB1836 controller from data sheet.
IN1/3		IN2/4		MODE
----		-----		----
1		0		forward
1		1		brake (both outputs low), power hungry mode.
0		1		reverse
0		0		coast (both outputs off), power saving mode.
*/

void	pwm_set( signed int left_speed, signed int right_speed )
{
	
	if ( right_speed > 0 ) {	
		PORTB |= (1<<1); 				//IN1=1
        TCCR0A |= (3<<COM0B0);			//set COM0B0=COM0B1=1: OC0B=IN2=0 on upcount, 1 on compare match
		OCR0B = right_speed;			//(this should be forward on upcount, brake on compare match)
	} 
	if ( right_speed==0 ) {
		TCCR0A &= ~(3<<COM0B0);			//disconnect PWM channel B from PORTD
		PORTB &= ~(1<<1); 				//IN1=0 ; set coast mode
		PORTD &= ~(1<<5); 				//IN2=0
	}
	if ( right_speed < 0 ) {	
		PORTB &= ~(1<<1); 				//IN1=0
		TCCR0A |= (1<<COM0B1);
		TCCR0A &= ~(1<<COM0B0);			//set COM0B1=1, COM0B0=0: OC0B=IN2=1 on upcount, 0 on compare match
		OCR0B = (-right_speed);			//(this should be reverse on upcount, coast on compare match)
	} 
	if ( left_speed > 0 ) {	
		PORTB |= (1<<2); 				//IN3=1
        TCCR0A |= (3<<COM0A0);			//set COM0A0=COM0A1=1 Result: OC0A=IN4=0 on upcount, 1 on compare match
		OCR0A = left_speed;				//(this should be forward on upcount, brake on compare match)
	} 
	if ( left_speed==0 ) {
		TCCR0A &= ~(3<<COM0A0);			//disconnect PWM channel A from PORTD
		PORTB &= ~(1<<2); 				//IN3=0 ; set coast mode
		PORTD &= ~(1<<6); 				//IN4=0
	}
	if ( left_speed < 0 ) {	
		PORTB &= ~(1<<2);		 		//IN3=0
		TCCR0A |= (1<<COM0A1);
		TCCR0A &= ~(1<<COM0A0);			//set COM0A1=1, COM0A0=0: OC0A=IN4=1 on upcount, 0 on compare match
		OCR0A = (-left_speed);			//(this should be reverse on upcount, coast on compare match)
	} 

}

/*
** MAIN. Exercise left and right motors.
*/
int main(void)
{
	signed int i;
	pwm_init();
	while(1)
	{
	for(i=0;i<255;i++)
		{
		pwm_set(i,i);
		_delay_ms(20);		
		}
	for(i=255;i>0;i--)
		{
		pwm_set(i,i);
		_delay_ms(20);
		}
	for(i=0;i<255;i++)
		{
		pwm_set(-i,-i);
		_delay_ms(20);
		}
	for(i=255;i>0;i--)
		{
		pwm_set(-i,-i);
		_delay_ms(20);
		}
	}
	return 0;
}