Go Down

Topic: Maze Solver_Left wall follow robot ===== Need help (Read 313 times) previous topic - next topic

MohamedMostafaRagab

hi all
i'm start to make amaze solver robot
after search i found that pid is best thing to control left wall follow
i found a code to control the robot using PID Wall follow robot
 
i start to modify the code to work with the simple maze
but i face some proble
1- pid tuning
2- eddges control

if any body can help please

Code: [Select]
here is my modified code if you can help me

#include <SR04.h>

/Driver Pins/
const int LeftMotorForward = 3;
const int LeftMotorBackward = 5;
const int RightMotorForward = 9;
const int RightMotorBackward = 6;

int WF_LEFT_US_SENSOR_TRIG = 13;
int WF_LEFT_US_SENSOR_ECHO = 12;
int WF_FRONT_US_SENSOR_TRIG = 10;
int WF_FRONT_US_SENSOR_ECHO = 11;

int NO_OBSTACLE = 0;
int OBSTACLE = 1;

int WF_DISTANCE = 8;
int WF_FRONT_DISTANCE = 12;

SR04 wf_sr04_left = SR04(WF_LEFT_US_SENSOR_ECHO,WF_LEFT_US_SENSOR_TRIG);
SR04 wf_sr04_front = SR04(WF_FRONT_US_SENSOR_ECHO,WF_FRONT_US_SENSOR_TRIG);

int WF_Left = 0;
int WF_Front = 0;

int WF_Left_Status = 0;
int WF_Front_Status = 0;

//Obstacle in Left: Keep Following: PID values
float WF_Kp = 10;
float WF_Ki = 0.003;
float WF_Kd = 0.4;

//Obstacle in Front: Take a turn: PID values
float MY_Kp_Front = 40;
float MY_Ki_Front = 0;
float MY_Kd_Front = 20;

float WF_Error = 0;
int WF_Correction = 0;
float WF_Integral = 0;
float WF_Derivative = 0;
float WF_LastError = 0;

float MY_Error_Front = 0;
float MY_Correction_F = 0;
float MY_Integral_Front = 0;
float MY_Derivative_Front = 0;
float MY_LastError_Front = 0;

int WF_LeftTurnSpeed = 0;
int WF_RightTurnSpeed = 0;

//Here 0 stands for true and 1 stands for false
int rightTurnBegin = 0;
int leftTurnBegin = 0;
int straightLineBegin = 0;

void setup() {

Serial.begin(9600);
pinMode(RightMotorForward, OUTPUT);
pinMode(LeftMotorForward, OUTPUT);
pinMode(LeftMotorBackward, OUTPUT);
pinMode(RightMotorBackward, OUTPUT);
delay(1000);
}

void loop() {

beginning:

WF_Front_Status = WF_GET_FRONT_US_STATUS();

if(WF_Front_Status == OBSTACLE) {

    //Initialize WALL only once, just in the beginning of right turn
    if (rightTurnBegin == 0)
      WF_INITIALIZE_WALL();

    WF_CONTINUE_WALL_FRONT();

    //Make the right turn flag false, until the next time there arises a right turn
    rightTurnBegin = 1;
    straightLineBegin = 0;

    goto beginning;
}

//When a the robot starts following the straight line (until there is a obstacle), initialize the flag to true.
rightTurnBegin = 0;

WF_Left = wf_sr04_left.Distance();
WF_Error = WF_Left - WF_DISTANCE;
WF_Integral = (WF_Error + WF_Integral);
WF_Derivative = (WF_Error - WF_LastError);

WF_Correction = WF_Kp * WF_Error + WF_Kd * WF_Derivative + WF_Ki * WF_Integral;

//if WF_Error is less than 10, means the robot is following the wall correctly, keep continuing
if (WF_Error < 10) {

  //Initialize the wall, just in the beginning of straight line
  if (straightLineBegin == 0)
    WF_INITIALIZE_WALL();

  if(WF_Correction > 70 && WF_Correction > 0)
    WF_Correction = 70;

  if(WF_Correction < -70 && WF_Correction < 0)
    WF_Correction = -70;

  WF_LeftTurnSpeed = 71 - WF_Correction;
  WF_RightTurnSpeed = 71 + WF_Correction;

  leftTurnBegin = 0;
  straightLineBegin = 1;

} else {
 
  //if WF_Error is greater than 10, means the robot has to take left turn

  //Initialize the wall and wall front, just in the beginning of left turn
  if (leftTurnBegin == 0) {
    WF_INITIALIZE_WALL();
    WF_INITIALIZE_WALL_FRONT();
  }

  //PID for left turn
  int speed = 2.5 * WF_Error + 8 * WF_Derivative;

  if (speed > 70 && speed > 0)
    speed = 70;

  if (speed < -70 && speed < 0)
    speed = -70;

  WF_LeftTurnSpeed = 71 - (speed);
  WF_RightTurnSpeed = 71 + (speed);

  leftTurnBegin = 1;
  straightLineBegin = 0;

}
analogWrite(LeftMotorForward,WF_LeftTurnSpeed);
analogWrite(RightMotorForward,WF_RightTurnSpeed);

WF_LastError = WF_Error;
WF_INITIALIZE_WALL_FRONT();
}

