/******************************************************************************
 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 
     pwm to right motor from pin 
     
     dir to left motor from pin 
     dir to right motor from pin 
     
     reset should have 1K resistor to +5V
     GND pins 11, and 31
     +5V pins 10, 32, 30
     
  
  
 +-------------------------------------+
 |                          Sig Vd Gnd |
 |  +---------+   5V O     PB0 [o o o] | 
 |  | 7805  O |   Vd O     PB1 [o o o] | 
 |  +---------+   V+ .     PB2 [o o o] | 
 |                         PB3 [o o o] | right pwm
 |                         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] | left pwm
 |        |          |     PD2 [o o o] |
 |        |          |     PD3 [o o o] |
 |        |          |     PD4 [o o o] |
 |        |          |     PD5 [o o o] | left dir
 |        +----------+     PD6 [o o o] | right dir
 |      E.D.S BABYBOARD III            |
 +-------------------------------------+

  
  
*******************************************************************************/
#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

//Motor Setup

#define LDir                 5
#define RDir                 6
#define LForward()           SetBit    (LDir, PORTD)
#define LReverse()           ClearBit  (LDir, PORTD)
#define RForward()           SetBit    (RDir, PORTD)
#define RReverse()           ClearBit  (RDir, PORTD)  
#define PWMRight             OCR0
#define PWMLeft              OCR2



void pwm_init    ( );
void adc_init    ( );
void serial_init ( );
int  Analog      ( uint8_t n);
void SetPWM      ( uint8_t left, uint8_t right );
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 |=========================


//------------------| initialize ADC |----------------------

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

//------------------| initialize PWM |----------------------

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  ;
  
 }

//------------------| initialize serial |----------------------

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

}

//------------------| Get analog value |----------------------

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;  
}

//----------------| Main Pogram Loup |-----------------

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;               
                                
        // range correction       erm, did it all come out ok?
        
        if (SpeedL > 255) SpeedL = 255;
        if (SpeedR > 255) SpeedR = 255;        
        
        //output
        SetPWM( SpeedL, SpeedR );
      }

}

//-----------------| Set Pwm rates |------------------


void SetPWM( uint8_t left, uint8_t right ) {
   
     if (left > 0) {
       PWMLeft = left;
       LForward();       
     } else {
       PWMLeft = -(left);
       LReverse();       
     }
     
     if (right > 0) {
       PWMRight = right;
       RForward();       
     } else {
       PWMRight = -(right);
       RReverse();       
     }
    
}

//----------| Serial character recieve |-------------

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;
         
  }
}