So, we were able to get our code working for the most part. But there are 2 things we had to do, that I don’t understand why, other than it works.
double l = left.calculate(encoder_position_left);
double r = right.calculate(encoder_position_right);
double gyro_heading = ... your gyro code here ... // Assuming the gyro is giving a value in degrees
double desired_heading = Pathfinder.r2d(left.getHeading()); // Should also be in degrees
double angleDifference = Pathfinder.boundHalfDegrees(desired_heading - gyro_heading);
double turn = 0.8 * (-1.0/80.0) * angleDifference;
setLeftMotors(l + turn);
setRightMotors(r - turn);
what are the magic numbers or formula in: 0.8 * (-1.0/80.0) * angleDifference;
For the “…your gyro code here…” we used the navx GetYaw(), but that caused problems. Our gentle S curve from center to switch, when using getYaw(), caused the bot to turn almost 90 degrees and head towards the corner. If we used -getYaw() it worked.
I’m afraid the ‘fix’ will work on our current path, and when we try another path it might break.
Brian