Hello everyone,
I'm trying to make integration to my project with the following components:
*SR-04 X4 to detect distance
*Tsop receiverX3
*Dc motor driver X4
When i read the IR signal with my receiver with this program I see that the receiver receives the signal from the transmitter successfully using this program:
int frontReceiver = 15; //Read from the front receiver
int rightReceiver=16;
int leftReceiver=14;
#define LED_FRONT_GREEN 52
#define LED_RIGHT_RED 53
#define LED_LEFT_YELLOW 51
void setup()
{
Serial.begin(9600);
pinMode(LED_FRONT_GREEN,OUTPUT); //LED FRONT
pinMode(LED_RIGHT_RED,OUTPUT); //LED RIGHT
pinMode(LED_LEFT_YELLOW,OUTPUT); //LED LEFT
pinMode(leftReceiver,INPUT);
pinMode(frontReceiver, INPUT);
pinMode(rightReceiver,INPUT);
}
void loop() {
int ReceiverF = digitalRead(frontReceiver);
if(ReceiverF==0)
{
digitalWrite(LED_FRONT_GREEN,HIGH);
delay(100);
digitalWrite(LED_FRONT_GREEN,LOW);
delay(100);
}
int ReceiverR=digitalRead(rightReceiver);
if(ReceiverR==0)
{
digitalWrite(LED_RIGHT_RED,HIGH);
delay(100);
digitalWrite(LED_RIGHT_RED,LOW);
delay(100);
}
int ReceiverL=digitalRead(leftReceiver);
if(ReceiverL==0)
{
digitalWrite(LED_LEFT_YELLOW,HIGH);
delay(100);
digitalWrite(LED_LEFT_YELLOW,LOW);
delay(100);
}
}
But when i am trying to upload the program that includes all the components the reading from the receivers is really bad.
I tried to decrease the delay in my program but it didn't help.
Here is my program that make the reading from the receivers bad.
#include <Servo.h>
#include<NewPing.h>
Servo myservo;
#include <AFMotor.h>
#define MAX_SPEED 255
#define SLOW_SPEED 255
#define MAX_DISTANCE 400
#define echoPinLeft 30 //LEFT ULTRASONIC
#define trigPinLeft 31 //LEFT ULTRASONIC
#define echoPinFRONT 47 //FRONT ULTRASONIC
#define trigPinFRONT 46 //FRONT ULTRASONIC
#define echoPinRIGHT 23 //RIGHT ULTRASONIC
#define trigPinRIGHT 22 //RIGHT ULTRASONIC
#define LED_FRONT_GREEN 52
#define LED_RIGHT_RED 53
#define LED_LEFT_YELLOW 51
AF_DCMotor Motor1(1,MOTOR12_64KHZ);
AF_DCMotor Motor2(2,MOTOR12_64KHZ);
AF_DCMotor Motor3(3,MOTOR12_64KHZ);
AF_DCMotor Motor4(4,MOTOR12_64KHZ);
NewPing sonarLEFT(trigPinLeft, echoPinLeft, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
NewPing sonarFront(trigPinFRONT, echoPinFRONT, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
NewPing sonarRIGHT(trigPinRIGHT, echoPinRIGHT, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
int frontReceiver = 15; //Read from the front receiver
int rightReceiver=16;
int leftReceiver=14;
int flag=0;
void drive_forward();
void stop_driving();
void drive_fast();
void drive_right();
void drive_left();
void drive_back();
void rotateRight90();
void lookLeftservo();
void lookRightServo();
void turnOnLeftLed();
int ReadDistanceFront();
int ReadDistanceLeft();
int ReadDistanceRight();
void avoid_obstacle();
void turnORightLed();
void turnOnLedFront();
void IR_signal();
void setup()
{
Serial.begin(9600);
pinMode(LED_FRONT_GREEN,OUTPUT); //LED FRONT
pinMode(LED_RIGHT_RED,OUTPUT); //LED RIGHT
pinMode(LED_LEFT_YELLOW,OUTPUT); //LED LEFT
pinMode(leftReceiver,INPUT);
pinMode(frontReceiver, INPUT);
pinMode(rightReceiver,INPUT);
pinMode(trigPinLeft,OUTPUT);
pinMode(echoPinLeft,INPUT);
pinMode(trigPinFRONT,OUTPUT);
pinMode(echoPinFRONT,INPUT);
pinMode(trigPinRIGHT,OUTPUT);
pinMode(echoPinRIGHT,INPUT);
//myservo.attach(22);
}
void loop()
{
IR_signal();
}
void IR_signal()
{
int ReceiverF = digitalRead(frontReceiver);
int ReceiverR=digitalRead(rightReceiver);
int ReceiverL=digitalRead(leftReceiver);
int distanceFront=ReadDistanceFront();
int distanceLeft= ReadDistanceLeft();
int distanceRight=ReadDistanceRight();
int counter=0;
if((distanceFront<70||distanceLeft<70||distanceRight<70) && (ReceiverF==LOW||ReceiverR==LOW ||ReceiverL==LOW) )
{
stop_driving();
Serial.println("at the destination");
}
else if (ReceiverF==LOW &&distanceFront>70) //No obstacle FRONT
{
turnOnLedFront();
drive_forward();
Serial.println("no obstacle, drive forward");
}
else if (ReceiverR==LOW && distanceRight>70) //NO obstacle Right
{
turnORightLed();
rotateRight90();
Serial.println("rotate right");
}
else if (ReceiverL==LOW &&distanceLeft>70) //NO obstacle LEFT
{
turnOnLeftLed();
rotateLeft90();
Serial.println("rotate left");
}
return;
}
void turnOnLeftLed()
{
digitalWrite(LED_LEFT_YELLOW,HIGH);
delay(10);
digitalWrite(LED_LEFT_YELLOW,LOW);
delay(10);
}
void turnORightLed()
{
digitalWrite(LED_RIGHT_RED,HIGH);
delay(10);
digitalWrite(LED_RIGHT_RED,LOW);
delay(10);
}
void turnOnLedFront()
{
digitalWrite(LED_FRONT_GREEN,HIGH);
delay(10);
digitalWrite(LED_FRONT_GREEN,LOW);
delay(10);
}
void drive_forward()
{
drive_fast();
Motor1.run(FORWARD);
Motor2.run(FORWARD);
Motor3.run(FORWARD);
Motor4.run(FORWARD);
}
void drive_fast()
{
Motor1.setSpeed(MAX_SPEED);
Motor2.setSpeed(MAX_SPEED);
Motor3.setSpeed(MAX_SPEED);
Motor4.setSpeed(MAX_SPEED);
}
void drive_slow()
{
Motor1.setSpeed(SLOW_SPEED);
Motor2.setSpeed(SLOW_SPEED);
Motor3.setSpeed(SLOW_SPEED);
Motor4.setSpeed(SLOW_SPEED);
return;
}
void stop_driving()
{
Motor1.run(RELEASE);
Motor2.run(RELEASE);
Motor3.run(RELEASE);
Motor4.run(RELEASE);
}
void drive_left()
{
drive_slow();
Motor1.run(FORWARD);
Motor2.run(BACKWARD);
Motor3.run(FORWARD);
Motor4.run(BACKWARD);
}
void rotateRight90()
{
drive_slow();
Motor1.run(BACKWARD);
Motor2.run(FORWARD);
Motor3.run(BACKWARD);
Motor4.run(FORWARD);
delay(500);
stop_driving();
}
void rotateLeft90()
{
drive_slow();
Motor1.run(FORWARD);
Motor2.run(BACKWARD);
Motor3.run(FORWARD);
Motor4.run(BACKWARD);
delay(500);
stop_driving();
}
void drive_right()
{
drive_slow();
Motor1.run(BACKWARD);
Motor2.run(FORWARD);
Motor3.run(BACKWARD);
Motor4.run(FORWARD);
return;
}
void drive_back()
{
drive_fast();
Motor1.run(BACKWARD);
Motor2.run(BACKWARD);
Motor3.run(BACKWARD);
Motor4.run(BACKWARD);
}
int ReadDistanceLeft()
{
delay(30);
int sum=0;
int distance=0;
for (int i=0;i<10;i++)
{
distance=sonarLEFT.ping_cm();
sum=sum+distance;
}
distance=sum/10;
if (distance==0)
distance=5000;
return distance;
}
int ReadDistanceFront()
{
delay(30);
int distance=0;
int sum=0;
for (int i=0;i<10;i++)
{
distance=sonarFront.ping_cm();
sum=sum+distance;
}
distance=sum/10;
if (distance==0)
distance=5000;
return distance;
}
int ReadDistanceRight()
{
delay(30);
int distance=sonarRIGHT.ping_cm();
int sum=0;
for (int i=0;i<10;i++)
{
sum=sum+distance;
}
distance=sum/10;
if (distance==0)
distance=5000;
return distance;
}
