Hello . Since I started in robotics, I need some help with the code for my robot. I need to track it has passed the edge of the maze is painted black, he must navigate the white and does not go beyond the edge of the track. I use the robot with the collector 4 engines based on the Arduino Mega and 2 track sensor but something in my code does not work, he just goes forward and all. help me please.Code without errors but the robot goes forward only And sorry for my weird English.
#include <AFMotor.h> //Link library for working with shield
#define LEFT_SENSOR_PIN 33
#define RIGHT_SENSOR_PIN 35
#define STATE_FORWARD 0
#define STATE_RIGHT 1
#define STATE_LEFT 2
int state = STATE_FORWARD ;
// Plug motors to the terminal blocks M1, M2, M3, M4
AF_DCMotor motor1(1); //задний левый
AF_DCMotor motor2(2); //задний правый
AF_DCMotor motor3(3); //передний правый
AF_DCMotor motor4(4); //передний левый
void runForward()
{
state = STATE_FORWARD;
motor1.run(FORWARD); // Ask moving forward
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
motor1.setSpeed(150); // Set the speed
motor2.setSpeed(150);
motor3.setSpeed(150);
motor4.setSpeed(150);
}
void runRIGHT()
{
state = STATE_RIGHT;
motor1.run(FORWARD); // Ask moving forward
motor4.run(FORWARD);
motor1.setSpeed(150); // Set the speed
motor4.setSpeed(150);
}
void runLEFT()
{
state = STATE_LEFT;
motor3.run(FORWARD); // Ask moving forward
motor2.run(FORWARD);
motor3.setSpeed(150); // Set the speed
motor2.setSpeed(150);
}
void setup(){
// Set the maximum speed of the motor (similar work PWM)
motor1.setSpeed(255);
motor1.run(RELEASE);
motor2.setSpeed(255);
motor2.run(RELEASE);
motor3.setSpeed(255);
motor3.run(RELEASE);
motor4.setSpeed(255);
motor4.run(RELEASE);
}
void loop(){
boolean left = digitalRead(LEFT_SENSOR_PIN);
boolean right = digitalRead(RIGHT_SENSOR_PIN);
int targetState;
if (left == right) {
targetState = STATE_FORWARD;
} else if (left) {
targetState = STATE_RIGHT;
}
else
{
targetState = STATE_LEFT;
}
if (state == targetState) {
return;
}
switch (targetState) {
case STATE_FORWARD:
runForward();
break;
case STATE_RIGHT:
runRIGHT();
break;
case STATE_LEFT:
runLEFT();
break;
}
delay(50);
}
