field robot C program using wiringPi lib

FieldRobot_withRaspberryPi.c

/*
Project : Field Robot
task: counting plants with laser sensor and auto nevigation 
Group: FR6
prepared by : AKBAR MOSHUR ALAM

*/

#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>
#include <wiringSerial.h>
#include <softPwm.h>
#include <termios.h>
 
#define motor_d_1	 		27
#define motor_d_2		 	28
#define motor_n_1	 		23
#define motor_n_2	 		24

#define ultra_left_trig    	 	 4
#define ultra_left_echo		 	 5
#define ultra_right_trig	 	 2
#define ultra_right_echo	 	 3
#define laser_left			 21
#define laser_right			 22

void setup()
{
	softPwmCreate(motor_d_1,0,100);
	softPwmCreate(motor_d_2,0,100);
	softPwmCreate(motor_n_1,0,100);
	softPwmCreate(motor_n_2,0,100);

	pinMode(ultra_left_trig, OUTPUT);
	pinMode(ultra_left_echo, INPUT);
	pullUpDnControl(ultra_left_echo, PUD_UP);

	pinMode(ultra_right_trig, OUTPUT);
	pinMode(ultra_right_echo, INPUT);
	pullUpDnControl(ultra_right_echo, PUD_UP);

	pinMode(laser_left, INPUT);
	pullUpDnControl(laser_left, PUD_UP);

	pinMode(laser_right, INPUT);
	pullUpDnControl(laser_right, PUD_UP);


}

int check_left()
{
	
	long startTime;
	long totalTime;
	long D;
    
	
    	digitalWrite(ultra_left_trig, HIGH);
    	delay(10);      
    	digitalWrite(ultra_left_trig, LOW);
			while(digitalRead(ultra_left_echo)==0)
		 		startTime = micros();
       			while(digitalRead(ultra_left_echo)==1)
         		totalTime = micros()-startTime;
        
        		 D = totalTime / 58;						// distance calculation in cm
         
         return D;

}

int check_right()
{
	long startTime_r;
	long totalTime_r;
	long D_r;
  
	
    	digitalWrite(ultra_right_trig, HIGH);
    	delay(10);      
    	digitalWrite(ultra_right_trig, LOW);
			while(digitalRead(ultra_right_echo)==0)
		 		startTime_r = micros();
       			while(digitalRead(ultra_right_echo)==1)
         		totalTime_r = micros()-startTime_r;
        
         		D_r = totalTime_r / 58; 					// distance calculation in cm
        return D_r;
}

void move_forward()
{
	softPwmWrite(motor_d_1,100);
	softPwmWrite(motor_d_2,0);
}
void move_back()
{
	softPwmWrite(motor_d_1,0);
	softPwmWrite(motor_d_2,100);
}
void stop()
{
	softPwmWrite(motor_d_1,0);
	softPwmWrite(motor_d_2,0);
}
void turn_left()
{
	stop();
	softPwmWrite(motor_n_1,100);
	softPwmWrite(motor_n_2,0);
	delay(100);
	move_forward();
	delay(100);
	stop();
	softPwmWrite(motor_n_1,100);
	softPwmWrite(motor_n_2,0);

}
void turn_right()
{
	stop();
	softPwmWrite(motor_n_1,0);
	softPwmWrite(motor_n_2,100);
	delay(100);
	move_forward();
	delay(100);
	stop();
	softPwmWrite(motor_n_1,0);
	softPwmWrite(motor_n_2,100);



}
void move()
{
	move_forward();
	delay(12000);
	turn_left();
	move_forward();
	delay(12000);
	turn_right();
	move_forward();
	while(1)
	{
		check_left();
		check_right();
// if the is any obstracle from the left side in 15cm distance,the robot will move right
		if(check_left()<15) 
		{
			softPwmWrite(motor_d_1,100);
			softPwmWrite(motor_d_2,0);
			softPwmWrite(motor_n_1,0);
			softPwmWrite(motor_n_2,30);

		}

// if the is any obstracle from the right side in 15cm distance,the robot will move left
		if(check_right()<15)
		{
			softPwmWrite(motor_d_1,100);
			softPwmWrite(motor_d_2,0);
			softPwmWrite(motor_n_1,30);
			softPwmWrite(motor_n_2,0);

		}

// if the is any obstracle from the left side in 15cm distance and also from right,the robot will  stop
		if(check_left()<15 && check_right()<15)
		{
			softPwmWrite(motor_d_1,0);
			softPwmWrite(motor_d_2,0);
			softPwmWrite(motor_n_1,0);
			softPwmWrite(motor_n_2,0);

		}
	}
}

void count()
{	
	long num;
	long sum=0;
	int total=0;
	long num_r;
	long sum_r=0;	
	
	if(digitalRead(laser_left)!=LOW)
	{
		
		num=1;
		sum=sum+num;
		total=sum+sum_r;
		
		
			printf(" total amount is %d",total);
		
		
	}
	delay(120000);
	if(digitalRead(laser_right)!=LOW)
	{
		
		num_r=1;
		sum_r=sum_r+num_r;
		total=sum+sum_r;
		
		
		
			printf(" total amount is %d",total);

		
	}

}



int main()

{
	setup();
	int fd;
	fd=serialOpen("/dev/ttyBLU",9600);
	setup();
	while(1)
	{
		if (serialDataAvail(fd) == 1)
{
	move();
	count();
}
		if (serialDataAvail(fd) == 0)
{
	stop();
}
}
	return 0 ;
}