PID ALGORITHM
Set position = 0 for run forward and set PWM L= R = constant
Set position = -5 and that robot restrains for turn left have to set left medium slow or medium decrease and set right medium fast or medium increase .Position =-5 for run L is medium decrease and R is medium increase.
Else hand, Set position = 5 and that robot restrains for turn right have to set left medium fast or medium increase and set right medium slow or medium decrease .Position = 5 for run L is medium increase and R is medium decrease.
Set position =-10 and that robot restrains for turn left have to set left very slow and set right very fast.Position =-10 for run L is very slow and R is very fast.
Else hand, Set position = 10 and that robot restrains for turn right have to set left very fast and set right very slow.Position = 10 for run L is very fast and R is very slow.
Position =-15 to left and position =15 to right, set in memory SRAM after build left motor backward and right motor forward on condition turn left. And for turn right is opposite this condition.
PIDsensors(){
switch(sensor){
case 0b01000000: position=-10; break;
…
case 0b00110000: position=-5; break;
case 0b00011000: position=0; break;
case 0b00001100: position=5; break;
…
case 0b00000010: position=10; break;
}
int proportional = ((int)position); // The "proportional" term should be 0 when we are on the line.
int derivative = proportional - last_ proportional; // Compute the derivative
integral += proportional; //integral (sum) of the position.
last_proportional = proportional; // Remember the last position.
P=kp*proportional;
I=Ki*integral;
D=Kd*derivative;
// Compute the difference between the two motor power settings, m1 - m2. If this is a positive number //the robot will turn to the right. If it is a negative number, the robot will turn to the left, and the //magnitude of the number determines the sharpness of the turn.
Power=P+I+D;
L=max + power;
R=max - power;
Set _motors(L,R);
}
…
…
While(1){
Read _sensors(); // Get the position of the line.
PIDsensors(); // get PWM value for motors
}
MEMBUAT ROBOT LINE FOLLOWER
