/*

 +-------------------------------------+
 |                          Sig Vd Gnd |
 |  +---------+   5V O     PB0 [o o o] | chan0 pwm
 |  | 7805  O |   Vd O     PB1 [o o o] | chan1 pwm
 |  +---------+   V+ .     PB2 [o o o] | chan2 pwm
 |                         PB3 [o o o] | chan3 pwm
 |                         PB4 [o o o] | chan0 dir
 |                         PB5 [o o o] | chan1 dir
 |                         PB6 [o o o] | chan2 dir
 |                         PB7 [o o o] | chan3 dir
 |                         PA0 [o o o] | chan0 ctrl voltage
 |                         PA1 [o o o] | chan0 feedback voltage
 |        +----------+     PA2 [o o o] | chan1 ctrl voltage
 |        |O         |     PA3 [o o o] | chan1 feedback voltage
 |        |          |     PA4 [o o o] | chan2 ctrl voltage
 |        |          |     PA5 [o o o] | chan2 feedback voltage
 |        |          |     PA6 [o o o] | chan3 ctrl voltage
 |        |          |     PA7 [o o o] | chan3 feedback voltage
 |        |          |     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] | Debug
 |      E.D.S BABYBOARD III            |
 +-------------------------------------+


*/
/*
Servo Recieves:

 [0][c3][c2][c1][c0][v2][v1][v0]
 [1][v9][v8][v7][v6][v5][v4][v3]
 
 c0-c3 are command
 v0-v9 are value
 
 Commands:
 0  Clear ping counter                                   {always obey command}
 1  pong if ping counter matches id and inc ping counter {always obey command}
 2  listen (servo number) 256 = all                      {always obey command}
 3  ignore (servo number) 256 = all                      {always obey command}
 4  turn servo on                                        {obey if listening}
 5  turn servo off                                       {obey if listening}
 6  set servo position to argument                       {obey if listening}
 7  get servo position                                   {obey if listening}
 15 send device model                                    {obey if listening}
 
Servo Sends:
 [0][p3][p2][p1][p0][v2][v1][v0]
 [1][v9][v8][v7][v6][v5][v4][v3] 
 
 p0-p3 are paramiter number
 v0-v9 are value
 
 Paramiter numbers:
 
 0 pong           (value will be servo number)
 1 servo position (returns position sensor value)
 2 device model   (returns 10 bit device model number)

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


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

#define ABS(a)                ((a) < 0 ? -(a) : (a))
#define SIGN(x)               (x)==0?0:(x)>0?1:-1
#define NOP()                 asm volatile ("nop"::)
// limit v to within low (l) and high (h) limits
#define limit(v, l, h)        ((v) > (h)) ? (h) : ((v) < (l)) ? (l) : (v)
// write value to bit on port
#define WriteBit(b, p, v)    p = (p & ~(1<<b))|((v&1)<<b)

#define debugH()      SetBit(6, PORTD)
#define debugL()      ClearBit(6, PORTD)
#define debugToggle() PORTD ^= (1<<PD6)



#define OUTPUT   1
#define INPUT    0

#define PWM0BIT  0
#define PWM1BIT  1
#define PWM2BIT  2
#define PWM3BIT  3

#define PWM0PORT PORTB
#define PWM1PORT PORTB
#define PWM2PORT PORTB
#define PWM3PORT PORTB

#define DIR0BIT  4
#define DIR1BIT  5
#define DIR2BIT  6
#define DIR3BIT  7

#define DIR0PORT PORTB
#define DIR1PORT PORTB
#define DIR2PORT PORTB
#define DIR3PORT PORTB

// servo id numbers

#define ID0  0
#define ID1  1
#define ID2  2
#define ID3  3

/*****************************| VARIABLES |********************************/
 
volatile int AdcValues[8];
unsigned char pwmcount;
volatile unsigned char pwm0, pwm1, pwm2, pwm3;
volatile unsigned char p0, p1, p2, p3;
unsigned char command;
unsigned int  argument;
unsigned char pingCtr;
unsigned char listen[4];
unsigned char power[4];
unsigned int cmdpos[4];
 
/************************| FUNCTION DECLARATIONS |*************************/
 
void timerInit(void);
void AnalogInit (void);
int  Analog (int n);
unsigned int dobyte(char data) ;

/****************************| CODE... |***********************************/

int main (void) {
 
  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 |OUTPUT << PD6 |INPUT << PD7);        

  pwm0 = 0;
  pwm1 = 0;
  pwm2 = 0;
  pwm3 = 0;
  
  USART_Init( 103 ); // 9600 @ 16Mhz
  AnalogInit();
  timerInit();
  power[0] = 0; power[1] = 0; power[2] = 0; power[3] = 0; // all servos off
  
  // turn interrupts on
  sei();
  
  // calculate servo loops
  while(1){
    
    if ( (UCSRA & (1<<RXC)) ) {
        dobyte(USART_Receive());    
    } 
   
    temp = cmdpos[0]-AdcValues[1];
    pwm0 = limit(ABS(temp), 0, 255);
    if (temp > 0)     SetBit(DIR0BIT, DIR0PORT);
    else              ClearBit(DIR0BIT, DIR0PORT);
    
    temp = cmdpos[1]-AdcValues[3];
    pwm1 = limit(ABS(temp), 0, 255);
    if (temp > 0)     SetBit(DIR1BIT, DIR1PORT);
    else              ClearBit(DIR1BIT, DIR1PORT);
    
    temp = cmdpos[2]-AdcValues[5];
    pwm2 = limit(ABS(temp), 0, 255);
    if (temp > 0)     SetBit(DIR2BIT, DIR2PORT);
    else              ClearBit(DIR2BIT, DIR2PORT);
    
    temp = cmdpos[3]-AdcValues[7];
    pwm3 = limit(ABS(temp), 0, 255);
    if (temp > 0)     SetBit(DIR3BIT, DIR3PORT);
    else              ClearBit(DIR3BIT, DIR3PORT);
    
  }

}

