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

`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;

}