Commit d2a2dd8d authored by Andre Lipke's avatar Andre Lipke

Fixed twitching bug in execMove with high kI

parent 4cd19054
......@@ -35,8 +35,9 @@ void auton::execMove(float distance, float l, float r) {
return;
}
float scr = sctrl.step((lcurrent - lbase) / l - (rcurrent - rbase) / r, 0.0);
motors::leftDrive.set(clamp(lctrl.step(lcurrent, ltarget) + scr, -127.0, 127.0));
motors::rightDrive.set(clamp(rctrl.step(rcurrent, rtarget) - scr, -127.0, 127.0));
float fwd = lctrl.step(lcurrent, ltarget);
motors::leftDrive.set(clamp(fwd + scr, -127.0, 127.0));
motors::rightDrive.set(clamp(fwd - scr, -127.0, 127.0));
wait(20);
}
motors::leftDrive.set(0);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment