the robot needs first follow a black line, then enter a maze, then hit a patch of the black tape and go back to follow a black line but i never done any coding projekt this big before so I copy paste a lot from what i found. i have to use the motor shield and the adafruit V1 Libary. I cannot use New Ping since the V1 libary also uses the Timer 2 from what i could read. The line follow works well enough for my purpose. To make it more mangeable i only do the maze mode for now. My problem is that the robot is really allover the place, sometimes it gets stuck for no reason and sometimes i just spinns in cirles and the most frustation thing is that i cannot reproduce the issues. Maybe somebody has any ideas what i can do ?
#include <AFMotor.h>
#define trigPin1 10 //Front
#define echoPin1 7
#define trigPin2 12 //Right
#define echoPin2 13
#define trigPin3 8 //Left
#define echoPin3 9
AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(2, MOTOR12_1KHZ);
//AF_DCMotor motor3(3, MOTOR12_1KHZ);
//AF_DCMotor motor4(4, MOTOR12_1KHZ);
void setup() {
//declaring pin types
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
delay(250);
//begin serial communication
Serial.begin(9600);
}
//-----------------------------------------------------------------------------------------------------------------Move command
void forward() {
motor1.run(FORWARD);
motor1.setSpeed(35);
motor2.run(FORWARD);
motor2.setSpeed(35);
}
void veryfast() {
motor1.run(FORWARD);
motor1.setSpeed(255);
motor2.run(FORWARD);
motor2.setSpeed(255);
}
void turn_left() {
motor1.run(BACKWARD);
motor1.setSpeed(40);
motor2.run(FORWARD);
motor2.setSpeed(40);
}
void turn_right() {
motor1.run(FORWARD);
motor1.setSpeed(40);
motor2.run(BACKWARD);
motor2.setSpeed(40);
}
void reverse() {
motor1.run(BACKWARD);
motor1.setSpeed(40);
motor2.run(BACKWARD);
motor2.setSpeed(40);
}
void stop() {
motor1.run(RELEASE);
motor1.setSpeed(0);
motor2.run(RELEASE);
motor2.setSpeed(0);
}
void loop() {
//------------------------------------------------------------------------------------------------------------------------------
long durfront, durright, durleft, FrontSensor, RightSensor, LeftSensor;
digitalWrite(trigPin1, LOW);
delayMicroseconds(2); // delays are required for a succesful sensor operation.
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
durfront = pulseIn(echoPin1, HIGH);
FrontSensor = durfront * 0.034 / 2; // convert the distance to centimeters.
digitalWrite(trigPin2, LOW);
delayMicroseconds(2); // delays are required for a succesful sensor operation.
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
durright = pulseIn(echoPin2, HIGH);
RightSensor = durright * 0.034 / 2; // convert the distance to centimeters.
digitalWrite(trigPin3, LOW);
delayMicroseconds(2); // delays are required for a succesful sensor operation.
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
delayMicroseconds(2);
durleft = pulseIn(echoPin3, HIGH);
LeftSensor = durleft * 0.034 / 2; // convert the distance to centimeters.}
Serial.print("Front = ");
Serial.println(FrontSensor);
Serial.print("Right = ");
Serial.println(RightSensor);
Serial.print("Left = ");
Serial.println(LeftSensor);
Serial.println();
//delay(500);
//------------------------------------------------------------------------------------------------ Maze:
/*if (FrontSensor > 10)
{
if(LeftSensor > 7 && LeftSensor < 11)
{
forward();
}
if(LeftSensor >=11)
{
turn_left();
}
if(LeftSensor <=6)
{
turn_right();
}
}*/
if (FrontSensor <= 6 && RightSensor <= 7 && LeftSensor <= 7)
{
reverse();
delay(70);
turn_right();
delay(300);
}
//------------------------------------------------------------------------------------ obstacle on right and front sides
else if (FrontSensor <= 5 && RightSensor <= 6 && LeftSensor >= 6)
{
turn_left();
//delay(2);
// turn left side
}
//------------------------------------------------------------------------------------ obstacle on left and front sides
else if (FrontSensor <= 5 && RightSensor >= 6 && LeftSensor <= 6)
{
turn_right();
//delay(2);
// turn right side
}
//----------------------------------------------------------------------------------- right > left
else if (FrontSensor <= 6 && RightSensor >= LeftSensor)
{
turn_right();
delay(2);
}
//----------------------------------------------------------------------------------- right < left
else if (FrontSensor <= 6 && RightSensor <= LeftSensor)
{
turn_left();
delay(2);
}
//----------------------------------------------------------------------------------- right 1 sensor
/*else if (FrontSensor >= 6 && RightSensor <= 7 && LeftSensor >= 7)
{
turn_right();
//delay(30);
}
//------------------------------------------------------------------------------------ left 1 sensor
else if (FrontSensor >= 6 && RightSensor >= 7 && LeftSensor <= 7)
{
turn_left();
//delay(30);
}*/
else
{
forward();
}
}
