I want my code to do this:
When I tell it do drive forward, say 60 inches, it should drive 40 inches at the standard velocity, then slow down. This way, it will slowly decelerate. I do not want to go through the time of coding and tuning a PID loop (for now, maybe I’ll look into it after our next tournament before state), so I thought that this should work.
However, I do not know of any commands which return the amount left for a drivetrain to move. For this reason, I came up with this code which might work. I would like to know, would running two drive commands on top of each other mess up the code or would it theoretically work fine?
void driveForward(double dist){
//Robot velocity is large
robot.setDriveVelocity(80, percent);
//The total distance needed to travel
robot.driveFor(forward, dist, inches, false);
//When the robot is 20 in. away from the target, it will proceed to the next task
robot.driveFor(forward, dist-20, inches, true);
//gradually reduce the drive velocity
robot.setDriveVelocity(60, percent);
wait(1, sec);
robot.setDriveVelocity(45, percent);
wait(1, sec);
robot.setDriveVelocity(30, percent);
//Robot is 4-5 inches from target, moving slowly to reduce jerk at the end of the motion
}
I didn’t use multiple drive forward commands because I fear that something like this would cause the internal drivetrain PID to correct itself multiple times, resulting in lots of error from the internal PID. Would the multiple drive commands mess up the code?
