Here's is a working bit of code that I have made in NXC to do pivot turns (heading corrections), but I am sure much smarter people have come up with a simpler, cleaner way to do this. Please feel free to provide critique and/or other feedback.
Cheers!
-Loopy
Code: Select all
task move()
{
while (true){
// calculate relative heading and use Proportional correction to pivot
// towards target heading..."relative_heading" is equal to the angle
// difference between "target_heading" and current compass heading
while (abs(target_heading - Compass_val) > 1){
if (target_heading - Compass_val < -179){
relative_heading = target_heading - Compass_val + 360;
}
else{
relative_heading = target_heading - Compass_val;
}
// calculate pivot direction, always taking shortest CW or CCW path
if (relative_heading < 0){
steer = -100;
}
else{
steer = 100;
}
// set pwr for use in OnFwdSync
pwr = abs(relative_heading);
// limit min/max pwr (to prevent stalls and whirling dervish spins)
if (pwr > 35)
{
pwr=35;
}
if (pwr < 15)
{
pwr=15;
}
// do the needful (make the pivot turn)
OnFwdSync(OUT_BC, pwr, steer);
}
// stop, wait and set new "target_heading" (new target heading)
Off(OUT_BC);
Wait(3000);
target_heading=target_heading+90;
// limit target heading to 0->359 deg
if (target_heading > 359)
{
target_heading = 0;
}
}
}