//------------------------| FUNCTIONS |------------------------

unsigned int dobyte(char data) {

  unsigned int reply;
  reply = 0;

  if ((data & 0x80)==0) { //process comamnd on second byte
   command = (data >> 3);
   argument = (argument & 0x7F8) | (data & 0x07); // glue in its 0 through 2
  } else {
   argument = (argument & 0x07) | ((data & 0x7F) << 3); //glue in bits 3 through 9
   switch(command) {
     case 0: // Clear ping counter
       pingCtr = 0;
     break;
     case 1: // pong if ping counter matches id and inc ping counter
      if ((pingCtr == ID0) | (pingCtr == ID1) | (pingCtr == ID2) | (pingCtr == ID3)) {
        reply = ((pingCtr << 5) & 0xFF00)|(pingCtr & 0x07)|0x8000;
      }
      pingCtr++;
     break;
     case 2: // listen(id)
       if (argument == ID0)      listen[0] = 1;
       else if (argument == ID1) listen[1] = 1;
       else if (argument == ID2) listen[2] = 1;
       else if (argument == ID3) listen[3] = 1;
       else if (argument == 256) { listen[0] = 1; listen[1] = 1; listen[2] = 1; listen[3] = 1; }
     break;
     case 3: // ignore(id)
       if (argument == ID0)      listen[0] = 0;
       else if (argument == ID1) listen[1] = 0;
       else if (argument == ID2) listen[2] = 0;
       else if (argument == ID3) listen[3] = 0;
       else if (argument == 256) { listen[0] = 0; listen[1] = 0; listen[2] = 0; listen[3] = 0; }
     break;
     case 4: // servo on 
       if (listen[0]) power[0] = 1;
       if (listen[1]) power[1] = 1;
       if (listen[2]) power[2] = 1;
       if (listen[3]) power[3] = 1;
     break;
     case 5: // servo off
       if (listen[0]) power[0] = 0;
       if (listen[1]) power[1] = 0;
       if (listen[2]) power[2] = 0;
       if (listen[3]) power[3] = 0;
     break; 
     case 6: // set servo position (position)
       if (listen[0]) cmdpos[0] = argument;
       if (listen[1]) cmdpos[1] = argument;
       if (listen[2]) cmdpos[2] = argument;
       if (listen[3]) cmdpos[3] = argument;
     break; 
     case 7: // get servo position
       if (listen[0])      reply = ((AdcValues[0] << 5) & 0xFF00)|(AdcValues[0] & 0x07)|0x8000|(1<<3);
       else if (listen[1]) reply = ((AdcValues[2] << 5) & 0xFF00)|(AdcValues[2] & 0x07)|0x8000|(1<<3);
       else if (listen[2]) reply = ((AdcValues[4] << 5) & 0xFF00)|(AdcValues[4] & 0x07)|0x8000|(1<<3);
       else if (listen[3]) reply = ((AdcValues[6] << 5) & 0xFF00)|(AdcValues[6] & 0x07)|0x8000|(1<<3);
     break; 
     case 15: // return device model
       reply = 0x0001;  
     break; 
   }
  }
  return reply;
}

/*********************************************************
  !!!!!!!!! SEVERLY OPTIMIZED CODE  !!!!!!!!!
   REFERNENCE ASSEMBLER OUTPUT WHEN MODIFYING
               THIS PWM FUNCTION
                    gcc -O1
**********************************************************/
SIGNAL (SIG_OVERFLOW0) {

 if (++pwmcount == 0){
  p0 = pwm0; p1 = pwm1; p2 = pwm2; p3 = pwm3;
  if (power[0] != 0) SetBit(PWM0BIT, PWM0PORT);
  if (power[1] != 0) SetBit(PWM1BIT, PWM1PORT);
  if (power[2] != 0) SetBit(PWM2BIT, PWM2PORT);
  if (power[3] != 0) SetBit(PWM3BIT, PWM3PORT);
 };

 if ((p0 ^ pwmcount) == 0) { ClearBit(PWM0BIT, PWM0PORT); }
 if ((p1 ^ pwmcount) == 0) { ClearBit(PWM1BIT, PWM1PORT); }
 if ((p2 ^ pwmcount) == 0) { ClearBit(PWM2BIT, PWM2PORT); }
 if ((p3 ^ pwmcount) == 0) { ClearBit(PWM3BIT, PWM3PORT); }
}

void timerInit(void) {

  // set prescaler to /1
  TCCR0 = (0 << FOC0 | 0 << WGM00 |0 << COM01 |0 << COM00 |0 << WGM01 |0 << CS02 |0 << CS01 |1 << CS00);
  
  // set interrupt mask register 
  TIMSK |= (1<<TOIE0);  

}

void AnalogInit (void) {

  int i;

  // Activate ADC with Prescaler 
  ADCSRA =  1 << ADEN  |
            1 << ADSC  | /* start a converstion, irq will take from here */
            0 << ADATE |
            0 << ADIF  |
            1 << ADIE  | /* enable interrupt */
            1 << ADPS2 |
            0 << ADPS1 |
            0 << ADPS0 ;
                        
            
  for (i = 0; i < 8; AdcValues[i++] = 0);
  
}

int Analog (int n) {
  return AdcValues[n & 7];
}

SIGNAL(SIG_ADC) {
  int i;

  i = ADMUX & 7;       // what channel we on?
  AdcValues[i] = ADC;  // save value
  i++;                 // next value
  ADMUX = i & 7;       // select channel
  ADCSRA |= _BV(ADSC); // start next conversion

  return;
}