# Help with Programming V5 Encoder PID for Turning

I need help with programming a pid controller for turning accurately with encoders. The code i have now, if i run just a turn with a value of 290 for encoder degrees the robot turns perfectly 90 degrees. But if i run that same turn of 290 encoder degrees during a autonomous routine the robot does not turn 90 degrees. In a nutshell, how would i make a pid controller for encoder turns that has consistent values throughout an autonomous routine.

It’s hard to do. The wheels themselves slip.

Try using a gyro sensor.

I’ve seen some teams use unpowered wheels with just encoders on them. This can get more accurate results since the wheels slip a little less than they would when driven by motors.

The robot had a problem when it got on the platform.
The accuracy of the red encoder is also a problem.

@9227 What type of wheels do you have?

If you use all omnis and center of mass of the robot is roughly coincides with the geometric center between the wheels, then you should have very little slip, given that you do not accelerate too fast. I’ve seen teams with such robots having very precise turning with just the encoders.

On the other hand, if you have a traction wheel pair and center of mass of the robot is far from the geometric center between those traction wheels, than you will get a lot of wheel slip even if you don’t accelerate very fast. The mass of the robot will resist turning to the point that traction wheels could start slipping sideways.

As @edjubuh said unpowered tracking wheels could help you to get super precise position tracking, but that would be the final step after you sort out center of gravity thing.

1 Like

I’m using V5 integrated motor encoders and referencing from left front wheel. I’m an utilizing Omni wheels and a good bit of my robots weight is in the front

@9227, did I understand correctly that you are using just one encoder as the feedback sensor for your turning?

I am a bit confused when you say that:

What is different between the test described in the first sentence and your autonomous run?

Also, are the wheels on each side of the robot chained together? If not, can you take the reading from encoder on each wheel, execute a 90 deg turn and then subtract original numbers from the new readings, print them to the screen and then tell us.

Ideally, both (front and back) numbers from the each side should be almost identical and those on the other drivetrain side same with the opposite sign.

If the numbers differ it is indication that the wheels are slipping and/or robot’s center of rotation differs from the geometric center between the wheels.

Could you move any weight to the back of the robot to make it more balanced?

1 Like

The only things that is different from the test run and the autonomous run is that in the test, the only line being called is the turn. But in the autonomous routine the turn line is being called when the robot needs to execute the turn in the line for the autonomous run

There are three wheels on each side and 4 motors for drivetrain. The front wheels are chained with the middle wheels on each side with motors driving each side. The back two wheels are idependantly driven by one motors each.

Hmm, could you insert a delay into autonomous to let the robot come to the complete stop before executing the turn?

Also, do you have any variables modified by previous autonomous steps that turning step expects to be at a certain value? Do you reset encoder counts before each step?

we have a wait time after the turn and the encoder gets reset everytime a target is set

Could you post the code? At this point my best guess for the cause of the issue is that one of the global variables and/or encoder state has different value by the time you get to turning step (as opposed to what it had at the beginning of the test).

``````#include "main.h"
using namespace okapi;

pros::Vision visionX (10);

std::string direction;
int driveTarget;
bool newDriveTarget;

// PID CONTROLLER FUNCTIONS + TASK

//positive is forward
void drive(int speed) {

LeftF.move_velocity(speed);
LeftB.move_velocity(speed);

RightF.move_velocity(speed);
RightB.move_velocity(speed);

}

void turnRight(int speed) {

LeftF.move_velocity(-speed);
LeftB.move_velocity(-speed);

RightF.move_velocity(speed);
RightB.move_velocity(speed);

}

void turnLeft(int speed){

LeftF.move_velocity(speed);
LeftB.move_velocity(speed);

RightF.move_velocity(-speed);
RightB.move_velocity(-speed);

}

void driveStop() {

LeftF.move_velocity(0);
LeftB.move_velocity(0);

RightF.move_velocity(0);
RightB.move_velocity(0);

}

void setDrive(std::string dir, int target)
{
direction = dir;
driveTarget = target;
LeftF.tare_position();
LeftB.tare_position();
RightF.tare_position();
RightB.tare_position();
//	Gyro1.reset();
newDriveTarget = true;
if (direction == "left" || direction == "right" || direction == "back")
driveTarget *= -1;
}

