Continue to Site

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.

  • 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.

help on code for micromouse robot with 3ultrasonic sensors

Status
Not open for further replies.

behnam23

New Member
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:
/*****************************************************
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;
      }
      } 
      }
     

     
     
     
     
}

     
     

       
     

      }
     
     
     
     

}




[/code]
 
Status
Not open for further replies.

Latest threads

New Articles From Microcontroller Tips

Back
Top