#include <Mega32.h> 
//timeout values for each task
#define t1 8 
#define maxSpeed 250
#define begin {
#define end }   

//State machine state names
#define NoPush 1 
#define MaybePush 2
#define Pushed 3
#define MaybeNoPush 4
unsigned char PushState;
unsigned char PushFlag;                        //message indicating a button push      

char led;           
char Ain;
int hitTimerx, hitTimery, startMotor;

void initialize(void); //all the usual mcu stuff
void buttons(void);    //input debouncer  
void step(void);       //steps the motor
char forward(char);
char backward(char);
          
char time1, key, leftDir, rightDir, Rstep, Lstep;      //timeout counters
char yhit, lefthit, righthit, turncounter, reversecounter;
int speed, stepTimer, motorPWM;  //controls speed of motor 
         
//**********************************************************
//timer 0 compare ISR
interrupt [TIM0_COMP] void timer0_compare(void)
begin      

  //get the sample            
  if(startMotor>0) startMotor--;
  if(hitTimerx>0) hitTimerx--;     
  if(hitTimery>0) hitTimery--;   
  if(motorPWM>0) motorPWM--;
                                                                
  if (motorPWM >= 45 & rightDir & leftDir & startMotor ==0) PORTD = 0xff;     //turn motor on
  else                                    PORTD = 0x00;                        
  if (motorPWM == 0)                      motorPWM = 50;
  
  
  Ain = ADCH;                      
  

  if((ADMUX & 0x01) == 0x01)        //if ADMUX == 1, y-axis accelerometer        
  begin 
    //ledy = ~0x00;
    if(hitTimery == 0)
    begin
      //front collision
           if(Ain > 172)                        //x collision threshold max
                                                           //176
      begin            
        hitTimery = 5000;                                  //top hit 
        yhit = 1;   
        //led = led ^ 0x80;             
      end  
    end //hitTimery               
  end //if admux 
  
  if((ADMUX & 0x01) == 0x00)                                                //if ADMUX == 0, x-axis accelerometer     
  begin 
    if(hitTimerx == 0)
    begin         
      if(Ain<120)                        //140                          //left hit
      begin
         hitTimerx = 5000;          
         lefthit = 1;
         //led = led ^ 0x80;
      end
      if(Ain>165)         //104                              //right hit
      begin
        hitTimerx = 5000; 
        righthit = 1; 
        //led = ~0x80;
      end                    
    end   
  end  


  ADMUX = ADMUX ^ 0x01;       //FLIP BIT 0
  ADCSR.6=1; 

  //Decrement the three times if they are not already zero
  if (time1>0)          --time1;
  if (stepTimer<maxSpeed)  stepTimer++;
end  



//**********************************************************       
//Entry point and task scheduler loop
void main(void)
begin  
  initialize(); 
  //main task scheduler loop 
  while(1)
  begin     
    //if (time1==0)              buttons();
    if (stepTimer==maxSpeed)  step();
  end
end  
  
//**********************************************************          
//Debounce and interpret buttons
void buttons(void) 
begin  
  time1=t1;  //reset the task timer
  //key = ~PIND;
  switch (PushState)
  begin
     case NoPush: 
        if (key) PushState=MaybePush;
        else PushState=NoPush;
        break;
     case MaybePush:
        if (key) 
        begin
           PushState=Pushed;   
           PushFlag=1;
        end
        else PushState=NoPush;
        break;
     case Pushed:  
        if (key) PushState=Pushed; 
        else PushState=MaybeNoPush;    
        break;
     case MaybeNoPush:
        if (key) PushState=Pushed; 
        else 
        begin
           PushState=NoPush;
           PushFlag=0;
        end    
        break;
  end

  if (PushFlag)
  begin
    if (key == 0x01) speed++;
    if (key == 0x02) speed--;
    if (key == 0x80) leftDir=~leftDir;
    if (key == 0x40) rightDir=~rightDir;
  end
end

