PID issue or otherwise?

Programming Support VEXcode Pro V5 Text Tech Support
1

I have a function in my code that tells the robot to drive for a certain number of inches. The function is listed here.

/* @brief drive for inches
@param distance the distance driven for in inches
@param waiting adds a 50 msec wait after the drive to ensure robot stops effectively */
void driveFor(float distance, bool waiting)
{
RightDriveSmart.resetPosition();
LeftDriveSmart.resetPosition();
double heading = Inertial.heading(deg);
PID drivePID = PID(0.65, 0.01, 0.01, distance - ((wheelSize * pi) * ((RightDriveSmart.position(degrees) * dtGearRatio) / 360.0)), 12, 0.75, 30, 12, -12);
PID correct = PID(0.1, 0, 0, restrain(heading - Inertial.heading(deg), -180, 180), 0, 0, 999, 8, -8);
float t = 0;

while(!drivePID.settled())
{
float output = drivePID.update(distance - ((wheelSize * pi) * ((RightDriveSmart.position(degrees) * dtGearRatio) / 360.0)));
float correction = correct.update(restrain(heading - Inertial.heading(deg), -180, 180));

LeftDriveSmart.spin(forward, (1*output) + correction, volt);
RightDriveSmart.spin(forward, (1*output) - correction, volt);


task::sleep(10);
t += 0.01;

}
LeftDriveSmart.stop(brake);
RightDriveSmart.stop(brake);
if (waiting)
{
wait(0.05, sec);
}
printf(“\033[34mDrove for %.3f inches, in %.2f seconds (1p)\n”, ((wheelSize * pi) * ((RightDriveSmart.position(degrees) * dtGearRatio) / 360.0)), t);
}
`

In my program, when I tell the code to drive reverse for 7.0 inches, and wait 50 msec …

driveFor(-7, true);

… it first drives forward 7 inches, and then reverse 7, and continues on with the code as it should. This causes it to ram the wall and sometimes whitescreen the brain. For obvious reasons, this is not appreciated. This only happens with this first line of my code in my autonomous.cpp file. It also doesn’t happen with other autonomous routes.
To avoid snarky comments, yes, I could simply add an extra second or so and turn 90 degrees so that the robot doesn’t hit the wall, have it do its thing, and then turn back to the correct heading and continue on with the autonomous program. However, I would like to fix this for future reference for myself, and in the event that this, or something similar has happened for others. Is there something I am doing wrong in my function that would make this only happen on the first line of my code, or does anyone forsee another software solution I could implement?

Thank you for your help.

2

Not exactly related to your question, but change your battery cable and/or battery. white screen is caused by momentary power loss.

and in terms of the code issue, you would need to provide the PID class source code (please use code tags rather than quoting the code)

2 Likes