As what I promised, I will post my simply PID(PD) controller code here:<\/p>\n
void PID(void)\u00a0\u00a0 \u00a0\u00a0 \u00a0<\/span> It works pretty well especially if you don’t have a high resolution encoder or accelerometer as feedback. If you use high resolution encoder to generate your own speed profile, you will need to add sensor error onto rotational error associated with encoder for PID.<\/p>\n <\/p>\n","protected":false},"excerpt":{"rendered":" As what I promised, I will post my simply PID(PD) controller code here: void PID(void)\u00a0\u00a0 \u00a0\u00a0 \u00a0 {\u00a0\u00a0 \u00a0 \u00a0\u00a0 \u00a0if((leftSensor > hasLeftWall && rightSensor > hasRightWall))\/\/has both walls \u00a0\u00a0 \u00a0{\u00a0 \/\/ccw direction is positive \u00a0\u00a0 \u00a0\u00a0\u00a0 \u00a0errorP = rightSensor … Continue reading
\n{\u00a0\u00a0 \u00a0<\/span>
\n\u00a0\u00a0 \u00a0if((leftSensor > hasLeftWall && rightSensor > hasRightWall))\/\/has both walls<\/span>
\n\u00a0\u00a0 \u00a0{\u00a0 \/\/ccw direction is positive<\/span>
\n\u00a0\u00a0 \u00a0\u00a0\u00a0 \u00a0errorP = rightSensor – leftSensor<\/span> – 63;\/\/63 is the offset between left and right sensor when mouse in the middle of cell<\/span>
\n\u00a0\u00a0 \u00a0\u00a0\u00a0 \u00a0errorD = errorP – oldErrorP;<\/span>
\n\u00a0\u00a0 \u00a0}\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 \u00a0<\/span>
\n\u00a0\u00a0\u00a0 else if((leftSensor > hasLeftWall))\/\/only has left wall<\/span>
\n\u00a0\u00a0 \u00a0{<\/span>
\n\u00a0\u00a0 \u00a0\u00a0\u00a0 \u00a0errorP = 2 * (leftMiddleValue – leftSensor);<\/span>
\n\u00a0\u00a0 \u00a0\u00a0\u00a0 \u00a0errorD = errorP – oldErrorP;<\/span>
\n\u00a0\u00a0 \u00a0}<\/span>
\n\u00a0\u00a0 \u00a0else if((rightSensor > hasRightWall))\/\/only has right wall<\/span>
\n\u00a0\u00a0 \u00a0{<\/span>
\n\u00a0\u00a0 \u00a0\u00a0\u00a0 \u00a0errorP = 2 * (rightSensor – rightMiddleValue);<\/span>
\n\u00a0\u00a0 \u00a0\u00a0\u00a0 \u00a0errorD = errorP – oldErrorP;<\/span>
\n\u00a0\u00a0 \u00a0}<\/span>
\n\u00a0\u00a0 \u00a0else if((leftSensor < hasLeftWall && rightSensor <hasRightWall))\/\/no wall, use encoder or gyro<\/span>
\n\u00a0\u00a0 \u00a0{<\/span>
\n\u00a0\u00a0 \u00a0\u00a0\u00a0 \u00a0errorP = 0;\/\/(leftEncoder – rightEncoder*1005\/1000)*3;<\/span>
\n\u00a0\u00a0 \u00a0\u00a0\u00a0 \u00a0errorD = 0;<\/span>
\n\u00a0\u00a0 \u00a0}<\/span>
\n\u00a0\u00a0 \u00a0totalError = P * errorP + D * errorD;<\/span>
\n\u00a0\u00a0 \u00a0oldErrorP = errorP;<\/span>
\n\u00a0\u00a0\u00a0 setLeftPwm(leftBaseSpeed – totalError);<\/span>
\n\u00a0\u00a0\u00a0 setRightPwm(rightBaseSpeed + totalError);\u00a0\u00a0 \u00a0<\/span>
\n}<\/span><\/p>\n