Software > Software

help on code for micromouse robot with 3ultasonic sensors

(1/1)

behnam23:
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;
      }
      }
      }
     

     
     
     
     
}
 
     
     

       
     

      }
     
     
     
     

}
--- End code ---




[/code]

Navigation

[0] Message Index

Go to full version