- CODE: SELECT_ALL_CODE
signed int set_value = 180;
signed int feedback_value = 0;
unsigned char kp = 2;
unsigned char ki = 0;
unsigned char kd = 1;
void calcPID ()
{
signed long error_value = 0;
static signed long pre_error = 0;
static signed long integral = 0;
signed long derivative = 0;
signed long output = 0;
signed int motor_direction = 0;
unsigned char pwm_value=0;
error_value = set_value - feedback_value; // Calculate the error.
integral = integral + error_value; // Calculate integral.
derivative = error_value - pre_error; // Calculate derivative.
output = (kp * error_value) + (ki * integral) + (kd * derivative); // Calculate the output, pwm.
if (output > 255) output = 255; // Limit the output to maximum 255.
else if (output < -255) output = -255;
if (output > 0) goRight((unsigned char)output); // When output is positive, turn right.
else if (output < 0) goLeft((unsigned char)(-output)); // When error is negative, turn left.
else tempStop(); // When ouput is 0, stop the motor.
pre_error = error_value; // Save as previous error.
}
i need to make it move forward, then reset encoder reading, then move right...
but only managed to do this...
- CODE: SELECT_ALL_CODE
void loop()
{
while((feedback_value = getLeftEncoder()) != set_value)
{
calcPID(); //pid to move the robot forward
}
resetEncoder();
while((feedback_value = getLeftEncoder()) != set_valueRotate)
{
calcPIDRight(); //pid to move the robot right 90°
}
}
but what happens is it wont make the robot go right... it stucks in the first while loop... is there a good way to write looping to do 2 PID continuously?