/*
 * oc-pwm.c - use output compare to generate PWD without interrupts
 */

/*
 * -- from NG02 motor.c -------
 * TCCR1A = _BV(WGM10);
 * TCCR1B = _BV(WGM12) | _BV(CS11);
 * TIMSK |= _BV(TOIE1) | _BV(OCIE1B) | _BV(OCIE1A);
 *
 * WGM mode 5 fast PWM 8-bit, CLKio/8, port normal i/o operation, now PWM
 * ----------------------------
 * if FWD clear R PWM pin on match  ==> WGM 5, COM 2
 *        clear L PWM pin on match  ==> WGM 5, COM 2
 *        set   R PWM pin on TOVF   ==> WGM 5, COM 2
 *        set   L PWM pin on TOVF   ==> WGM 5, COM 2
 *
 * if REV set   R PWM pin on match  ==> WGM 5, COM 3
 *        set   L PWM pin on match  ==> WGM 5, COM 3
 *        clear R PWM pin on TOVF   ==> WGM 5, COM 3
 *        clear L PWM pin on TOVF   ==> WGM 5, COM 3
 *
 * Use OC1A for left motor PWM
 *     OC1B for right motor PWM
 */

#include <avr/io.h>  /* ports and registers */
#include <trace.h>

void motor_init(void);
void both_motor_fwd(void);
void both_motor_rev(void);
void initialize(void);

void initialize() {
  TRACE_PORT_DDR = ALL_OUT;
  TRACE_PORT = 0;
  TCCR0 = _BV(CS00);
}

void both_motor_stop() {
  if ((TIFR & _BV(TOV1)) == 0) {
	while ((TIFR & _BV(TOV1)) == 0) ;
	TIFR |= _BV(TOV1);
  }
  TRACE_ON(3);
  TCCR1A = 0;
  TCCR1B = 0;
  OCR1A = 255;
  OCR1B = 255;
  TIFR |= _BV(TOV1);
  TRACE_OFF(3);
}

void both_motor_fwd() {
  OCR1A = 80;
  OCR1B = 160;
  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
  TCCR1B = _BV(WGM12) | _BV(CS11);
  TCNT1 = 0;
  TIFR |= _BV(TOV1);
}

void both_motor_rev() {
  OCR1A = 80;
  OCR1B = 160;
  TCCR1A = _BV(COM1A1) | _BV(COM1A0) | _BV(COM1B1) | _BV(COM1B0) | _BV(WGM10);
  TCCR1B = _BV(WGM12) | _BV(CS11);
  TCNT1 = 0;
  TIFR |= _BV(TOV1);
}

void motor_init() {
  DDRB |= _BV(PB6) | _BV(PB5);
  OCR1A = 80;
  OCR1B = 0;
  /* WGM mode 5, clk/8 */
  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
  TCCR1B = _BV(WGM12) | _BV(CS11);
  TCNT1 = 0;
  TIFR |= _BV(TOV1);
}

int main() {
  initialize();
  motor_init();
  int count;
  
  TIFR |= _BV(TOV1);
  while (1) {
	TRACE_ON(0);
	TRACE_ON(1);
	count = 5;
	do {
	  while ((TIFR & _BV(TOV1)) == 0) ;
	  TIFR |= _BV(TOV1);
	} while (--count);
	TRACE_OFF(1);
	/* stop motors */
	both_motor_stop();
	/* wait 16 used */
	TCNT0 = 0;
	TIFR |= _BV(TOV0);
	while ((TIFR & _BV(TOV0)) == 0) ;
	/* start reverse */
	both_motor_rev();
	TIFR |= _BV(TOV1);
	TRACE_ON(2);
	count = 5;
	do {
	  while ((TIFR & _BV(TOV1)) == 0) ;
	  TIFR |= _BV(TOV1);
	} while (--count);
	TRACE_OFF(2);
	/* stop motors */
	both_motor_stop();
	/* wait 16 used */
	TCNT0 = 0;
	TIFR |= _BV(TOV0);
	while ((TIFR & _BV(TOV0)) == 0) ;
	/* start fwd */
	both_motor_fwd();
	TRACE_OFF(0);
  }

  return 0;
}