//**********************************************************
//Call step state machines and output to motor
void step(void)
begin
  stepTimer = speed;   
  if(yhit)
  begin      
     if(righthit)  
     begin    
       //~48 steps per revolution
       turncounter = 80;
       speed = 210;
       leftDir = ~leftDir;
     end
     if(lefthit)   
     begin
       turncounter = 80;
           rightDir = ~rightDir;
           speed = 210;   
     end
     if(!lefthit && !righthit)    
     begin              
       turncounter = 110;
       reversecounter = 50;
       speed = 210;
       leftDir = 0x00;
       rightDir = 0x00;       
     end
     yhit=0;
     righthit=0; 
     lefthit=0;   
  end
  if (reversecounter == 1)  
  begin
    rightDir = ~rightDir;
    hitTimerx = 20;
    hitTimery = 20; 
  end
  if (turncounter == 1)
  begin
    rightDir = 0xff;
    leftDir = 0xff;
    hitTimery = 20;
    hitTimerx = 20;
  end
  if ((turncounter>0)&&(!reversecounter))  turncounter--;  
  if (reversecounter>0)   reversecounter--; 
  if ((!turncounter)&&(speed<235))  speed++;
  
  //step left motor 
  
  if (leftDir)  Lstep = forward(Lstep);
  else          Lstep = backward(Lstep);
  //step right motor
  if (rightDir) Rstep = forward(Rstep);
  else          Rstep = backward(Rstep);
  
  if (startMotor==0)
  begin
     //output bits to motor
     PORTC = (Lstep<<4) | Rstep;  
  end 
  
end

//**********************************************************
//Motor step functions
//Step the motor in the FORWARD direction
char forward(char x)
begin  

//PORTD = 0xff;           //turn motor on
  
  switch(x)
  begin
    case (1+4):
      x = 2+4;
      break;
    case (2+4):
      x = 2+8;
      break;
    case (2+8):
      x = 1+8;
      break;
    case (1+8):
      x = 1+4;
      break;
    default:
      x = 1+4;
      break;
  end
return x;
end

//Step the motor in the backward direction
char backward(char x)
begin     
//PORTD = 0x00;           //turn motor off
  switch(x)
  begin
    case (1+4):
      x = 1+8;
      break;
    case (1+8):
      x = 2+8;
      break;
    case (2+8):
      x = 2+4;
      break;
    case (2+4):
      x = 1+4;
      break;
    default:
      x = 1+4;
      break;
  end
return x;
end

//********************************************************** 
//Set it all up
void initialize(void)
begin
  //set up the ports
  DDRD=0xff;    // PORT D is an ouput  
  PORTD=0x00;  
  
  DDRC=0xff;    // PORT C is an ouput  
  PORTC=0; 
  led = 0xff;
      
  //set up timer 0  
  TIMSK=2;                //turn on timer 0 cmp match ISR 
  OCR0 = 250;                  //set the compare re to 250 time ticks
  //prescalar to 64 and turn on clear-on-match
  TCCR0=0b00001011;
  //one tick = 4ms 
  
  //init the  message
  //for no button push
  PushFlag = 0;
  //init the state machine
  PushState = NoPush;
  
  speed=235;
  stepTimer=speed;          
  //init the task timers
  time1=t1;
  
  leftDir=0xff;
  rightDir=0xff;
  Lstep=5;
  Rstep=5;
  yhit=0;
  righthit=0;
  lefthit=0;                  
  motorPWM = 10;
  
  //init the A to D converter 
  //bit7:6 use internal 2.5V reference with capacitor at AREF pin
  //bit 5 left adjust result
  ADMUX = 0b11100000;              
  
  hitTimerx = 7000;
  hitTimery = 7000; 
  startMotor = 5000;               //SET BACK TO 5000

  //enable ADC and set prescaler to 1/128*16MHz=125,000
  //and clear interupt enable
  //and start a conversion
  ADCSR = 0b11000111; 
  //init the UART
  UCSRB = 0x18;
  UBRRL = 103;

  //crank up the ISRs
  #asm
          sei
  #endasm 
end