void pidController (void* param){

int driveState = 0;

//pd constants
const float encoderKp = 0.25;
const float encoderKd = 0.081;
const float turnKp = 0.9;//0.55 original
const float turnKd = 1.2;//0.02 original
const float maxTurnPower = 100;

int proportion;
int derivative;
int error;
int prevError;
int raw;

while(true) {
if (!pros::competition::is_autonomous()) driveState = 0;
else if (direction == "null") driveState = 1;
else if (direction == "forward" || direction == "back") driveState = 2;
else if (direction == "left") driveState = 3;
else if (direction == "right") driveState = 4;

switch(driveState) {
//manual drive case
case 0:

//auton no pid case
driveStop();

break;

case 1:

//auton no pid case
driveStop();

break;

case 2:

// encoder drive pid

error = driveTarget - LeftF.get_position();

proportion = encoderKp * error;

derivative = encoderKd * (error - prevError);
prevError = error;
if (newDriveTarget) derivative = 0;

raw = proportion + derivative;

drive(raw);

newDriveTarget = false;
break;

case 3:

//encoder turn pid left

error = driveTarget - ((LeftF.get_position()+ LeftB.get_position())/2);

proportion = turnKp * error;

derivative = turnKd * (error - prevError);
prevError = error;
if (newDriveTarget) derivative = 0;

raw = proportion + derivative ;

turnLeft(raw);

newDriveTarget = false;

break;

case 4:

//encoder turn pid right

error = driveTarget - ((RightF.get_position()+ RightB.get_position())/2);

proportion = turnKp * error;

derivative = turnKd * (error - prevError);
prevError = error;
if (newDriveTarget) derivative = 0;

raw = proportion + derivative ;

turnRight(raw);

newDriveTarget = false;

break;
//default case: manual controls
default:

//auton no pid case
driveStop();

break;
}
pros::delay(20);
}
}

