roboforum.ru

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

#define begin {
#define end   }
#define dTime 1000
#define srTime 300
#define slTime 200
#define rTime 100
#define left 0
#define right 1
#define forward 2
#define stop 3

char mic1, roundFinished, firstFinished, secondFinished;                              //mic ADC
char m1value, m2value, m3value;                                                                     //mic signals
int driveTime, steerTime, resetTime;                                                            //timers for driving
char drive, steer, enableResetTime, direction, turnedLeft, turnedRight;         //driving var

void initialize(void);             
void resetWheel(void);

//==================================
//Timer0 Interrupt
interrupt [TIM0_COMP] void tim0isr(void) 
begin
   //rear motor timer
   if (drive==1 && driveTime>0) driveTime--;
   //steering motor timer
   if (steer==1 && steerTime>0) steerTime--;         
   //reseting steering wheel timer
   if (enableResetTime==1) resetTime--;
end //timer0 ISR

//==================================
//ADC Interrupt
interrupt [ADC_INT] void adcComp(void) 
begin
   if (mic1==1)                        //first mic ADC
   begin
         m1value = ADCH;               //retrieve ADC value
         ADMUX = 0b01100010;         //switch pin
         mic1 = 2;         
         firstFinished = 1;
   end
   else   
   begin
      if (mic1==2)                     //second mic ADC
      begin
         m2value = ADCH;               //retrieve ADC value
         ADMUX = 0b01100011;         //switch pin
         mic1 = 3;
         secondFinished = 1;
      end
      else                                 //third mic ADC
      begin
         m3value = ADCH;               //retrieve ADC value
         ADMUX = 0b01100001;         //switch pin
         mic1 = 1;
         roundFinished = 1;
      end
   end
end //ADC ISR

//==================================
//initialize
void initialize(void)
begin
   DDRA = 0x00;            //ADC
   DDRB = 0xff;            //motor B7=f/r B6=l/r B5=pwm
   DDRC = 0xff;            //LED
   DDRD = 0xff;            //LED
   //TIM0 SETUP Interrupt every 1ms
   OCR0 = 249;
   TIMSK = 0b00000010;
   TCCR0 = 0b00001011;

   //ADC SETUP
   //init the A to D converter
   //channel zero/ left adj /EXTERNAL Aref
   ADMUX = 0b01100001;   
   //enable ADC and set prescaler to 1/128*16MHz=125,000
   ADCSR = 0b10001110;

   //init variables
   mic1 = 1;
   firstFinished = 0;
   secondFinished = 0;
   roundFinished = 0;
   drive = 0;
   steer = 0;   
   driveTime = dTime;
   steerTime = srTime;
   resetTime = rTime;
   turnedRight = 0;
   turnedLeft = 0;

   #asm
      sei
   #endasm
end //initialize
     
//==================================
//resetWheel
void resetWheel(void)
begin
   if (turnedLeft==1)         //previous turned left, turn right
   begin
      PORTB.6 = 1;
      PORTB.5 = 1;
   end
   if (turnedRight==1)         //previous turned right, turn left
   begin
      PORTB.6 = 0;
      PORTB.5 = 1;
   end               
   turnedLeft = 0;
   turnedRight = 0;
   enableResetTime = 1;
   resetTime = rTime;
end

//==================================
//main
void main(void)
begin
   initialize();     
   PORTD=0xff;            //turn LED off
   delay_ms(2000);
   
   ADCSR.6 = 1;         //start ADC

   while(1)
   begin       
      if (resetTime==0 && enableResetTime==1)            //reseting wheels done
      begin
         PORTB.5 = 0;                                                //stop PWM
         enableResetTime = 0;
         delay_ms(2000);                                             //delay for noise from the motor
         ADCSR.6 = 1;                                                // anoter ADC
      end
      if (driveTime==0 && drive==1)                  //forward drive done
      begin       
         drive = 0;
         direction = stop;
         PORTB.7 = 0;
         if (turnedLeft==1 || turnedRight==1)
         begin
            resetWheel();
            PORTC=0b00110011;
         end
         else
         begin
            delay_ms(2000);
            ADCSR.6 = 1;
         end
      end
      if (steerTime==0 && steer==1)                  //steering done
      begin       
         PORTC = 0xaa;
         steer = 0;
         direction = stop;
         PORTB.5 = 0;
      end                 
      if (firstFinished==1)                                       //first mic ADC done
      begin           
         firstFinished = 0;
         ADCSR.6 = 1;            
      end
      if (secondFinished==1)                                    //second mic ADC done
      begin           
         secondFinished = 0;
         ADCSR.6 = 1;            
      end
      if (roundFinished==1)                                       //third mic ADC done
      begin
         roundFinished = 0;
         //right turn
         if (m1value>=180 && m2value<180 &&m3value<180)   //first mic closest to the source
         begin
            roundFinished = 0;
            direction = right;
         end
         //left turn
         if (m1value<180 && m2value>=180 && m3value<180)   //second mic closest to the source
         begin
            roundFinished = 0;
            direction = left;
         end
         //forward
         if (m1value<180 && m2value<180 && m3value>=180)   //third mic closest to the source
         begin
            direction = forward;
            roundFinished = 0;
         end   
         if (m1value<180 && m2value<180 && m3value<180)   //no signals received
         begin
            roundFinished = 0;
            ADCSR.6 = 1;            
         end   
         if (m1value>=180 && m2value>=180 && m3value>=180)   //reset
         begin
            PORTD=0x00;
            initialize();
            roundFinished = 0;
         end                 
         //driving right b6=0 left b6=1
         if (direction==forward)            //drive forward
         begin
            PORTC = 0b01111111;
            drive = 1;
            steer = 0;
            direction = stop;
            driveTime = dTime;
            PORTB.7 = 1;            
         end
         if (direction==left)               //drive left
         begin
            PORTC = 0b10111111;
            drive = 1;
            steer = 1;
            direction = stop;
            driveTime = dTime;
            steerTime = slTime;
            PORTB.7 = 1;
            PORTB.6   =   0;      
            PORTB.5   =   1;   
            turnedLeft = 1;
         end
         if (direction==right)               //drive right
         begin
            PORTC = 0b11011111;
            drive = 1;
            steer = 1;
            direction = stop;
            driveTime = dTime;
            steerTime = srTime;
            PORTB.7 = 1;
            PORTB.6   =   1;      
            PORTB.5   =   1;      
            turnedRight = 1;
         end
      end
   end //while
end //main