/*##### Motor Shield (L298N) #####*/
/* To import NEWPING library go to - https://bitbucket.org/teckel12/arduino-new-ping/downloads/NewPing_v1.8.zip */
const int tDelay = 370; // without tires was 370 with tires:not turning good, do not use.
// 896 after checks, could be 897/895/ more or less
// constanst to turn 90 degrees with delay
const int ENA = 5; // right side
const int ENB = 6; // left side
/*
* IN1: HIGH; IN2: LOW --> Direction 1
* IN1: LOW; IN2: HIGH --> Direction 2
* IN3: HIGH; IN4: LOW --> Direction 1
* IN3: LOW; IN4: HIGH --> Direction2
*/
const int IN1 = 11; //RIGHT SIDE
const int IN2 = 10; //RIGHT SIDE
const int IN3 = 9; //LEFT SIDE
const int IN4 = 8; //LEFT SIDE
/*const int leftTrigger=0;
const int leftEcho=1;
//const int forTrigger =13;
//const int forEcho=12;
const int rightTrigger = 4;
const int rightEcho =3;
const int rightBottomTrigger = 7;
const int rightBottomEcho = 2;
*/
#include <NewPing.h>
#define rightTrigger 14
#define rightEcho 15
#define leftTrigger 19
#define leftEcho 18 // **************************** DEFINE replaces the "const int"
#define forTrigger 13
#define forEcho 12
#define MAX_DISTANCE 200
NewPing forSonar( forTrigger, forEcho, MAX_DISTANCE);
NewPing rightSonar( rightTrigger , rightEcho , MAX_DISTANCE);
NewPing leftSonar( leftTrigger , leftEcho, MAX_DISTANCE);
int counter = 0;
void setup()
{
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(forTrigger, OUTPUT);
pinMode(forEcho, INPUT);
pinMode(rightTrigger, OUTPUT);
pinMode(rightEcho, INPUT);
pinMode(leftTrigger, OUTPUT);
pinMode(leftEcho, INPUT);
// Enable Motor A, Motor B: Constant Speed
analogWrite(ENA, 60); // change back to 80
analogWrite(ENB, 60); // change back to 80
// Serial communication
Serial.begin(9600);
}
/* if(rightOpen())
turnRight()
elseif(frontOpen())
goForward()
else turnLeft()
*/
void loop() // *********** TODO -- PERFECT turn right, => right now it is too much - decrease angle, decrease curve
{
// testTurn();
//turnTillWall();
inspiration();
}
void uTurn()
{
analogWrite(ENA, 100);
analogWrite(ENB, 100);
digitalWrite(IN1,HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4,LOW);
delay(650);
}
void testTurn()
{
for(int i = 0; i < 4 ; i++)
{
turnRight(tDelay);
stopDrive1(500);
}
stopDrive1(10000);
}
void stickToRight()
{
if(getSonarDistance(rightSonar) - getSonarDistance(leftSonar) >= 3)
{
turnRight1(10);
}
if(getSonarDistance(leftSonar) - getSonarDistance(rightSonar) >= 3)
{
turnLeft1(10);
}
else
{
driveForward1(10);
}
}
long microsecondsToCentimeters(long microseconds) //********************************************************* Function : from microSec to CM
{
return microseconds / 29 / 2;
}
// digitalWrietTry
void driveForward1(int milliseconds) //********************************************************* Function : drive forward
{
analogWrite(ENA, 60);
analogWrite(ENB, 60);
digitalWrite(IN1,HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4,HIGH);
if (milliseconds > 0)
delay(milliseconds);
}
void driveBackward1(int milliseconds) //********************************************************* Function : drive backward
{
analogWrite(ENA, 60);
analogWrite(ENB, 60);
digitalWrite(IN1,LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4,LOW);
if(milliseconds > 0)
delay(milliseconds);
}
void stopDrive1(int milliseconds) //********************************************************* Function : stop
{
digitalWrite(IN1,LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4,LOW);
if(milliseconds > 0)
delay(milliseconds);
}
void turnLeft1(int milliSeconds) //********************************************************* Function : turn left
{
analogWrite(ENA, 155);
analogWrite(ENB, 0);
digitalWrite(IN1,HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4,HIGH);
if(milliSeconds > 0)
delay(milliSeconds);
}
void turnRight(int milliSeconds)
{
analogWrite(ENA, 0); //was 0
analogWrite(ENB, 155); //was 155
digitalWrite(IN1,HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4,HIGH);
if(milliSeconds > 0)
delay(milliSeconds);
}
void turnRight1(int milliSeconds) //********************************************************* Function : turn right
{
analogWrite(ENA, 80); // change back to 150
analogWrite(ENB, 80);
digitalWrite(IN1,LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4,HIGH);
if(milliSeconds > 0)
delay(milliSeconds);
}
int getSonarDistance(NewPing sonar) //********************************************************* Function : recieves a specific Sonar and returns distance
{
delay(100);
unsigned int distance = sonar.ping_cm();
//Serial.print(distance);
//Serial.println("cm");
if(distance !=0)
return distance;
else
return 1000; // Comes in place of the "0" so it wont disturb the loop that check "smaller than..."
}
void funRun() //****************************************** Function : just avoiding objcects and turns left
{
while(getSonarDistance(forSonar) > 30 || getSonarDistance(forSonar) == 0 )
{
driveForward1(50);
}
while(getSonarDistance(forSonar) < 30)
{
driveBackward1(70);
delay(50);
turnLeft1(800);
}
}
void funRun2() // ********************* Function: same as funRun , just if turned left 4 times, it turns Right. PROBLEM not always return to its initial spot- in maze it will be ok
{
while(getSonarDistance(forSonar) > 30 || getSonarDistance(forSonar) == 0 )
{
driveForward1(50);
}
while(getSonarDistance(forSonar) < 30)
{
if(counter==4)
{
driveBackward1(70);
delay(50);
turnRight1(800);
counter = 0;
}
else
{
driveBackward1(70);
delay(50);
turnLeft1(800);
counter++;
}
}
}
void checkSpanish()
{
if(getSonarDistance(forSonar) >8)
{
if(getSonarDistance(rightSonar) > 7 && getSonarDistance(rightSonar) <13 )
{
analogWrite(ENA , 60); // 120
analogWrite(ENB , 90); // 150
driveForward1(50);
}
if(getSonarDistance(rightSonar) >= 13)
{
analogWrite(ENA , 150); //255
analogWrite(ENB , 10); // 60
driveForward1(100);
}
if(getSonarDistance(rightSonar) <= 7 )
{
analogWrite(ENA , 10); //60
analogWrite(ENB , 150); //255
driveForward1(100);
}
}
if( getSonarDistance(rightSonar) <=20 && getSonarDistance(rightSonar) > 20 && getSonarDistance(forSonar) <=8)
turnRight1(700);
if(getSonarDistance(rightSonar) > 20 && getSonarDistance(rightSonar) > 20 && getSonarDistance(forSonar) <=8)
turnRight1(700);
if(getSonarDistance(rightSonar) <=20 && getSonarDistance(rightSonar) > 20 && getSonarDistance(forSonar) <=8)
turnLeft1(800);
if( getSonarDistance(rightSonar) <=20 && getSonarDistance(rightSonar) <= 20 && getSonarDistance(forSonar) <=8)
turnRight1(1200); // rotate 180
}
void inspiration()
{
if(getSonarDistance(rightSonar) >25)
{
turnRight(tDelay); // 90 degrees
}
else if(getSonarDistance(forSonar) > 20)
{
driveForward1(20); // previous version 100
}
else if(getSonarDistance(leftSonar) >30)
{
turnLeft1(tDelay);
stopDrive1(500);// 90 degrees
}
else
{
uTurn(); //tDelay*2
}
}
void turnTillWall()
{
if(getSonarDistance(rightSonar) > 6 )
{
while(getSonarDistance(rightSonar) > 6 )
{
turnRight(105); // 8 times = full turn (90 degrees)
}
}
else if(getSonarDistance(forSonar) > 20)
{
driveForward1(50); // previous version 100
}
else if(getSonarDistance(leftSonar) >20)
{
turnLeft1(tDelay);
stopDrive1(500);// 90 degrees
}
else
{
turnRight(1350); //tDelay*2
}
}
/* void correctParallel()
{
for(int i = 0 ; i < 5; i++)
{
if(rightBottomSonar - toprigthSonar == i)
turnLeft(i*100); // just a measure to turn need to check this
else if(topright - rightBottom == i)
turnRight(i*100); // same
}
}
*/
Comments