It’s probably best to make a separate function for the PID that you can call whenever you want. This way you can just pass the targetPos as a parameter to the function.
void drivePID(double targetPos){
//this is a constant that you will need to tune
double kP = 0.1;
//error is just the difference between target and current pos
//here I am just getting it from the front left motor, but it doesn't really matter which one you use
double error = targetPos - FrontLeft.position(deg);
//accuracy is how close you want the robot to get to the target; if it is too small it will take too much time to reach the target, but if it's too large it won't be accurate
//this is needed because the robot will pretty much never be exactly at the target position
double accuracy = 5;
//run the loop while the robot is not at target position
while (abs(error) > accuracy){
//update error
error = targetPos - FrontLeft.position(deg);
//calculate output power by multiplying error by kP
double pow = error * kP;
//assign the power to the motors
FrontLeft.setVelocity(pow, pct);
FrontRight.setVelocity(pow, pct);
BackLeft.setVelocity(pow, pct);
BackRight.setVelocity(pow, pct);
FrontLeft.spin();
FrontRight.spin();
BackLeft.spin();
BackRight.spin();
}
//stop when done
FrontLeft.stop();
FrontRight.stop();
BackLeft.stop();
BackRight.stop();
}
Then, you can just call it in your auton whenever you want to use it.
Thanks for your help but the code is having trouble with reversing. Any thoughts why that might be happening? The Turning is also a little bugged. Only the forward is working. Sometimes the code executes and sometimes it doesn’t. Really weird.
Could you share the auton code where you are calling the PID function? You have to keep in mind that the targetPos is absolute. This means that it is trying to go to the absolute position on the encoder, not from where it currently is. Also, what do you mean by the turning is bugged? This function is only for forward and backward, it is not supposed to do any turning. And yes, the slow velocity is caused by an untuned kP.
You don’t need to set the direction yourself because when the targetPos is less than the currentPos, the error will be negative and it will go backwards on its own. And when you tell it to go to 1770 then go to 500 in reverse, it’s not going to 1770 then back 500 to 1270, instead it will go all the way back to 500. This may or may not be what you want, but it’s something to take note of.
You don’t need to, just give it a target position that is less than its current position and it will figure out on its own that the target is behind it and that it needs to go backwards to get there.
For example if I want it to reverse 200 degrees you are saying do -200? Also the degree value is wrong for exmample 200 degrees is not the same as 200 from my previous code. Do you know what might have causes this?
I think you are misunderstanding how the absolute position works. When you say go to 500, it is not going to 500 forward from where it is, it will go until the encoder’s position reads 500. It does not matter where it currently is. If you want it to go back 200, you would have to tell it to go to currentPos - 200. You should take a look at the motor’s info on the brain screen. You can find it by clicking on devices and then selecting the port. This will show you the encoder’s readings. You can use this to get an understanding of how the encoder value changes as you move the robot.