void WF_INITIALIZE_WALL(void) {
WF_Integral = 0;
WF_Derivative = 0;
WF_LastError = 0;
}

void WF_INITIALIZE_WALL_FRONT(void) {
MY_Integral_Front = 0;
MY_Derivative_Front = 0;
MY_LastError_Front = 0;
}

//When there is an obstacle in front, take a right turn with the PID parameters declared in the beginning of the program.

void WF_CONTINUE_WALL_FRONT(void) {

    WF_Front = wf_sr04_front.Distance();
    MY_Error_Front = (WF_Front - WF_FRONT_DISTANCE);
    MY_Integral_Front = (MY_Error_Front + MY_Integral_Front);
    MY_Derivative_Front = (MY_Error_Front - MY_LastError_Front);

    MY_Correction_F = MY_Kp_Front * MY_Error_Front + MY_Kd_Front * MY_Derivative_Front + MY_Ki_Front * MY_Integral_Front;

    if(MY_Correction_F > 70 && MY_Correction_F > 0)
      MY_Correction_F = 70;

    if(MY_Correction_F < -70 && MY_Correction_F < 0)
      MY_Correction_F = -70;

    WF_LeftTurnSpeed = 71 - MY_Correction_F;
    WF_RightTurnSpeed = 71 + MY_Correction_F;
analogWrite(LeftMotorForward,WF_LeftTurnSpeed);
analogWrite(RightMotorForward,WF_RightTurnSpeed);

    MY_LastError_Front = MY_Error_Front;
}

int WF_GET_LEFT_US_STATUS(void) {
if(wf_sr04_left.Distance() < WF_DISTANCE)
return OBSTACLE;
else
return NO_OBSTACLE;
}

int WF_GET_FRONT_US_STATUS(void) {
if(wf_sr04_front.Distance() < WF_FRONT_DISTANCE)
return OBSTACLE;
else
return NO_OBSTACLE;
}
 

slipstick

1- pid tuning
2- eddges control
 
That doesn't exactly tell us what your code does that's different from what you want it to do and of course we don't have your robot to test it ourselves. A little more detail of the problems might help.

Steve

MohamedMostafaRagab

#2
Apr 14, 2019, 10:54 pm Last Edit: Apr 15, 2019, 05:21 am by MohamedMostafaRagab
That doesn't exactly tell us what your code does that's different from what you want it to do and of course we don't have your robot to test it ourselves. A little more detail of the problems might help.

Steve
Thanks for response i'm trying to upload a video
the robot not stable at wall follow and at turn it's hit the wall always
 

PaulS

How do you expect to maintain a reasonably constant distance from a wall when the ONLY thing you know is that there is, or is not, a wall on the left?
The art of getting good answers lies in asking good questions.

MohamedMostafaRagab

How do you expect to maintain a reasonably constant distance from a wall when the ONLY thing you know is that there is, or is not, a wall on the left?
from all I found PID is the Best Way but i can't tune the parameters if you can help me i appreciate

PaulS

Quote
from all I found PID is the Best Way
PID is ONLY useful when you know the distance to the wall, and the distance you want to maintain. It is useless when all you know is that there is a wall somewhere to the left, or there isn't.

Quote
but i can't tune the parameters
For obvious reasons.
The art of getting good answers lies in asking good questions.

MohamedMostafaRagab

PID is ONLY useful when you know the distance to the wall, and the distance you want to maintain. It is useless when all you know is that there is a wall somewhere to the left, or there isn't.

sure i want to mainten the distance to be always 12 Cm


For obvious reasons.

i can't tune it takes me long time but result is ugly

PaulS

Quote
i can't tune it takes me long time but result is ugly
I was looking at only one part of your code, where you decided whether there was a wall on the left, or not. I missed that in another part of your code, you know the distance to the wall. So I looked again, and I saw this:
Code: [Select]
//if WF_Error is less than 10, means the robot is following the wall correctly, keep continuing
if (WF_Error < 10) {


This tells me that you want to maintain a distance of 12 cm +/- 10 cm. Its no wonder that you can't tune your system. The error value used in the PID calculation should be the absolute value of WF_Error, and the sign of WF_Error should be used along with the Output value to determine what adjustments to make.
The art of getting good answers lies in asking good questions.

Go Up