/*
** motor control for ORANGUTAN from pololu
** PWM code for atmega8/LB1836M motor controller by Jim Remington (sjames_remington _at_ yahoo.com)
** This code uses 8-bit fast PWM mode with motor coasting in off state for both forward and reverse.
** WinAVR

REQUIRED INCLUDES AND DEFINES:

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdlib.h> //abs function
#define 	sei()   __asm__ __volatile__ ("sei" ::)
#define 	cli()   __asm__ __volatile__ ("cli" ::)

***GLOBALS REQUIRED for communication with interrupt routines***

int l_speed,r_speed;

*/

// OUTPUT COMPARE 1A Interrupt
// This interrupt occurs on compare match of TCNT1 with OCR1A.
// set right motor controller to coast mode (outputs "off")

SIGNAL (SIG_OUTPUT_COMPARE1A)
{
	PORTB &= ~_BV(1);  //IN1=PB.1=0
	PORTD &= ~_BV(5);  //IN2=PD.5=0
}

// OUTPUT COMPARE 1B Interrupt
// This interrupt occurs on compare match of TCNT1 with OCR1B.
// set left motor controller to coast mode (outputs "off")

SIGNAL (SIG_OUTPUT_COMPARE1B)
{
	PORTB &= ~_BV(2); 	//IN3=PB.2=0
	PORTD &= ~_BV(6);	//IN4=PD.6=0
}

// TIMER 1 OVERFLOW Interrupt
// This interrupt occurs each time TCNT1 reaches 0xFF (TOP) in 8-bit mode (488 Hz/1 MHz CPU clock)
// check direction and set motor control lines for each motor, as appropriate
// This routine could be used for timekeeping as this is a regular interrupt 

SIGNAL (SIG_OVERFLOW1)
{
	if (r_speed>0) PORTB |= _BV(1);	//forward, PB.1=1 PD.5=0
	if (r_speed<0) PORTD |= _BV(5);	//reverse, PD.5=1 PB.1=0
// for speed==0, do nothing as motors are off
	if (l_speed>0) PORTB |= _BV(2);	//forward, PB.2=1 PD.6=0
	if (l_speed<0) PORTD |= _BV(6);	//reverse, PD.6=1 PB.2=0
}
/*
** 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);		//make outputs as required
	DDRD |= (1<<5) | (1<<6);
	
// set outputs to brake both motors using the following two lines:
//	PORTB |= _BV(1)|_BV(2);		//IN1=IN3=1 : brake
//	PORTD |= _BV(5)|_BV(6);		//IN2=IN4=1

// or, set outputs to off (coast) by using the following two lines:
	PORTB &= ~(_BV(1) | _BV(2));	//IN1=IN3=0 : coast
	PORTD &= ~(_BV(5) | _BV(6));	//IN2=IN4=0

// set timer1 to fast PWM mode 5: WGM12+WGM10 
// and to increment at fosc/8 (125 kHz for 1 MHz clock)

	TCCR1A = _BV (WGM10);   //0x01
	TCCR1B = (_BV(WGM12) | _BV (CS11));  //0x0A
	OCR1A = 0;
	OCR1B = 0;

// enable interrupts for timer1 overflow and output compares

	TIMSK |= (_BV(OCIE1A) | _BV(OCIE1B) | _BV (TOIE1));
}

/*
** pwm_set - Set the two PWM speeds, l_speed and r_speed (globals).  Each ranges from -255 to 255
**
** Warning: Timer1 is 16 bits. DO NOT SET SPEEDS to EXCEED +/- 255 or behavior will be unexpected!

Assumes left motor = PB.2 and PD.6, right = PB.1 and PD.5. 
If direction of travel is wrong, switch motor leads

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

void	pwm_set(void)
{
	PORTB &= ~(_BV(1)|_BV(2));		//IN1=IN2=0 :coast
	PORTD &= ~(_BV(5)|_BV(6));		//IN2=IN4=0
	OCR1A = abs(r_speed);			//set compare registers
	OCR1B = abs(l_speed);
}