#include <Mega32.h>
#include <stdio.h>

#define stop 0
#define straight 3
#define left_turn 1
#define right_turn 2
#define middle 4
#define left 5
#define right 6
#define move 7
#define begin {
#define end   }

int sensor_timer, go_timer, turn_timer;
int Ain, midRd, leftRd, rightRd; 		//raw A to D number
char car_state, sensor_state;
bit sensor, go, turn;

void main(void);
void initialize(void);
void sensor_check(void);
void move_car(void);

//------------------------------------------------------------------
interrupt [TIM2_OVF] void timer2_ovf(void)
begin
   TCNT2 = 6;
   sensor_timer ++;
   if (turn)
   turn_timer++;

end

//------------------------------------------------------------------
void main(void)
begin
   initialize();
   #asm
   sei
   #endasm

   while(1)
   begin
     sensor_check();
   end
end


//------------------------------------------------------------------
void sensor_check(void)
begin
     switch (sensor_state)
     begin
            case move:
            move_car();
            sensor_timer = 0;
            sensor_state = right;

       break;
       case right:
       if (sensor_timer >= 2)
       begin
            //get the sample
            Ain = ADCH;
	    ADMUX =  0b01100001;   //Read Port A1
            //start another conversion
            ADCSR.6=1;
            rightRd = Ain;
            sensor_state = left;
       end
       else sensor_state = right;
       break;

       case left:
       if (sensor_timer >= 4)
       begin
            //get the sample
            Ain = ADCH;
 	    ADMUX =  0b01100000;   //Read Port A0
            //start another conversion
            ADCSR.6=1;
            leftRd = Ain;  
            sensor_state = middle;
       end
       else sensor_state = left;
       break;

       case middle:
       if (sensor_timer >= 6)
       begin
            // get the sample
            Ain = ADCH;
 	    ADMUX = 0b01100010;   //Read Port A2
            // start another conversion
            ADCSR.6=1;
            midRd = Ain;  
            sensor_state = move;       
       end
       else sensor_state = middle;
       break;
    end
end


//------------------------------------------------------------------
void move_car(void)
begin
     //printf("%d\n\r",midRd);
     switch (car_state)
     begin

          case stop:
          // Car is not over a path
          if (midRd>80 && leftRd>80 && rightRd>80)
          begin
               TCCR0 = 0b00000000;
               PORTB.3 = 0;
               //Stops steering motor
               PORTA.3 = 0;
               PORTA.4 = 0;
               PORTA.5 = 0;
               PORTA.6 = 0;
          end
          
          else
          begin
                TCCR0 = 0b01101100;   //Enable PWM mode PRESCALER 64    
                 OCR0 = 128; 
                 car_state = straight;
 		      end
 		      break;
 		      
          case straight:
          if (midRd>80 && leftRd>80 && rightRd>80)
          begin
               TCCR0 = 0b00000000;
               PORTB.3 = 0;
               //Stops steering motor
               PORTA.3 = 0;
               PORTA.4 = 0;
               PORTA.5 = 0;
               PORTA.6 = 0; 
               car_state = stop;
          end

          else if (leftRd<40 && rightRd>80)
          begin
               //Turn off other transistors
               PORTA.5 = 0;
               PORTA.6 = 0;
               //Power steering motor left
               PORTA.3 = 1;
               PORTA.4 = 1;
               car_state =left_turn; 
                         turn = 1;
               end

          else if (rightRd<40 && leftRd>80)
          begin
               //Turns off other transistors
               PORTA.3 = 0;
               PORTA.4 = 0;
               //Power steering motor right
               PORTA.6 = 1;
               PORTA.5 = 1;
               car_state = right_turn; 
               turn = 1;
	  end
          break;

          case left_turn: 
          if (turn_timer >= 1500)
          begin
              // Car is not over a path
              if (midRd>80 && leftRd>80 && rightRd>80)
              begin   
                   //Turn off all motors
                   TCCR0 = 0b00000000;  
                   PORTB.3 = 0;
                   PORTA.3 = 0;
                   PORTA.4 = 0;
                   PORTA.5 = 0;
                   PORTA.6 = 0;  
                   car_state = stop;
              end    
              turn_timer = 0;
          end

          //If middle sensor is not over the path
          if (midRd>80 && leftRd<40 && rightRd>80)
          begin
               //Power steering motor left
               PORTA.3 = 1;
               PORTA.4 = 1;
               PORTA.5 = 0;
               PORTA.6 = 0;
          end

          //Car has readjusted itself to course - ie it has turned left
          else if (midRd<40 && leftRd>80 && rightRd>80)
          begin
               //Left Brake
               PORTA.5 = 0;
               PORTA.4 = 0;
               PORTA.3 = 1;   
               PORTA.6 = 1;
               car_state = stop;
          end

          //If the car overshoots to the left
          else if (rightRd<40 && leftRd>80)
          begin
              //Left Brake
              PORTA.5 = 0;
              PORTA.4 = 0;
              PORTA.3 = 1;
              PORTA.6 = 1;
              car_state = right_turn;
          end
          break;

          case right_turn:  
     
          if (turn_timer >= 1500)
          begin
    	       if (midRd>80 && leftRd>80 && rightRd>80)
    	       begin
                    //Turn off all motors
                    TCCR0 = 0b00000000; 
    	            PORTB.3 = 0;
                    PORTA.3 = 0;
                    PORTA.4 = 0;
                    PORTA.5 = 0;
                    PORTA.6 = 0;
                    car_state = stop;
    	       end   
    	       turn_timer = 0;
          end

          //If middle sensor is not over the path
          if (midRd>80 && rightRd<40 && leftRd>80)
          begin
               //Power steering motor right
               PORTA.6 = 1;
               PORTA.5 = 1;
               PORTA.4 = 0;
               PORTA.3 = 0;
          end

          //Car has readjusted itself to course - ie it has turned right
          else if (midRd<40 && leftRd>80 && rightRd>80)
          begin
              //Right Brake
              PORTA.5 = 0;
              PORTA.4 = 0;
              PORTA.3 = 1;
              PORTA.6 = 1;
              car_state = stop;
          end

          //If the car overshoots to the right
          else if (leftRd<40 && rightRd>80)
          begin
              //Right Brake
              PORTA.5 = 0;
              PORTA.4 = 0;
              PORTA.3 = 1;
              PORTA.6 = 1;
              car_state = left_turn;
          end
          break;
     end
end


//------------------------------------------------------------------
void initialize(void)
begin
     DDRA = 0b11111000;         //Makes PortA0 - A2 inputs, A3 - A7 outputs 
     DDRB.3 = 1;                //Makes PortB0 output
     TCCR2 = 0b00000011;        //Normal Mode with Prescaler of 64
     TIMSK = 0b01000000;        //Enable Timer 0 interrupt
     sensor_timer = 0;
     car_state = stop;
     sensor_state = right;
     PORTA.4 = 0;               //No power to motor circuitry
     PORTA.5 = 0;
     PORTA.6 = 0;
     PORTA.7 = 0;
     go_timer = 0;
     ADMUX = 0b01100010;
     ADCSR = 0b11000111;        //ADC running with prescaler of 128 so 8 usec per reading
end