void autonomous() {

// test

//setDrive("left", 210);
//pros::delay(4000);

//________________

//setDrive("null",0);

//Catapult.set_brake_mode(pros:: E_MOTOR_BRAKE_HOLD);

//while(catapultPT.get_value() < 1980){
//Catapult.move_velocity(50);
//}

/*  Catapult.move_velocity(0);

runIntake(200);

Flipper.tare_position();

Flipper.move_absolute(120, 140);

setDrive("forward", 1200);
pros::delay(1200);

setDrive("null", 0);
pros::delay(100);

lineBackupLeft(100);
pros::delay(200);

drive(20);
pros::delay(200);

driveStop();
pros::delay(10);

setDrive("back", 430);
pros::delay(650);

setDrive("null", 0);
pros::delay(100);
//_______________________
setDrive("left",235);
pros::delay(960);
//_______________________
//Catapult.move_velocity(90);
pros::delay(800);

Catapult.move_velocity(0);

setDrive("null",0);
pros::delay(100);

setDrive("forward", 1200);
pros::delay(1200);

Flipper.move_absolute(420, 140);

setDrive("back",500);
pros::delay(600);

setDrive("null",0);
pros::delay(100);

setDrive("right", 260);
pros::delay(800);

setDrive("forward", 600);
pros::delay(800);

setDrive("null",0);

Flipper.move_absolute(0,140);

setDrive("forward", 400);
pros::delay(1000);

setDrive("null",0);
pros::delay(100);

setDrive("right", 270);
pros::delay(800);

}``````

@9227, I didn’t see anything obvious in your code that I thought could cause the issue, but I might have missed something.

What does lineBackupLeft(100); do?

Could motors be overheating and running at lower power and out of time?
Try to increase timeout after the left turn and see if it could turn 90 deg given enough time.

In general, I would recommend against the asynchronous pidController() task that cannot communicate back when it is finished.

My preference would be to call the function for each drive step giving it target position and max time after which it should give up. That function would remember its start time, and then exit if either target was reached (within some small range) or max time had elapsed and return to the caller if it was able to reach the target.

Then you could decide if you want to continue autonomous in case of timeout, depending on what specific step it was.

1 Like

How do I go about writing a function that uses time for pid

What @technik3k said.

Also, there’s no minimum value for your PID. As the error gets closer to 0 so does your velocity. It’s likely never completing the turn before the delay() expires. Or it’s getting to an error small enough that it won’t move the robot any more. Or both.

I would also only look at one encoder. Averaging two for turns might do more harm than good. Pick the wheel that slips the least for each turn direction.

What @technik3k suggests is to not have your PID as a task just running out there on its own, really only changing states when you send it new values. Turn it into a callable function and send it parameters to include a time value that it checks as it loops. The PID function loops until it hits the target angle or the timeout value is met.

Derivative is somewhat pointless in this application, too. Your error is always decreasing so you don’t really need a term to react to the rate of change of the error.

I doubt this will compile, but here’s the idea. It combines your setDrive() function and pidController() function:

``````void pidController (std::string direction, int driveTarget, int timeout) {

int driveState = 0;

LeftF.tare_position();
LeftB.tare_position();
RightF.tare_position();
RightB.tare_position();
//	Gyro1.reset();
if (direction == "left" || direction == "right" || direction == "back")
driveTarget *= -1;

//pd constants
const float encoderKp = 0.25;
const float encoderKd = 0.081;
const float turnKp = 0.9;//0.55 original
const float turnKd = 1.2;//0.02 original
const float maxTurnPower = 100;

int proportion;
int derivative;
int error;
int prevError;
int raw;
long current_time

if (!pros::competition::is_autonomous()) driveState = 0;
else if (direction == "null") driveState = 1;
else if (direction == "forward" || direction == "back") driveState = 2;
else if (direction == "left") driveState = 3;
else if (direction == "right") driveState = 4;

switch(driveState) {
case 3:
//encoder turn pid left
current_time = millis();
error = driveTarget - ((LeftF.get_position() + LeftB.get_position())/2);

while (error <= 0) {
proportion = turnKp * error;
derivative = turnKd * (error - prevError);
prevError = error;
raw = proportion + derivative ;
if (raw < 20) {
raw = 20;
}

turnLeft(raw);

error = driveTarget - ((LeftF.get_position()+ LeftB.get_position())/2);
if (millis() > (current_time + timeout)) {
break;
}

pros::delay(20);
}
}

break;

}

void autonomous() {

pidController("left",235);

}
``````
1 Like

this is what i came up with but the code does not stop running it keeps going.

``````void driveENC(std::string dir, int target, int timeout){

int drivestate = 0;

const float encoderKp = 0.9;
const float encoderKd = 1.2;

int proportion;
int derivative;
int error;
int prevError;
int raw;
long current_time;

if (dir == "back") target *= -1;

if (dir == "forward") drivestate = 1;
else if (dir  == "back") drivestate = 2;
else if (dir == "null") drivestate = 0;

switch(drivestate){

case 0:
// no pid gyro
break;
case 1:

// forward
current_time = pros::millis();
error = target - LeftF.get_position();

while (error >= 0) {
proportion = encoderKp * error;
derivative = encoderKd * (error - prevError);
prevError = error;
raw = proportion + derivative ;

if (raw < 20) {
raw = 20;
}

drive(raw);
error = target - LeftF.get_position();

if (pros::millis() > (current_time + timeout)) {
break;
}
pros::delay(20);  }

break;
case 2:

// backward
current_time = pros::millis();
error = target - LeftF.get_position();

while (error <= 0) {
proportion = encoderKp * error;
derivative = encoderKd * (error - prevError);
prevError = error;
raw = proportion + derivative ;

if (raw < 20) {
raw = 20;
}

drive(raw);
error = target - LeftF.get_position();

if (pros::millis() > (current_time + timeout)) {
break;
}
pros::delay(20);  }

break;
case 3:
// no pid gyo
break;
default:
// no pid gyro
break;

}

}``````

Time to surround your variables and logic blocks with print statements to see what the actual values are and what condition isn’t being met.

I’d start with the while conditions. It’s possible that the >=0 above should be >0, or that the signs are wrong and the error is actually increasing from a negative state.