/******************************************************************************
 Title:    ADC -> PWM
 Author:   dan williams
 Date:     Apr 17 2005
 Software: AVR-GCC 3.3 
 Hardware: Any AVR with built-in ADC, tested with ATmega8 at 1Mhz
  
  
  bot controller:
     joystick x (0-5V) on pin 40  PA0
     joystick y (0-5V) on pin 39  PA1
     
     pwm to left motor from pin 21  PD7
     pwm to right motor from pin 4  PB3
     
     dir to left motor from pin 1    PB0
     dir to right motor from pin 2   PB1
     
     reset should have 1K resistor to +5V
     GND pins 11, and 31
     +5V pins 10, 32, 30
     
  
*******************************************************************************/
#include<avr/io.h>

#define Forward 1
#define Reverse 0

#define Threashold 16

#define abs(A) ((A)>0)?(A):(-A)

void pwm_init( );
void adc_init( );
int  Analog  ( uint8_t n);
void SetPWM  ( uint8_t channel, uint8_t value) ;
void SetDir  ( uint8_t side, uint8_t dir );
void dostuff ( ) ;



int main (void)  {
  DDRB = 0xFF;      // port B output
  DDRD = 0xFF;      // port D output
  
  adc_init();          
  pwm_init();
  dostuff();
}
 
   
//------------------------| FUNCTIONS |------------------------

void SetDir  ( uint8_t side, uint8_t dir ) {

 if (side == 0) {
 
   if (dir == 0) {
     PORTB |= 0x01;
   } else {
     PORTB &= (~0x01);
   }
   
 } else {
 
   if (dir == 0) {
     PORTB |= 0x02;
   } else {
     PORTB &= (~0x02);
   }
     
 }
}


void SetPWM( uint8_t channel, uint8_t value) {
   if (channel == 0)   
      OCR0 = value;
   if (channel == 1)   
      OCR2 = value;
}

void adc_init() {
  // Activate ADC with Prescaler 
  ADCSRA =  1 << ADEN  |
            0 << ADSC  |
            0 << ADATE |
            0 << ADIF  |
            0 << ADIE  |
            1 << ADPS2 |
            0 << ADPS1 |
            0 << ADPS0 ;
}

void pwm_init() {
  // clear pwm levels
  OCR0 = 0; 
  OCR2 = 0;
  
  // set up WGM, clock, and mode for timer 0
  TCCR0 = 0 << FOC0  | 
          1 << WGM00 | /* fast pwm */
          1 << COM01 | /* inverted */
          0 << COM00 | /*   this bit 1 for interted, 0 for normal  */
          1 << WGM01 | /* fast pwm */
          0 << CS02  | /* no prescale */
          0 << CS01  |
          1 << CS00  ;
  
  // set up WGM, clock, and mode for timer 2
  TCCR2 = 0 << FOC2  | 
          1 << WGM20 |
          1 << COM21 |
          0 << COM20 | /*   this bit 1 for interted, 0 for normal  */
          1 << WGM21 |
          0 << CS22  |
          0 << CS21  |
          1 << CS20  ;
  
 }


int Analog ( uint8_t n ) {

    // Select pin ADC0 using MUX
    ADMUX = n & 7;
    
    //Start conversion
    ADCSRA |= _BV(ADSC);
    
    // wait until converstion completed
    while (ADCSRA & _BV(ADSC) ) {}
    
    // get converted value
    return ADC;  
}


void dostuff ( ) {
      int X, Y;
      int SpeedL, SpeedR;
      
      while (1) {
        // Gather values
        X = Analog(0) / 2;
        Y = Analog(1) / 2;
        
        // Offset correction
        X -= 255;
        Y -= 255;        
                 
        // Threasholding
        if ( (X > -Threashold) & (X < Threashold) )  X = 0;
        if ( (Y > -Threashold) & (Y < Threashold) )  Y = 0;        
                    
        // Tractor mixing:
        SpeedL = X + Y;
        SpeedR = Y - X;
        
        // Direction Setting
        if (SpeedL < 0) {
           SetDir(0, Reverse );
           SpeedL = -SpeedL;
        } else {
           SetDir(0, Forward );
        }
        
        if (SpeedR < 0) {
           SetDir(1, Reverse ); 
           SpeedR = -SpeedR;
        } else {
           SetDir(1, Forward );
        }
                                
        // range correction       
        if (SpeedL > 255) SpeedL = 255;
        if (SpeedR > 255) SpeedR = 255;        
        
        //output
        SetPWM( 0, SpeedL );
        SetPWM( 1, SpeedR );
      }

}