Created
August 22, 2016 13:05
-
-
Save AkbarAlam/a3382d22b1a6460dc4f1fe4cadca02cd to your computer and use it in GitHub Desktop.
field robot C program using wiringPi lib
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
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 ; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment