No, placement for motor speed should be inside the while loop. You are correct in saying the motors will not be assigned the
speed
variable since they are outside the loop.
Inside the loop, right before you assign a new value to
error
, set
previous_error
to
error
.
It might look something like this:
while ( want PID to be enabled )
{
error = targ_val-cur_val;
//random PID stuff
// like setting motor speed and calculating derivative, etc.
previous_error = error;
}
What you have currently is actually fine because you’re not modifying or using either
previous_error
or
error
after you change them until the next loop
Yes. There are several bugs in the code that was posted
bug 1: semicolon in the conditional for the while loop
bug 2: you’re missing minus signs? This might be just my Chrome browser bugging out but it seems like the first line in the loop should have been
error=target_value-sensor_reading;
and later when calculating
derivative
it should have been
derivative=error-previous_error;
bug 3: The motors are written to outside of the loop (already discussed).
bug 4: Why is
integral
being reset to zero in the second if statement inside the loop? Not a very big deal but for optimal tuning I might have set it to a cap (like, 40/Ki) instead of resetting all the way back to zero.
All in all, your final code should look something like this:
Kp = 0.5;
Ki = 0.2;
Kd = 0.1;
task autonomous();
{
int error=0;
int integral=0;
int derivative=0;
int previous_error=0;
int speed=0;
int targ_speed=100;//or whatever other speed you want
while (nMotorEncoder[backLeft] < 1000;)
{
cur_speed = ????; // calculate current speed somehow
error = targ_speed-cur_speed;
integral = integral + error;
if (error == 0) {
integral = 0;
}
if ( integral>40/Ki ) {
integral = 40/Ki; // this caps the maximum power effect integral can have at 40
}
derivative = error -previous_error;
previous_error = error;
speed = Kp*error + Ki*integral + Kd*derivative;
motor[backLeft] = speed;
motor[backRight] = speed;
motor[frontLeft] = speed;
motor[frontRight] = speed;
}
}
I am kind of confused as to what you are trying to do with this PID code. Do you want to drive at a certain exact velocity? Or go a certain distance? Go a certain distance at an exact velocity?
If you want to go a certain distance, you can just do
while( nMotorEncoder[backleft]<1000 ) {
motor[backLeft] = 127;
motor[backRight] = 127;
motor[frontLeft] = 127;
motor[frontRight] = 127;
}
motor[backLeft] = -127;
motor[backRight] = -127;
motor[frontLeft] = -127;
motor[frontRight] = -127;
delay(200); // eliminate momentum