/*

 +-------------------------------------+
 |                          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] |
 |      E.D.S BABYBOARD III            |
 +-------------------------------------+


*/
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.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 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

/*****************************| VARIABLES |********************************/
 
volatile int AdcValues[8];
unsigned char pwmcount;
volatile unsigned char pwm0, pwm1, pwm2, pwm3;
volatile unsigned char p0, p1, p2, p3;
 
/************************| FUNCTION DECLARATIONS |*************************/
 
void timerInit(void);
void AnalogInit (void);
int  Analog (int n);

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

int main (void) {
 
  int temp;

  // 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);        

  pwm0 = 0;
  pwm1 = 0;
  pwm2 = 0;
  pwm3 = 0;
  
  AnalogInit();
  timerInit();
  
  // turn interrupts on
  sei();
  
  // calculate servo loops
  while(1){
    temp = AdcValues[0]-AdcValues[1];
    pwm0 = limit(ABS(temp), 0, 255);
    if (temp > 0)     SetBit(DIR0BIT, DIR0PORT);
    else              ClearBit(DIR0BIT, DIR0PORT);
    
    temp = AdcValues[2]-AdcValues[3];
    pwm1 = limit(ABS(temp), 0, 255);
    if (temp > 0)     SetBit(DIR1BIT, DIR1PORT);
    else              ClearBit(DIR1BIT, DIR1PORT);
    
    temp = AdcValues[4]-AdcValues[5];
    pwm2 = limit(ABS(temp), 0, 255);
    if (temp > 0)     SetBit(DIR2BIT, DIR2PORT);
    else              ClearBit(DIR2BIT, DIR2PORT);
    
    temp = AdcValues[6]-AdcValues[7];
    pwm3 = limit(ABS(temp), 0, 255);
    if (temp > 0)     SetBit(DIR3BIT, DIR3PORT);
    else              ClearBit(DIR3BIT, DIR3PORT);
    
  }

}

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

/*********************************************************
  !!!!!!!!! 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;
  SetBit(PWM0BIT, PWM0PORT);
  SetBit(PWM1BIT, PWM1PORT);
  SetBit(PWM2BIT, PWM2PORT);
  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;
}