Table 1.
PID control algorithm.
| γ = β − α //Heading angle deviation γ |
| if (γ > 180.0) //Optimize the direction |
| γ = γ − 360 |
| if (γ < −180.0) |
| γ= γ + 360 |
| Pvalue = KP × γ //Proportion value |
| if (|γ|< 20.0) |
| Ivalue = Ivalue + KI × γ × Timelnterval //Integral value |
| else |
| Ivalue = 0.0 |
| if (Ivalue >30.0) //Set the threshold of Ivalue |
| Ivalue = 30.0 |
| else if (Ivalue < −30.0) |
| Ivalue = −30.0 |
| Dvalue = KD × (γ − previous γ) / TimeInterval //Differential value |
| if (Dvalue > 20.0) //Set the threshold of Dvalue |
| Dvalue = 20.0 |
| else if (Dvalue < −20.0) |
| Dvalue = −20.0 |
| previous γ = γ |
| ɛ = Pvalue + Ivalue + Dvalue //error output ɛ |