/*
** PWM routines for ORANGUTAN motor controller from Pololu
** atmega8/LB1836M motor controller, WinAVR
** Direct control: interrupts are not used in this version
** sjames_remington _at_ yahoo.com
** for more information, see http://www.uoxray.uoregon.edu/orangutan
*/

#include <avr/io.h>
/*
** Delays
*/
#define F_CPU 1000000UL
#include <util/delay.h>
#define	Delay( x )	_delay_loop_2( x )
// above, x is uint16

/*
** PWM_Init - Set up the pwm ports (PORT B1=IN1, B2=IN3, D5=IN2, D6=IN4)
*/
void	pwm_init( void ) 
{
	DDRB |= (1<<1) | (1<<2);
	DDRD |= (1<<5) | (1<<6);
	TCCR1A = 1; // 8 bit pwm phase correct
	TCCR1B = 1; // clock/1  use TCCR1B=2 (clock/8) for faster CPU clocks
	OCR1A = 0;
	OCR1B = 0;
}

/*
** pwm_set - Set the two PWM speeds.  Each ranges from -255 to 255
** assumes left motor = PB.2 and PD.6, right = PB.1 and PD.5. 
** Note: speeds > +/- 255 WILL LEAD to unexpected behavior!

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 functions better at low speed.
It may be advantageous to reverse motor leads and the definition of forward and reverse speeds.

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( int left_speed, int right_speed )
{
	
	if ( right_speed > 0 ) 
	{	
	PORTD &= ~_BV(5);	 	//IN2=0
        TCCR1A |= _BV(COM1A1);
	TCCR1A &= ~_BV(COM1A0);		//set COM1A1=1, COM1A0=0. Result: OC1A=IN1=1 on upcount, =0 on compare match
	OCR1A = right_speed;		//(this should be forward on upcount, coast on compare match)
	} 
	if (right_speed==0)
	{
	TCCR1A &= ~(_BV(COM1A1)|_BV(COM1A0)); //disconnect PWM channel A from PORTB
	PORTD &= ~_BV(5); 		//IN2=0
	PORTB &= ~_BV(1); 		//IN1=0 ; set coast mode (conserves power). Set both=1 for brake mode
	}
	if ( right_speed < 0 ) 
	{	
	PORTD |= _BV(5); 			//IN2=1
        TCCR1A |= (_BV(COM1A1)|_BV(COM1A0));	//set COM1A1=COM1A0=1. Result: OC1A=IN1=0 on upcount, =1 on compare match
	OCR1A = -right_speed;			//(this should be reverse on upcount, brake on compare match)
	} 
	if ( left_speed > 0 ) 
	{	
	PORTD &= ~_BV(6); 		//IN4=0
        TCCR1A |= _BV(COM1B1);
	TCCR1A &= ~_BV(COM1B0);		//set COM1B1=1, COM1B0=0. Result: OC1B=IN3=1 on upcount, =0 on compare match
	OCR1B = left_speed;		//(this should be forward on upcount, coast on compare match)
	} 
	if (left_speed==0)
	{
	TCCR1A &= ~(_BV(COM1B1)|_BV(COM1B0)); //disconnect PWM channel B from PORTB
	PORTD &= ~_BV(6); 		//IN4=0
	PORTB &= ~_BV(2); 		//IN3=0 ; set coast mode (conserves power). Set both=1 for brake mode
	}
	if ( left_speed < 0 ) 
	{	
	PORTD |= _BV(6); 			//IN4=1
        TCCR1A |= (_BV(COM1B1)|_BV(COM1B0)); //set COM1B1=COM1B0=1. Result: OC1B=IN1=0 on upcount, =1 on compare match
	OCR1B = -left_speed;			//(this should be reverse on upcount, brake on compare match)
	} 

}

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