/******************************************************************************
 Title:    ROV reciever
 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:
     
     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>
//#include <avr/interrupt.h>
//#include <avr/signal.h>
#include "usart.h"

#define OUTPUT             1
#define INPUT              0

#define Forward            1
#define Reverse            0

#define Threashold         20

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


#define SetBit(BIT, PORT)     PORT |= (1<<BIT)
#define ClearBit(BIT, PORT)   PORT &= ~(1<<BIT)
#define WriteBit(BIT, PORT, VALUE)  PORT = (PORT & ~(1<<BIT))|((VALUE&1)<<BIT)

#define IsHigh(BIT, PORT)    (PORT & (1<<BIT)) != 0
#define IsLow(BIT, PORT)     (PORT & (1<<BIT)) == 0

#define Left, 0
#define Right 1


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

 unsigned char state;
 unsigned char value;
 int           X, Y;
 unsigned char CX, CY;
 int           SpeedL, SpeedR;
 

int main (void)  {

  DDRA = (OUTPUT << PA0 | OUTPUT << PA1 |INPUT << PA2 |INPUT << PA3 |INPUT << PA4 |INPUT << PA5 |INPUT << PA6 |INPUT << PA7);
  DDRB = (OUTPUT << PB0 | OUTPUT << PB1 |INPUT << PB2 |OUTPUT << PB3 |INPUT << PB4 |INPUT << PB5 |INPUT << PB6 |INPUT << 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 |OUTPUT << PD7);        
  
  adc_init();          
  pwm_init();
  //serial_init();
  USART_Init( 207 ); //4800 @ 16Mhz
  USART_printstring("READY.\n\r");
  //sei();  //enable innterupts
  
  CX = 128;
  CY = 128;
      
  dostuff();
}
 
   
//------------------------| FUNCTIONS |------------------------

void SetDir  ( uint8_t side, uint8_t dir ) {

 if (side == Left) {
 
   if (dir == Reverse) {
     SetBit(0, PORTB);
   } else {
     ClearBit(0, PORTB);
   }
   
 } else {
 
   if (dir == Reverse) {
     SetBit(1, PORTB);
   } else {
     ClearBit(1, PORTB);
   }
     
 }
}


void SetPWM( uint8_t channel, uint8_t value) {
   if (channel == Left)   
      OCR0 = value;
   if (channel == Right)   
      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 */
          1 << COM00 | /*   this bit 1 for interted, 0 for normal  */
          1 << WGM01 | /* fast pwm */
          0 << CS02  | /* prescale by 8*/
          1 << CS01  |
          1 << CS00  ;
  
  // set up WGM, clock, and mode for timer 2
  TCCR2 = 0 << FOC2  | 
          1 << WGM20 |
          1 << COM21 |
          1 << COM20 | /*   this bit 1 for interted, 0 for normal  */
          1 << WGM21 |
          0 << CS22  |
          1 << CS21  |
          0 << CS20  ;
  
 }

void serial_init() {
   /* Set baud rate */
   UBRRH = (unsigned char)(25>>8);
   UBRRL = (unsigned char)25;
   /* set 2X baud rate */
   UCSRA = (1<<U2X);
   /* Enable receiver and interrupts */
   UCSRB = (1<<RXCIE)|(1<<RXEN);
   /* Set frame format: No parity, 8data, 1stop bit */
   UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0);   //For devices without Extended IO

}

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 ( ) {
        
      while (1) {
      
        processByte(USART_Receive());
       
        // Gather values
      //  X = Analog(0) / 2;
      //  Y = Analog(1) / 2;
      
        // Offset correction        
        X = ((CX - 128) * 2);
        Y = ((CY - 128) * 2);
                 
        // 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(Left, Reverse );
           SpeedL = -SpeedL;
        } else {
           SetDir(Left, Forward );
        }
        
        if (SpeedR < 0) {
           SetDir(Right, Reverse ); 
           SpeedR = -SpeedR;
        } else {
           SetDir(Right, Forward );
        }
                                
        // range correction       
        if (SpeedL > 255) SpeedL = 255;
        if (SpeedR > 255) SpeedR = 255;        
        
        //output
        SetPWM( Left, SpeedL );
        SetPWM( Right, SpeedR );
      }

}

//-----------------------------------------


  
void processByte(char newChar) {  
//SIGNAL (SIG_UART_DATA) {
 
 // char newChar; 
 // newChar = UDR;

  switch(state) {
  
    case 0:
       switch(newChar) {
         case 'P':
            state = 1;
         break;
       //  default:
         //   USART_Transmit( '?' );
       }
    break;
    
    case 1:
      switch(newChar){
        case 'X':
         state = 10;
        break;
        
        case 'Y':
         state = 20;
        break;
        
        default:
      //  USART_Transmit( '?' );
        state = 0;
      } 
    break;
      
    // scrap the error checking...
    case 10:      
    case 20:
      if (newChar > '9') { newChar -= 7; }
      value = (newChar&(0x0F)) << 4;      
      state++;
    break;
      
    case 11:
      if (newChar > '9') { newChar -= 7; }
      value |= (newChar&(0x0F)) ;    
      CX = value;
     // USART_printstring("OK.\n\r");
      state = 0;
    break;   
    
    case 21:
      if (newChar > '9') { newChar -= 7; }
      value |= (newChar&(0x0F)) ;    
      CY = value;
     // USART_printstring("OK.\n\r");
      state = 0;
    break;
         
  }
}