Help with PID in PROS

Programming Support
, ,
#1

I need help, I have been reading in some past posts but I can not solve the problem that my PID controller does not perform any action, if you could help me I thank you very much


//Left//
pros::Motor left1 (16, pros::E_MOTOR_GEARSET_18, true, pros::E_MOTOR_ENCODER_ROTATIONS);
pros::Motor left2 (18, pros::E_MOTOR_GEARSET_18, true, pros::E_MOTOR_ENCODER_ROTATIONS);
pros::Motor left3 (17, pros::E_MOTOR_GEARSET_18, true, pros::E_MOTOR_ENCODER_ROTATIONS);
//Right//
pros::Motor Right1 (15, pros::E_MOTOR_GEARSET_18, false, pros::E_MOTOR_ENCODER_ROTATIONS);
pros::Motor Right2 (4, pros::E_MOTOR_GEARSET_18, false, pros::E_MOTOR_ENCODER_ROTATIONS);
pros::Motor Right3 (11, pros::E_MOTOR_GEARSET_18, false, pros::E_MOTOR_ENCODER_ROTATIONS);

//********************************************//
//                   PID                      //
//********************************************//

//PID constantes
double kP = 0.00;
double kI = 0.00;
double kD = 0.00;

double turn_kP = 0.00;
double turn_kI = 0.00;
double turn_kD = 0.00;

// autonomous settings
int desiredvalue = 200;
int desiredturnvalue = 0;

int error;
int preverror = 0;
int derivitive;
int totalerror =  0;

int turnerror;
int turnpreverror = 0;
int turnderivitive;
int turntotalerror =  0;

bool resetdrivesensors = false;

//varibles modified for use 
bool enabledrivePID=true;

int drivePID(){

	while(enabledrivePID){
		if (resetdrivesensors){
          resetdrivesensors = false;

          left1.set_zero_position(0);
		  left2.set_zero_position(0);
		  left3.set_zero_position(0);
          Right1.set_zero_position(0);
		  Right2.set_zero_position(0);
		  Right3.set_zero_position(0);
      	}
	
		int left1motorposition = left1.get_position();
		int left2motorposition = left2.get_position();
		int left3motorposition = left3.get_position();
		int right1motorposition = Right1.get_position();
		int right2motorposition = Right2.get_position();
		int right3motorposition = Right3.get_position();

		///// lateral movement pid
	
		// get the average of the motors
		int averageposition = (left1motorposition + left2motorposition + left3motorposition + right1motorposition + right2motorposition + right3motorposition)/6;

		//potential
		error = averageposition - desiredvalue;

		//derivitive
		derivitive = error - preverror;

		//integral
		totalerror += error;

		int lateralmotorpower = error * kP + derivitive * kD + totalerror * kI;

		//turning movement pid

		// get the average of the motors
		int turndifference = left1motorposition + left2motorposition + left3motorposition + right1motorposition + right2motorposition + right3motorposition;

		//potential
		turnerror = turndifference - desiredturnvalue;

		//derivitive
		turnderivitive = turnerror - turnpreverror;

		//integral
		turntotalerror += turnerror;

		int turnmotorpower = turnerror * turn_kP + turnderivitive * turn_kD + turntotalerror * turn_kI;      

		
		left1.move_absolute(turnmotorpower, turnmotorpower);
		left2.move_absolute(turnmotorpower, turnmotorpower);
		left3.move_absolute(turnmotorpower, turnmotorpower);
		Right1.move_absolute(turnmotorpower, turnmotorpower);
		Right2.move_absolute(turnmotorpower, turnmotorpower);
		Right3.move_absolute(turnmotorpower, turnmotorpower);

		left1.move_absolute(lateralmotorpower, lateralmotorpower);
		left2.move_absolute(lateralmotorpower, lateralmotorpower);
		left3.move_absolute(lateralmotorpower, lateralmotorpower);
		Right1.move_absolute(lateralmotorpower, lateralmotorpower);
		Right2.move_absolute(lateralmotorpower, lateralmotorpower);
		Right3.move_absolute(lateralmotorpower, lateralmotorpower);
		
		preverror = error;
		turnpreverror = turnerror;
		
		pros::delay(20);
    }

  return 1;
}
#2

Look at these lines in particular:

double kP = 0.00;
double kI = 0.00;
double kD = 0.00;

int lateralmotorpower = error * kP + derivitive * kD + totalerror * kI;

Think about what’s happening with the math here; pretend error is an arbitrary value like 5, derivative and totalerror are 1. What would the output be if you did the math by hand? The answer is staring you in the face right there. :smiley:

1 Like
#3

Perhaps it has something to do with the fact that all of your constants are set to 0? If the error, change in error, and total error all have 0 effect on the motor power, why would your motor ever move?
Additionally, I’m not sure why you are using motor.move_absolute? That function is for making your motor move a specific distance. You don’t need that since the whole point of PID is moving a specific distance more accurately, so you probably want to use move_voltage instead.

3 Likes
#4

Okey thank you very much I will make the changes you told me. Also something that I had not noticed How do I execute the function in my void of autonomy?

#5

assuming you’re asking how to execute the drivePID function inside the autonomous function, you’d do this:

void autonomous() {
    drivePID();
}

But, that’d be blocking, so nothing could happen at the same time as your pid (which would last forever because there’s no exit condition)

The approach that is probably closer to what you want is spawning a task with the DrivePID function. This is how you’d do that:


void autonomous() {
    pros::Task pidTask(drivePID);
    // can do other stuff at the same time as drivePID
}
1 Like