1. Welcome to our site! Electro Tech is an online community (with over 170,000 members) who enjoy talking about and building electronic circuits, projects and gadgets. To participate you need to register. Registration is free. Click here to register now.
    Dismiss Notice

help on code for micromouse robot with 3ultrasonic sensors

Discussion in 'Robotics & Mechatronics' started by behnam23, Jul 18, 2014.

  1. behnam23

    behnam23 New Member

    Joined:
    Jul 18, 2014
    Messages:
    1
    Likes:
    0
    hi , i'm trying to build a micromouse robot with 3 ultrasonic sensors , my hardware part of the job is complete but i found some problems on its code because of my weak c language ,
    the problem is for exampe if i write that if(right sensor > 25cm) for robot to turn right and enters the path while turning the distance's are faulty so it breaks from the if that i wrote , i postet the code that i wrote till now i'd be happy if someone please give me some hints on how to do write a good code for this robot.





    Code (text):
    /*****************************************************
    This program was produced by the
    CodeWizardAVR V2.05.3 Standard
    Automatic Program Generator
    © Copyright 1998-2011 Pavel Haiduc, HP InfoTech s.r.l.
    http://www.hpinfotech.com

    Project :
    Version :
    Date    : 5/19/2014
    Author  : IBM-Lenovo
    Company :
    Comments:


    Chip type               : ATmega16A
    Program type            : Application
    AVR Core Clock frequency: 16.000000 MHz
    Memory model            : Small
    External RAM size       : 0
    Data Stack size         : 256
    *****************************************************/

    #include <mega16a.h>
    #include <alcd.h>
    #include <delay.h>
    #include <stdio.h>
    #include <math.h>


    #define trig_f PORTD.0
    #define echo_f PIND.1
    #define echo_l PIND.2
    #define trig_l PORTD.3
    #define trig_r PORTC.0
    #define echo_r PINC.1
    #define movf_r PORTB.1
    #define movb_r PORTB.4
    #define movf_l PORTB.5
    #define movb_l PORTB.0
    #define pwm_r OCR1A
    #define pwm_l OCR2
    int i=0,a=0,sum = 0,x=0,y=0 ;
    unsigned char lcd_buff[16];
    unsigned int d_f,d_l,d_r;
    unsigned long int timer_v;
    float timer_dis,distance,u;
    void turn(void);


    unsigned int check(void)
    {
           
           timer_v = TCNT0;
           timer_dis=timer_v*0.000064;
           distance=timer_dis*343.0;
           u=(float)distance*50;
           d_f=u;

         
         
         
         
         
          lcd_gotoxy(0,0);
          lcd_putsf("distance");
          lcd_gotoxy(0,1);
          sprintf(lcd_buff,"%u cm ",d_f);
          lcd_puts(lcd_buff);
          return d_f;
           

       

    }

    unsigned int check2(void)
    {

           timer_v = TCNT0;
           timer_dis=timer_v*0.000064;
           distance=timer_dis*343.0;
           u=(float)distance*50;
           d_r=u;

         
         
         
         
         

          lcd_gotoxy(6,1);
          sprintf(lcd_buff,"%u cm ",d_r);
          lcd_puts(lcd_buff);
          return d_r;
           

       

    }

    unsigned int check3(void)
    {

           timer_v = TCNT0;
           timer_dis=timer_v*0.000064;
           distance=timer_dis*343.0;
           u=(float)distance*50;
           d_l=u;

         
         
         
         
         

          lcd_gotoxy(12,1);
          sprintf(lcd_buff,"%u cm ",d_l);
          lcd_puts(lcd_buff);
          return d_l;
           

       

    }


    void calib(void)
    {
    if(d_r < 12)
          {
          pwm_r = 150;
          pwm_l = 80;
          movf_r = 1;
          movb_r = 0;
          movf_l = 1;
          movb_l = 0;
         
          }
          if(d_r < 10 )
          {
          pwm_r = 90;
          pwm_l = 0;
          movf_r = 1;
          movb_r = 0;
          movf_l = 0;
          movb_l = 0;      
         
          }
          if(d_l < 12)
          {
         
          pwm_r = 80;
          pwm_l = 150;
          movf_r = 1;
          movb_r = 0;
          movf_l = 1;
          movb_l = 0;
         
          }
         
          if(d_l < 10 )
          {
          pwm_l = 90;
          pwm_r = 0;
          movf_l = 1;
          movb_r = 0;
          movf_r = 0;
          movb_l = 0;
         
          }  
         
    }

    void move_f()
    {

    pwm_l = 100;
    pwm_r = 100;
    movb_r = 0;
    movf_l = 1;
    movf_r = 1;
    movb_l = 0;


    }

    void main(void)
    {
    // Declare your local variables here

    // Input/Output Ports initialization
    // Port A initialization
    // Func7=Out Func6=Out Func5=Out Func4=Out Func3=Out Func2=Out Func1=Out Func0=Out
    // State7=0 State6=0 State5=0 State4=0 State3=0 State2=0 State1=0 State0=0
    PORTA=0x00;
    DDRA=0xFF;

    // Port B initialization
    // Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
    // State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
    PORTB=0x80;
    DDRB=0x37;

    // Port C initialization
    // Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
    // State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
    PORTC=0x02;
    DDRC=0x01;

    // Port D initialization
    // Func7=Out Func6=In Func5=Out Func4=In Func3=Out Func2=In Func1=In Func0=Out
    // State7=0 State6=T State5=0 State4=T State3=0 State2=P State1=P State0=0
    PORTD=0x06;
    DDRD=0xA9;

    // Timer/Counter 0 initialization
    // Clock source: System Clock
    // Clock value: Timer 0 Stopped
    // Mode: Normal top=0xFF
    // OC0 output: Disconnected
    TCCR0=0x00;
    TCNT0=0x00;
    OCR0=0x00;

    // Timer/Counter 1 initialization
    // Clock source: System Clock
    // Clock value: 15.625 kHz
    // Mode: Fast PWM top=0x00FF
    // OC1A output: Non-Inv.
    // OC1B output: Discon.
    // Noise Canceler: Off
    // Input Capture on Falling Edge
    // Timer1 Overflow Interrupt: Off
    // Input Capture Interrupt: Off
    // Compare A Match Interrupt: Off
    // Compare B Match Interrupt: Off
    TCCR1A=0x81;
    TCCR1B=0x0D;
    TCNT1H=0x00;
    TCNT1L=0x00;
    ICR1H=0x00;
    ICR1L=0x00;
    OCR1AH=0x00;
    OCR1AL=0x00;
    OCR1BH=0x00;
    OCR1BL=0x00;

    // Timer/Counter 2 initialization
    // Clock source: System Clock
    // Clock value: 15.625 kHz
    // Mode: Fast PWM top=0xFF
    // OC2 output: Non-Inverted PWM
    ASSR=0x00;
    TCCR2=0x6F;
    TCNT2=0x00;
    OCR2=0x00;

    // External Interrupt(s) initialization
    // INT0: Off
    // INT1: Off
    // INT2: Off
    MCUCR=0x00;
    MCUCSR=0x00;

    // Timer(s)/Counter(s) Interrupt(s) initialization
    TIMSK=0x00;


    // USART initialization
    // USART disabled
    UCSRB=0x00;

    // Analog Comparator initialization
    // Analog Comparator: Off
    // Analog Comparator Input Capture by Timer/Counter 1: Off
    ACSR=0x80;
    SFIOR=0x00;

    // ADC initialization
    // ADC disabled
    ADCSRA=0x00;

    // SPI initialization
    // SPI disabled
    SPCR=0x00;

    // TWI initialization
    // TWI disabled
    TWCR=0x00;

    // Alphanumeric LCD initialization
    // Connections are specified in the
    // Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu:
    // RS - PORTA Bit 0
    // RD - PORTA Bit 1
    // EN - PORTA Bit 2
    // D4 - PORTA Bit 4
    // D5 - PORTA Bit 5
    // D6 - PORTA Bit 6
    // D7 - PORTA Bit 7
    // Characters/line: 16
    lcd_init(16);



    while (1)
          {
         
         
         
          if(PINB.7 == 1)
          {
         
          lcd_gotoxy(0,0);
          lcd_putsf("Press The Start");
          lcd_gotoxy(5,1);
          lcd_putsf("Button");
          delay_ms(1000);
          lcd_clear();

          }
           
         

     if(PINB.7 == 0)
     {

         
         
          TCCR0 = 0x00;
          TIMSK = 0x00;
          TCNT0 = 0x00;
          trig_f = 1;
          delay_us(20);
          trig_f = 0;
          while(echo_f == 0);
          TCCR0=0x05;
          while(echo_f == 1);
          TCCR0 = 0x00;
          check();
         
          TIMSK = 0x00;
          TCNT0 = 0x00;
          trig_r = 1;
          delay_us(20);
          trig_r = 0;
          while(echo_r == 0);
          TCCR0 = 0x05;
          while(echo_r == 1);
          TCCR0 = 0x00;
          check2();
         
          TIMSK = 0x00;
          TCNT0 = 0x00;
          trig_l = 1;
          delay_us(20);
          trig_l = 0;
          while(echo_l == 0);
          TCCR0 = 0x05;
          while(echo_l == 1);
          TCCR0 = 0x00;
          check3();
          delay_ms(200);
         
          sum = (d_l + d_r)/2 ;
          x= (d_l * d_l)+(d_f * d_f);
          x = sqrt(x);
          y = (d_r * d_r)+(d_f * d_f);
          y = sqrt(y);
         
         
         
          if(d_f>30)
          {
          if(d_r<10) i = 0;
          if(d_l<10) i = 1;
          }
         
         
          if ( d_f >= 25)
          {
          delay_ms(200);
          if(d_f >= 25)
          {

         
          move_f();
         
         
            if( d_r > sum+2)
                {
               
                pwm_r = 80;
                pwm_l = 140;
                movb_r = 0;
                movf_l = 1;
                movf_r = 1;
                movb_l = 0;
               
                }
                if(d_l > sum+2)
                {
                pwm_r = 140;
                pwm_l = 80;
                movb_r = 0;
                movf_l = 1;
                movf_r = 1;
                movb_l = 0;
               
                }
               
                if(d_r-6 > 2*d_l)
                {
                pwm_r = 120;
                pwm_l = 190;
                movb_r = 1;
                movf_l = 1;
                movf_r = 0;
                movb_l = 0;
                delay_ms(600);
                move_f();
                delay_ms(800);
               
                }
               

                 
         
         
          }
          }          
         
         
         
             

           
         


         
          if( d_f < 25)
          {
          delay_ms(200);
          if( d_f < 25)
          {
         

         
    //       if( d_r > sum+2)
    //            {
    //            
    //            pwm_r = 80;
    //            pwm_l = 140;
    //            movb_r = 0;
    //            movf_l = 1;
    //            movf_r = 1;
    //            movb_l = 0;
    //            
    //            }
    //            if(d_l > sum+2)
    //            {
    //            pwm_r = 140;
    //            pwm_l = 80;
    //            movb_r = 0;
    //            movf_l = 1;
    //            movf_r = 1;
    //            movb_l = 0;
    //            
    //            }
    //      
    //      
    //                  if(d_r<8||d_l<8)
    //      {
    //      if(d_l<8)
    //      {
    //      pwm_l = 100;
    //      pwm_r = 0;
    //      movb_r = 0;
    //      movf_l = 1;
    //      movf_r = 0;
    //      movb_l = 0;
    //      }
    //      if(d_r<8)
    //      {
    //      pwm_l = 0;
    //      pwm_r = 100;
    //      movb_r = 0;
    //      movf_l = 0;
    //      movf_r = 1;
    //      movb_l = 0;
    //      }
    //      
    //      
    //      }

         
         
          switch (i)
          {
         
          case 0 :
          if(d_l - d_r >= 2 || d_r - d_l >= 2)
          {
         
          pwm_l = 160;
          pwm_r = 180;
          movb_r = 0;
          movf_l = 0;
          movf_r = 1;
          movb_l = 1;
          };
          break;
          case 1 :
          if(d_l - d_r >= 2 || d_r - d_l >= 2)
          {
         
          pwm_l = 180;
          pwm_r = 160;
          movb_r = 1;
          movf_l = 1;
          movf_r = 0;
          movb_l = 0;
          };
          break;
          }
          }
          }
         

         
         
         
         
    }

         
         

           
         

          }
         
         
         
         

    }
    [​IMG]

    [​IMG]
    [​IMG][/code]
     

Share This Page