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