/*

avr @ 16Mhz
9600 baud

 +-------------------------------------+
 |                          Sig Vd Gnd |
 |  +---------+   5V O     PB0 [o o o] | portB, servos 1-8
 |  | 7805  O |   Vd O     PB1 [o o o] | 
 |  +---------+   V+ .     PB2 [o o o] |
 |                         PB3 [o o o] | 
 |                         PB4 [o o o] | 
 |                         PB5 [o o o] | 
 |                         PB6 [o o o] | 
 |                         PB7 [o o o] | 
 |                         PA0 [o o o] | 
 |                         PA1 [o o o] | 
 |        +----------+     PA2 [o o o] | 
 |        |O         |     PA3 [o o o] | 
 |        |          |     PA4 [o o o] | 
 |        |          |     PA5 [o o o] | 
 |        |          |     PA6 [o o o] | 
 |        |          |     PA7 [o o o] | 
 |        |          |     PC7 [o o o] | 
 |        |          |     PC6 [o o o] | 
 |        |          |     PC5 [o o o] | 
 |        | ATMEGA32 |     PC4 [o o o] | 
 |        |          |     PC3 [o o o] | 
 |        |          |     PC2 [o o o] | 
 |        |          |     PC1 [o o o] | 
 |        |          |     PC0 [o o o] | 
 |        |          |     PD7 [o o o] | 
 |        |          |     PD2 [o o o] |
 |        |          |     PD3 [o o o] |
 |        |          |     PD4 [o o o] | 
 |        |          |     PD5 [o o o] | 
 |        +----------+     PD6 [o o o] | 
 |      E.D.S BABYBOARD III            |
 +-------------------------------------+

packet:
[255][servo #][MSB][LSB]
where msb:lsb <=24000

pulse 0.5ms -> 2ms
update 50Hz


*/
#include <avr/io.h>
#include "usart.h"
#include "binary.h"
#include <avr/interrupt.h>


#define OUTPUT  1
#define INPUT   0

#define SetBit(BIT, PORT)     PORT |= (1<<BIT)
#define ClearBit(BIT, PORT)   PORT &= ~(1<<BIT)
#define IsHigh(BIT, PORT)    (PORT & (1<<BIT)) != 0
#define IsLow(BIT, PORT)     (PORT & (1<<BIT)) == 0

#define stopCounter()  TCCR1B = b00011000;
#define startCounter() TCCR1B = b00011001;

void countersInit();


volatile unsigned int position[8];
volatile unsigned char idx;

int main( void ) {
 
  unsigned char pkt[2];
  unsigned char ptr;
  unsigned int temp;
  unsigned char data;

    // set up directions 
  DDRA = (INPUT << PA0 | INPUT << PA1 |INPUT << PA2 |INPUT << PA3 |INPUT << PA4 |INPUT << PA5 |INPUT << PA6 |INPUT << PA7);
  DDRB = (OUTPUT << PB0 | OUTPUT << PB1 | OUTPUT << PB2 |OUTPUT << PB3 |OUTPUT << PB4 |OUTPUT << PB5 |OUTPUT << PB6 |OUTPUT << PB7);
  DDRC = (INPUT << PC0 | INPUT << PC1 |INPUT << PC2 |INPUT << PC3 |INPUT << PC4 |INPUT << PC5 |INPUT << PC6 |INPUT << PC7);
  DDRD = (INPUT << PD0 | INPUT << PD1 |INPUT << PD2 |INPUT << PD3 |INPUT << PD4 |INPUT << PD5 |INPUT << PD6 |INPUT << PD7);        

  ptr = 0;
  USART_Init( 103 );
  countersInit();
  idx = 7;

  for (temp = 0; temp < 8; temp++) position[temp] = 16000;  
  
  sei();
  
  while(1) {
      data = USART_Receive();
      if (0) {
      } else if (ptr == 0) {
        if (data == 255) ptr++;
      } else if (ptr == 1) {
        pkt[0] = data;
        ptr++;
      } else if (ptr == 2) {
        pkt[1] = data;
        ptr++;
      } else if (ptr == 3) {
        if (pkt[0] < 32) {
          // immediate move
          ptr = 0;
          temp = pkt[1];
          temp = (temp<<8);
          temp = temp | data;
          if (temp > 31000) temp = 31000;
          position[(pkt[0] & 0x07)] = temp;
        } else if ( pkt[0] < 64) {
          // cache position
        } else if (pkt[0] = 0xFF) {
          // enguage cached position
        }
      }              
  }
  
}

//   every 2.5ms  counter 2
ISR( TIMER2_COMP_vect ) {
  
  idx++;
  if (idx == 8) idx = 0;
  
  OCR1A = 8000 + position[idx];  
  
  TCNT1 = 0;
  
  SetBit(idx, PORTB);  
  
  startCounter();

}


//   variable .5ms to 2ms  counter 1
ISR( TIMER1_COMPA_vect ) {  

  PORTB = 0x00;
  stopCounter();
  
}


void countersInit() {

  // counter 1 to fast pwm, no output control,  interrupt enabled, clock source off.
  
  TCCR1A = b00000000; 
 
  TCCR1B = b00001000;
  // counter 2 to CTC cdiv=256 ocr2 = 156, interrupt enabled, clock source on.
  
  TCCR2 = b00001110;
  OCR2  = 156;  

  TIMSK  = b10010000;
}