Hello guys !
I am making robot that solves maze and after making it and coding I have a problem with matching code and physical robot. Can anybody help me with that problem ? I can put all my code and scheme here if you need. I think that someone with more experience than me can solve this problem with ease.
Thanks everybody in advance !
Us helping you without seeing your code and schematics would be like your robot solving the maze with no sensors and no motors.
Please read the forum guide in the sticky post at the top of most forum sections. This will tell you what you need to post and how to post it correctly.
1 Like
I'm sorry but I just want to open theme firstly and after that I wanted to hare code
Code:
int mode = 0;
# define STOPPED 0
# define PID_FORWARD 1
# define NO_LINE 2
# define HORIZONTAL_LINE 3
# define POS_LINE 4
# define RIGHT_TURN 5
# define LEFT_TURN 6
////////////////////////////Sensor Pins/////////////////////////
//Digital Pin
const int lineFollowsensor0 = 2; //left most/5
const int lineFollowsensor1 = 3;
const int lineFollowsensor2 = 4;
const int lineFollowsensor3 = 5;
const int lineFollowsensor4 = 12; //right most/5
//Analog Pin
const int farRightsensorPin = A0;
const int farLeftsensorPin = A5;
///////////////////////Initial Values of sensors////////////////
int sensor[5]={0, 0, 0, 0, 0},sensorL=0,sensorR=0;
int farRightsensor = 0;
int farLeftsensor = 0;
//////////////////////////////Motors Pin///////////////////////
//right wheel
int ENB = 10;
int motorInput3 = 7;
int motorInput4 = 6;
//left wheel
int motorInput1 = 9;
int motorInput2 = 8;
int ENA = 11;
//////////////////////////Motor speed variables/////////////////
int mspeed = 40; //PID_write_speed()
int rotspeed=60;
int rotspeedsharp=60;
float leftMotorSpeed=0;
float rightMotorSpeed=0;
int maxspeed =60; //calculatePID(), PID_write_speed()
/////////////////////////PID variables////////////////////////// calculatePID()
int Kp=12.5;
float Kd=30;
float Ki=0;
//initial values
int P=0, I=0,D=0, PIDvalue=0;
float previousError=0,previousI=0, error=0;
///////////////////////Variables to find SECOND PASS///////////////////
char path[100];
int pathLength=0;
int index=0;
int i,runner, countB;
int second_pass_steps=0;
int state=1; ///If state=1: loop will run, after the robot reaches the finish line, state will become 0 and escape the loop
void setup() {
pinMode(lineFollowsensor0, INPUT);
pinMode(lineFollowsensor1, INPUT);
pinMode(lineFollowsensor2, INPUT);
pinMode(lineFollowsensor3, INPUT);
pinMode(lineFollowsensor4, INPUT);
pinMode(farRightsensorPin, INPUT);
pinMode(farLeftsensorPin, INPUT);
pinMode(motorInput1, OUTPUT);
pinMode(motorInput2, OUTPUT);
pinMode(motorInput3, OUTPUT);
pinMode(motorInput4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
Serial.begin(9600); //setting serial monitor at rate of 9600
Serial.print("Started!!!");
delay(1500);
startEngine(); //low operating speed requires high speed kick start to escape inertia
}
///////////////////////////////////////////////////MAIN LOOP//////////////////////////////////////////
void loop()
{
Serial.println("First Pass !!");
first_pass(); //run the maze and return:shortest path, path[], length of shortest path
i=0;
stop_bot();
delay(5000);
/////////////PLACE THE ROBOT BACK TO START POINT////////////////
Serial.println("Second Pass !!");
state=1;
startEngine();
second_pass();
stop_bot();delay(20000);
}
///////////////////////////////////////////////////FIRST PASS////////////////////////////////////////////////
void first_pass() {
while(state){ //while(1) = while(true)
errorRead();
//Serial.println(" ");
//Printsensor();
switch (mode)
{
case HORIZONTAL_LINE: //CONT_LINE
//Printsensor();Serial.println("HORIZEN");
forward_a_bit(); // go foward a bit
errorRead();
if (mode==HORIZONTAL_LINE){ //stop and waiting for second pass
stop_bot;
printPath();shortenPath();delay(1000);
state=0;
}
else
{
left90(); //left90 itself has a delay function of 500 millisecs
path[index]='L';index=index+1; pathLength=pathLength+1; //save left path to path[]
engine_start_mini(); // engine start mini version
}
previousError = error;
break;
case NO_LINE:
forward_a_bit(); // go foward a bit
left90();left90_2(); //left90 itself has a delay function of 500 millisecs
path[index]='B';index=index+1; pathLength=pathLength+1; //save back path to path[]
engine_start_mini(); // engine start mini version
previousError = 0;
break;
case RIGHT_TURN:
//Printsensor();Serial.println("RIGHT");
analogWrite(ENA,mspeed);analogWrite(ENB,mspeed);forward();delay(550); // go foward a bit
errorRead();
if (mode == NO_LINE){
//Printsensor();Serial.print(" ");Serial.println("mode:NO_LINE");
stop_bot();delay(200);
right90();
path[index]='R';index=index+1; pathLength=pathLength+1; //save right path to path[]
engine_start_mini(); // engine start mini version
}
else if (mode==PID_FORWARD)
{
path[index]='S';index=index+1; pathLength=pathLength+1; //save straight path to path[]
}
break;
case LEFT_TURN:
forward_a_bit(); // go foward a bit
left90();
path[index]='L';index=index+1; pathLength=pathLength+1; //save left path to path[]
engine_start_mini(); // engine start mini version
break;
case PID_FORWARD:
calculatePID();
PID_write_speed();
forward();
break;
}
}
}
/////////////////////////////////////READ SENSOR VALUES, RETURN ERROR AND TYPE OF LINE THE ROBOT CURRENTLY FACING/////////////////////////////
void errorRead(){
sensor[0] = !digitalRead(lineFollowsensor0);
sensor[1] = !digitalRead(lineFollowsensor1);
sensor[2] = !digitalRead(lineFollowsensor2);
sensor[3] = !digitalRead(lineFollowsensor3);
sensor[4] = !digitalRead(lineFollowsensor4);
sensorR = digitalRead(farRightsensorPin);
sensorL = digitalRead(farLeftsensorPin);
if((sensor[0]== 0 )&&(sensor[1]== 0 )&&(sensor[2]== 0 )&&(sensor[3]== 0 )&&(sensor[4]== 0 )) {mode = HORIZONTAL_LINE; error = 0;}
else if((sensor[0]== 1 )&&(sensor[1]== 1 )&&(sensor[2]== 1 )&&(sensor[3]== 1 )&&(sensor[4]== 1 )) {mode = NO_LINE; error = 0;}
else if((sensorR == 0)&&(sensor[0] ==1 )) {mode = RIGHT_TURN; error = 0;}
else if((sensorL == 0)&&(sensor[4] ==1 )) {mode = LEFT_TURN; error = 0;}
else if((sensor[0]== 1 )&&(sensor[1]== 1 )&&(sensor[2]== 1 )&&(sensor[3]== 1 )&&(sensor[4]== 0 )) {mode = PID_FORWARD; error = 4;}
else if((sensor[0]== 1 )&&(sensor[1]== 1 )&&(sensor[2]== 1 )&&(sensor[3]== 0 )&&(sensor[4]== 0 )) {mode = PID_FORWARD; error = 3;}
else if((sensor[0]== 1 )&&(sensor[1]== 1 )&&(sensor[2]== 1 )&&(sensor[3]== 0 )&&(sensor[4]== 1 )) {mode = PID_FORWARD; error = 2;}
else if((sensor[0]== 1 )&&(sensor[1]== 1 )&&(sensor[2]== 0 )&&(sensor[3]== 0 )&&(sensor[4]== 1 )) {mode = PID_FORWARD; error = 1;}
else if((sensor[0]== 1 )&&(sensor[1]== 1 )&&(sensor[2]== 0 )&&(sensor[3]== 1 )&&(sensor[4]== 1 )) {mode = PID_FORWARD; error = 0;}
else if((sensor[0]== 1 )&&(sensor[1]== 0 )&&(sensor[2]== 0 )&&(sensor[3]== 1 )&&(sensor[4]== 1 )) {mode = PID_FORWARD; error =-1;}
else if((sensor[0]== 1 )&&(sensor[1]== 0 )&&(sensor[2]== 1 )&&(sensor[3]== 1 )&&(sensor[4]== 1 )) {mode = PID_FORWARD; error = -2;}
else if((sensor[0]== 0 )&&(sensor[1]== 0 )&&(sensor[2]== 1 )&&(sensor[3]== 1 )&&(sensor[4]== 1 )) {mode = PID_FORWARD; error = -3;}
else if((sensor[0]== 0 )&&(sensor[1]== 1 )&&(sensor[2]== 1 )&&(sensor[3]== 1 )&&(sensor[4]== 1 )) {mode = PID_FORWARD; error = -4;}
}
/////////////////////////////////////////////////////////PID CONTROLS//////////////////////////////////////////////////////////////
void calculatePID()
{
P = error;
if (leftMotorSpeed!=maxspeed || rightMotorSpeed!=maxspeed) //anti-winding
{I = I + previousI;previousI=I;}
D = error-previousError;
PIDvalue = (Kp*P) + (Ki*I) + (Kd*D);
previousError = error;
}
void PID_write_speed()
{
leftMotorSpeed = mspeed + PIDvalue;
rightMotorSpeed = mspeed - PIDvalue;
leftMotorSpeed=constrain(round(leftMotorSpeed), 0, maxspeed);
rightMotorSpeed=constrain(round(rightMotorSpeed), 0, maxspeed);
analogWrite(ENA,leftMotorSpeed);analogWrite(ENB,rightMotorSpeed);
//Serial.print(0);Serial.print(",");Serial.print(80);Serial.print(",");
//Serial.println(leftMotorSpeed);
}
///////////////////////////////////////////////////BASIC MOVEMENT CONTROLS/////////////////////////////////////////////
void forward_a_bit(){
analogWrite(ENA,mspeed);analogWrite(ENB,mspeed);forward();delay(550);stop_bot();delay(200);
}
void startEngine(){
analogWrite(ENA,60);analogWrite(ENB,60);
forward();
delay(200);
analogWrite(ENA,0);analogWrite(ENB,0);
delay(50);
}
void engine_start_mini(){
analogWrite(ENA,60);analogWrite(ENB,60);forward();delay(100);
}
void forward()
{
Serial.println("forward");
digitalWrite(motorInput1, LOW);
digitalWrite(motorInput2, HIGH);
digitalWrite(motorInput3, LOW);
digitalWrite(motorInput4, HIGH);
}
void stop_bot()
{
Serial.println("stop_bot");
digitalWrite(motorInput1, LOW);
digitalWrite(motorInput2, LOW);
digitalWrite(motorInput3, LOW);
digitalWrite(motorInput4, LOW);
}
void left90()
{
Serial.println("left90");
analogWrite(ENA,rotspeed);analogWrite(ENB,rotspeed);
sharpLeftTurn();
delay(320);
stop_bot();delay(500);
}
void left90_2()
{
Serial.println("left90");
analogWrite(ENA,rotspeed);analogWrite(ENB,rotspeed);
sharpLeftTurn();
delay(320);
stop_bot();delay(500);
}
void left180()
{
Serial.println("left180");
analogWrite(ENA,rotspeed);analogWrite(ENB,rotspeed);
sharpLeftTurn();
delay(720);
stop_bot();delay(500);
}
void right90()
{
Serial.println("right90");
analogWrite(ENA,rotspeed);analogWrite(ENB,rotspeed);
sharpRightTurn();
delay(340);
stop_bot();delay(500);
}
void sharpLeftTurn() {
Serial.println("sharpLeftTurn");
digitalWrite(motorInput1, HIGH);
digitalWrite(motorInput2, LOW);
digitalWrite(motorInput3, LOW);
digitalWrite(motorInput4, HIGH);
}
void sharpRightTurn() {
Serial.println("sharpRightTurn");
digitalWrite(motorInput1, LOW);
digitalWrite(motorInput2, HIGH);
digitalWrite(motorInput3, HIGH);
digitalWrite(motorInput4, LOW);
}
///////////////////////////////////////////////////////////////PRINTING STUFFS/////////////////////////////////////////////////////
void Printsensor(){
Serial.print(sensorL);Serial.print(" ");Serial.print(sensor[0]);Serial.print(sensor[1]);Serial.print(sensor[2]);Serial.print(sensor[3]);Serial.print(sensor[4]);
Serial.print(" "); Serial.print(sensorR);Serial.print(" ");
}
void printPath(){ //print the path after first pass
Serial.print("Final path: ");
for (i=0;i<=pathLength-1;i++)
{
Serial.print(path[i]);
}
}
////////////////////////////////////////////////////////////////SHORTEST_PATH///////////////////////////////////////////////////////
void del_replace(char path[],char replacement,int length,int position) // replace 3 element in an array with 'replacement'
{ //based on delete element in array algorithm used in Data structure
int i=0;
int pointer=0;
int a=length;
pointer=position;
for(i=pointer-1;i<=a-3;i++)
{
path[i+1]=path[i+3];
}
//int new_size = a-2;
path[pointer-1]=replacement;
}
void shortenPath(){
while(true){
for (i=0;i<=pathLength-1;i++)
{
if (path[i] == 'B') countB=countB+1; //check if B is gone
}
Serial.print("number of Bs: ");Serial.println(countB);
if (countB != 0)
{
for (i=0;i<=pathLength-1;i++) //index=B, look for behind & after
{
if (path[i] == 'B')
{
if (i==0) continue; //skip first element
else if (i==pathLength-1) break; //exit loop at end loop
else
{
if(path[i-1]=='L')
{
if(path[i+1]=='S')
{del_replace(path,'R',pathLength,i);countB=0;pathLength=pathLength-2;break;}
else if(path[i+1]=='R')
{del_replace(path,'B',pathLength,i);countB=0;pathLength=pathLength-2;break;}
else if(path[i+1]=='L')
{del_replace(path,'S',pathLength,i);countB=0;pathLength=pathLength-2;break;}
}
if(path[i-1]=='R')
{
if(path[i+1]=='L')
{del_replace(path,'B',pathLength,i);countB=0;pathLength=pathLength-2;break;}
}
if(path[i-1]=='S')
{
if(path[i+1]=='L')
{del_replace(path,'R',pathLength,i);countB=0;pathLength=pathLength-2;break;}
else if(path[i+1]=='S')
{del_replace(path,'B',pathLength,i);countB=0;pathLength=pathLength-2;break;}
}
}
}
}
for (i=0;i<=pathLength-1;i++)
{
Serial.print(path[i]);
} Serial.println(" ");Serial.print("pathLength: ");Serial.println(pathLength);
}
else break;
}
Serial.println("---------------------------");
Serial.print("SHORTEST PATH: ");
for (i=0;i<=pathLength-1;i++)
{
Serial.print(path[i]);
}
Serial.println(" ");
Serial.print("--------------------------- ");
}
////////////////////////////////////////////////////////////SECOND PASS//////////////////////////////////////////////////
void second_pass(){
second_pass_steps = pathLength;
while(state)
{
errorRead();
switch(mode)
{
case HORIZONTAL_LINE:
analogWrite(ENA,mspeed);analogWrite(ENB,mspeed);forward();delay(550);
errorRead();
if (mode==HORIZONTAL_LINE) {Serial.print("CONGRATULATIONS !!!");state=0;}
else
{
if(i != second_pass_steps ) {Serial.print("Path: ");Serial.println(path[i]);what_to_do(path[i]);i=i+1;}
break;
case RIGHT_TURN:
analogWrite(ENA,mspeed);analogWrite(ENB,mspeed);forward();delay(550);
if(i != second_pass_steps ) {Serial.print("Path: ");Serial.println(path[i]);what_to_do(path[i]);i=i+1;}
break;
case LEFT_TURN:
analogWrite(ENA,mspeed);analogWrite(ENB,mspeed);forward();delay(550);
if(i != second_pass_steps ) {Serial.print("Path: ");Serial.println(path[i]);what_to_do(path[i]);i=i+1;}
break;
case PID_FORWARD:
calculatePID();
PID_write_speed();
forward();
break;
}
}
}
}
void what_to_do(char instruction){
if (instruction == 'L') {Serial.println("Turn Left here");stop_bot();delay(200);left90();engine_start_mini();}
else if (instruction == 'R') {Serial.println("TurnRight here");stop_bot();delay(200);right90();engine_start_mini();}
else if (instruction == 'S') {Serial.println("Go straight here");}
}
Scheme:
The only difference is that I have 12v power supply
It's very difficult to read what pins are connected to what other pins in your diagram. Fritzing has a schematic view. Please use that and post the schematic diagram.
However, I can see that the 5V output from the motor driver is not connected to the 5V input of the Uno. It's connected to another pin in your diagram but I can read what that pin is called.
UPDATE:
I think you have connected the 5V from the motor driver to the IOREF pin of the Uno.
I had to look up what the purpose of that pin is. According to Wikipedia it is
IOREF: This pin on the Arduino/Genuino board provides the voltage reference with which the microcontroller operates. A properly configured shield can read the IOREF pin voltage and select the appropriate power source, or enable voltage translators on the outputs to work with the 5V or 3.3V.
So it's not clear if this pin can be used as a 5V input.
1 Like
If you need extra 5V and ground pins, you can use pins 2 and 6 on the ICSP header.
I don't have problem with supplynig power to arduino or sensors, so I didn't take this under consideration. Do you think that it could be a problem ? Besides that I made sensor calibration to the surface and it works fine, but I can't get smooth turns and traveling throught maze in live five days and I am desperate in solving that.
According to the official schematic for Uno, that pin is connected to the 5V pin, so I guess that is ok to use as a 5V input. How did you know this was ok to use that pin like that?
Please post that schematic as requested.
Please explain what this line does. Why did you assign a number with decimal places to an integer?
Also please correct the indentation of your code using Auto Format and post it again. Incorrect indentation can make it difficult to spot errors in code.
Please explain what you mean by this. In way way does the code not match the robot? Did you write that code for a different robot? If so, why can you not change it to match this robot?
you got code to line following robot. maze solving is something different.
1 Like
Use a right hand algorithm?
This is a bug.
This is a mashup of the two...
Here is the maze-solving part (shorten path versus shortest path, also where "kp=12.5" came from):
The line-following can be found on a few instructables and hacker pages with "LineFollower0"
But why does it compile?
Hi, @karlojar099
Welcome to the forum.
Do you know if your code works?
It sounds like you have written code but not tried it in the real world.
When you wrote your code, did you try it on hardware to see if you had any bugs?
Have you written code JUST to drive your motors?
To prove you can control them.
Have you written code to JUST read your sensors?
To prove you can read their output?
Can you please tell us your electronics, programming, arduino, hardware experience?
Thanks.. Tom.. 
1 Like
This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.