PID code not running

Programming Support
1

I have recently started coding our robot in pros, but whenever we try to run our autonomous code, it does not do anything and goes directly to driver control, which works fine. Code:

#include "main.h"
#define IMU_PORT 7
#define VISION_PORT 11


// Make PID move using inches and degrees

// using namespace pros;

pros::Controller master(pros::E_CONTROLLER_MASTER);
pros::Motor left_mtrB(1, pros::E_MOTOR_GEARSET_18, true, pros::E_MOTOR_ENCODER_DEGREES); //Defines motors
pros::Motor left_mtrM(2, pros::E_MOTOR_GEARSET_18,true, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor left_mtrF(3, pros::E_MOTOR_GEARSET_18,true, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor right_mtrB(4, pros::E_MOTOR_GEARSET_18,false, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor right_mtrM(5, pros::E_MOTOR_GEARSET_18,false, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor right_mtrF(6, pros::E_MOTOR_GEARSET_18,false, pros::E_MOTOR_ENCODER_DEGREES);

// pros::Motor intake(8, pros::E_MOTOR_GEARSET_18, true, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor cataL(7, pros::E_MOTOR_GEARSET_18, false, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor cataR(8, pros::E_MOTOR_GEARSET_18, true, pros::E_MOTOR_ENCODER_DEGREES);

pros::ADIDigitalOut WingLeft (9);
pros::ADIDigitalOut WingRight (10);
//name.setValue(true);

pros::Motor_Group leftDrive ({left_mtrF, left_mtrM, left_mtrB}); //Creating groups for easier access
pros::Motor_Group rightDrive ({right_mtrF, right_mtrM, right_mtrB});
pros::Motor_Group cataGroup ({cataL, cataR});

// pros::Imu imu_sensor(IMU_PORT);
// pros::Vision vision_sensor (VISION_PORT);
// imu_sensor.reset();

/**
 * A callback function for LLEMU's center button.
 *
 * When this callback is fired, it will toggle line 2 of the LCD text between
 * "I was pressed!" and nothing.
 */


void on_center_button() {
	static bool pressed = false;
	pressed = !pressed;
	if (pressed) {
		pros::lcd::set_text(2, "I was pressed!");
	} else {
		pros::lcd::clear_line(2);
	}
}

/**
 * Runs initialization code. This occurs as soon as the program is started.
 *
 * All other competition modes are blocked by initialize; it is recommended
 * to keep execution time for this mode under a few seconds.
 */
void initialize() {
	pros::lcd::initialize();
	pros::lcd::set_text(1, "Hello PROS User!"); //Know if it starts

	pros::lcd::register_btn1_cb(on_center_button);
}

/**
 * Runs while the robot is in the disabled state of Field Management System or
 * the VEX Competition Switch, following either autonomous or opcontrol. When
 * the robot is enabled, this task will exit.
 */
void disabled() {}

/**
 * Runs after initialize(), and before autonomous when connected to the Field
 * Management System or the VEX Competition Switch. This is intended for
 * competition-specific initialization routines, such as an autonomous selector
 * on the LCD.
 *
 * This task will exit when the robot is enabled and autonomous or opcontrol
 * starts.
 */
void competition_initialize() {}


////////////////// PID ////////////////////////

double kP = 0.90; 
double kI = 0.0001;
double kD = 0.01;

double turn_kP = 0.90; //turning constants
double turn_kI = 0.0000001;
double turn_kD = 0.01;

int desiredValue = 100; //how far you want to go
int turnDesiredValue = 0;

int maxTurnIntegral = 300; // These cap the integrals
int maxIntegral = 300;
int integralBound = 3; //If error is outside the bounds, then apply the integral. This is a buffer with +-integralBound degrees

int error;
int prevError;
int derivative;
int totalError;
// int targetPosition = 100;

int turn_error;
int turn_prevError;
int turn_derivative;
int turn_totalError;
// int turn_targetPosition;

bool enablePID = true;
bool resetDriveSensors = false;

double signnum_c(double x) {
  if (x > 0.0) return 1.0;
  if (x < 0.0) return -1.0;
  return x;
}

int drivePID (){
	while (enablePID) {

		if (resetDriveSensors) {
			resetDriveSensors = false;
			left_mtrB.tare_position();
			left_mtrM.tare_position();
			left_mtrF.tare_position();
			right_mtrB.tare_position();
			right_mtrM.tare_position();
			right_mtrF.tare_position();
		}

		int leftFPos = left_mtrF.get_position();
		int leftMPos = left_mtrM.get_position();
		int leftBPos = left_mtrB.get_position();
		int rightFPos = right_mtrF.get_position();
		int rightMPos = right_mtrM.get_position();
		int rightBPos = right_mtrB.get_position();

		int averagePosition = ((leftFPos + leftMPos + leftBPos + rightFPos + rightMPos + rightBPos) / 6);

		////////////////////////////////////////////////////////////////////////////////////////
		
		//Potential
		// switch position and desired value if it keeps moving
		error = ((averagePosition - desiredValue));// * abs(averagePosition - desiredValue));

		// Derivative
		derivative = error - prevError;

		// Integral
		if(abs(turn_error) < integralBound){
			turn_totalError+=turn_error; }
		else {
		turn_totalError = 0; }

		if (abs(turn_totalError) > maxTurnIntegral) {
			turn_totalError = signnum_c(turn_totalError) * maxTurnIntegral;
		}

		// else {}
		

		// turn_totalError += turn_error;

		double lateralMotion = error * kP + derivative * kD + totalError * kI;

		////////////////////////////////////////////////////////////////////////////////////////

		int turnDifference = (((leftFPos + leftMPos + leftBPos) - (rightFPos + rightMPos + rightBPos)) / 3);

		// switch difference and desired value if it keeps spinning
		turn_error = ((turnDifference - turnDesiredValue));// * (abs(turnDifference - turnDesiredValue)));

		turn_derivative = turn_error - turn_prevError;

		turn_totalError += turn_error;

		if(abs(turn_error) < integralBound){
			totalError+=error; }
		else {
		totalError = 0; }

		if (abs(totalError) > maxIntegral) {
			totalError = signnum_c(totalError) * maxIntegral;
		}

		double turnMotion = turn_error * turn_kP + turn_derivative * turn_kD + turn_totalError * turn_kI;

		////////////////////////////////////////////////////////////////////////////////////////

		leftDrive.move_voltage((lateralMotion + turnMotion) * 1000);
		rightDrive.move_voltage((lateralMotion - turnMotion) * 1000);

		prevError = error;
		turn_prevError = turn_error;
		pros::delay(20);
	}
	return 1;
}

void autonomous() {
	pros::delay(20000);
	enablePID = true;

	pros::Task ThisIsTheDrivePid(drivePID);

	resetDriveSensors = true;
	desiredValue = 300;
	turnDesiredValue = 600;

	pros::Task::delay(500);
	pros::delay(200);

	resetDriveSensors = true;
	desiredValue = 300;
	turnDesiredValue = 600;

	pros::Task::delay(500);
	pros::delay(200);

	resetDriveSensors = true;
	desiredValue = 0;
	turnDesiredValue = 600;

	pros::Task::delay(500);
	pros::delay(200);

	resetDriveSensors = true;
	desiredValue = 300;
	turnDesiredValue = 0;
}
	

	/**
	 * Runs the user autonomous code. This function will be started in its own task
	 * with the default priority and stack size whenever the robot is enabled via
	 * the Field Management System or the VEX Competition Switch in the autonomous
	 * mode. Alternatively, this function may be called in initialize or opcontrol
	 * for non-competition testing purposes.
	 *
	 * If the robot is disabled or communications is lost, the autonomous task
	 * will be stopped. Re-enabling the robot will restart the task, not re-start it
	 * from where it left off.
	 */



	/**
	* Runs the operator control code. This function will be started in its own task
	* with the default priority and stack size whenever the robot is enabled via
	* the Field Management System or the VEX Competition Switch in the operator
	* control mode.
	*
	* If no competition control is connected, this function will run immediately
	* following initialize().
	*
	* If the robot is disabled or communications is lost, the
	* operator control task will be stopped. Re-enabling the robot will restart the
	* task, not resume it from where it left off.
	*/

void opcontrol() {
	enablePID = false;

	bool cata_on = false;

	while (true) {

		int turn_importance = 1; //change to make turns faster or slower
		int power = master.get_analog(ANALOG_LEFT_Y); //Values from controller can be from -127 to 127
    	int turn = (master.get_analog(ANALOG_RIGHT_X) * turn_importance);


		int left = power+turn;
		int right = power-turn;

		leftDrive.move(left); //moving based on their power, it goes from -127 to 127, 
		rightDrive.move(right);

		if (master.get_digital(DIGITAL_R1)) {
			cataGroup.move_velocity(200);
		}
		else if (master.get_digital(DIGITAL_R2)) {
			cataGroup.move_velocity(-200);
		}
		else {
			cataGroup.move_velocity(0);
		}

		if (master.get_digital(DIGITAL_L1)) { // check if cata is on, and one button controls it
			if (cata_on = true){
				cata_on = false;
			}
			else if (cata_on = false){
				cata_on = true;
			}
		}

		if (cata_on = true) {
			cataGroup.move_velocity(200); //move cata, turn on
		}
		else{
			cataGroup.move_velocity(0); //turn off cata
		}


		pros::delay(10);
	}
}

I would also appreciate if someone could help me turn my units of movement from motor degrees to inches, and my turning to degrees. Our drivetrain gearing ratio is 36x48.

2

How are you running the code? Are you going to competition and then programming skills? Another thing I would recommend is switching the motor encoder, average, and error calculations to use floats or doubles instead of ints. I would also recommend using a function, as I believe that is a simpler way to use PID.

For the conversion from degrees to inches I am going to explain without a gear ratio and you can figure out the math and add the gear ratio yourself (so you can learn).

To start with the conversion you find out how many degrees correlate with how many inches. The simplest ways to do this is to consider one full rotation (360 degrees). When the wheels rotates once, the robot travels one wheel circumference. One wheel circumference can be calculated using the wheel diameter and the formula, circumference= πdiameter. The conversion from inches to degrees would then be degrees = inches(360 degrees/(π*wheel diameter)).

taking into account gear ratio

In order to take into account the gear ratio you need to take into account the motor spinning less for every full turn of the motor. If your output is spinning 1.333 times per every rotation of the input then for every 270 degrees (360/1.33333) then output gear is spinning 360 degrees. You would then take this into account in the conversion.

The other way is to edit the wheel travel distance to be 1.3333 times more than wheel circumference times diameter

I’m not sure how well I explained, it so feel free to ask me any additional questions

2 Likes
3

Thank you for your reply, to run the code all I was doing was once I downloaded it to the brain, I was just clicking the Run button. Is there anything else that I need to do to get autonomous running?

This is what my header file looks like:

image
image467×876 17.7 KB

It should run everything, but I am not sure.

I would also recommend using a function, as I believe that is a simpler way to use PID.

How would you suggest using a function? If I understand correctly, I defined a function that moves the robot and just called it in autonomous. This is my code:

#include "main.h"
#define IMU_PORT 7
#define VISION_PORT 11


// Make PID move using inches and degrees

// using namespace pros;

pros::Controller master(pros::E_CONTROLLER_MASTER);
pros::Motor left_mtrB(1, pros::E_MOTOR_GEARSET_18, true, pros::E_MOTOR_ENCODER_DEGREES); //Defines motors
pros::Motor left_mtrM(2, pros::E_MOTOR_GEARSET_18,true, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor left_mtrF(3, pros::E_MOTOR_GEARSET_18,true, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor right_mtrB(4, pros::E_MOTOR_GEARSET_18,false, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor right_mtrM(5, pros::E_MOTOR_GEARSET_18,false, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor right_mtrF(6, pros::E_MOTOR_GEARSET_18,false, pros::E_MOTOR_ENCODER_DEGREES);

// pros::Motor intake(8, pros::E_MOTOR_GEARSET_18, true, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor cataL(7, pros::E_MOTOR_GEARSET_18, false, pros::E_MOTOR_ENCODER_DEGREES);
pros::Motor cataR(8, pros::E_MOTOR_GEARSET_18, true, pros::E_MOTOR_ENCODER_DEGREES);

pros::ADIDigitalOut WingLeft (9);
pros::ADIDigitalOut WingRight (10);
//name.setValue(true);

pros::Motor_Group leftDrive ({left_mtrF, left_mtrM, left_mtrB}); //Creating groups for easier access
pros::Motor_Group rightDrive ({right_mtrF, right_mtrM, right_mtrB});
pros::Motor_Group cataGroup ({cataL, cataR});

pros::Imu imu_sensor(IMU_PORT);
pros::Vision vision_sensor (VISION_PORT);
// imu_sensor.reset();

/**
 * A callback function for LLEMU's center button.
 *
 * When this callback is fired, it will toggle line 2 of the LCD text between
 * "I was pressed!" and nothing.
 */


void on_center_button() {
	static bool pressed = false;
	pressed = !pressed;
	if (pressed) {
		pros::lcd::set_text(2, "I was pressed!");
	} else {
		pros::lcd::clear_line(2);
	}
}

/**
 * Runs initialization code. This occurs as soon as the program is started.
 *
 * All other competition modes are blocked by initialize; it is recommended
 * to keep execution time for this mode under a few seconds.
 */
void initialize() {
	pros::lcd::initialize();
	pros::lcd::set_text(1, "Hello 938F. Look behind you... or else!"); //Know if it starts

	pros::lcd::register_btn1_cb(on_center_button);
}

/**
 * Runs while the robot is in the disabled state of Field Management System or
 * the VEX Competition Switch, following either autonomous or opcontrol. When
 * the robot is enabled, this task will exit.
 */
void disabled() {}

/**
 * Runs after initialize(), and before autonomous when connected to the Field
 * Management System or the VEX Competition Switch. This is intended for
 * competition-specific initialization routines, such as an autonomous selector
 * on the LCD.
 *
 * This task will exit when the robot is enabled and autonomous or opcontrol
 * starts.
 */
void competition_initialize() {}

double signnum_c(double x) {
	if (x > 0.0) {
		return 1.0; }
	else if (x < 0.0) {
		return -1.0;
	}
	return x;
}

bool enablePID = false;

////////////////// PID ////////////////////////
void PID(int moveVal, int turnVal, bool reset) {
	double kP = 0.90; //First tune kP until there are minor movements back/forward, then tune Kd till they stop, and then add Ki until it looks good, ki is optional
	double kI = 0.0001;
	double kD = 0.01;

	double turn_kP = 0.90; //turning constants
	double turn_kI = 0.0000001;
	double turn_kD = 0.01;

	int desiredValue = moveVal; //how far you want to go
	int turnDesiredValue = turnVal;

	int maxTurnIntegral = 300; // These cap the integrals
	int maxIntegral = 300;
	int integralBound = 3; //If error is outside the bounds, then apply the integral. This is a buffer with +-integralBound degrees

	int error;
	int prevError;
	int derivative;
	int totalError;
	// int targetPosition = 100;

	int turn_error;
	int turn_prevError;
	int turn_derivative;
	int turn_totalError;
	// int turn_targetPosition;

	enablePID = true;
	bool resetDriveSensors = reset;

	while (enablePID) {

		if (resetDriveSensors) {
			resetDriveSensors = false;
			left_mtrB.tare_position();
			left_mtrM.tare_position();
			left_mtrF.tare_position();
			right_mtrB.tare_position();
			right_mtrM.tare_position();
			right_mtrF.tare_position();
		}

		int leftFPos = left_mtrF.get_position();
		int leftMPos = left_mtrM.get_position();
		int leftBPos = left_mtrB.get_position();
		int rightFPos = right_mtrF.get_position();
		int rightMPos = right_mtrM.get_position();
		int rightBPos = right_mtrB.get_position();

		int averagePosition = ((leftFPos + leftMPos + leftBPos + rightFPos + rightMPos + rightBPos) / 6);

		////////////////////////////////////////////////////////////////////////////////////////
		
		//Potential
		// switch position and desired value if it keeps moving
		error = ((averagePosition - desiredValue));// * abs(averagePosition - desiredValue));

		// Derivative
		derivative = error - prevError;

		// Integral
		if(abs(turn_error) < integralBound){
			turn_totalError+=turn_error; }
		else {
		turn_totalError = 0; }

		if (abs(turn_totalError) > maxTurnIntegral) {
			turn_totalError = signnum_c(turn_totalError) * maxTurnIntegral;
		}

		// else {}
		

		// turn_totalError += turn_error;

		double lateralMotion = error * kP + derivative * kD + totalError * kI;

		////////////////////////////////////////////////////////////////////////////////////////

		int turnDifference = (((leftFPos + leftMPos + leftBPos) - (rightFPos + rightMPos + rightBPos)) / 3);

		// switch difference and desired value if it keeps spinning
		turn_error = ((turnDifference - turnDesiredValue));// * (abs(turnDifference - turnDesiredValue)));

		turn_derivative = turn_error - turn_prevError;

		turn_totalError += turn_error;

		if(abs(turn_error) < integralBound){
			totalError+=error; }
		else {
		totalError = 0; }

		if (abs(totalError) > maxIntegral) {
			totalError = signnum_c(totalError) * maxIntegral;
		}

		double turnMotion = turn_error * turn_kP + turn_derivative * turn_kD + turn_totalError * turn_kI;

		////////////////////////////////////////////////////////////////////////////////////////

		leftDrive.move_voltage((lateralMotion + turnMotion) * 1000);
		rightDrive.move_voltage((lateralMotion - turnMotion) * 1000);

		prevError = error;
		turn_prevError = turn_error;

		// if (((|turnMotion| + |lateralMotion|) * 1000) < 100)

		pros::delay(20);
	}
}

void autonomous() {
	pros::delay(20000);
	enablePID = true;

	// // pros::Task ThisIsTheDrivePid(drivePID);

	// // resetDriveSensors = true;
	// // desiredValue = 300;
	// // turnDesiredValue = 600;

	// pros::Task::delay(500);
	// pros::delay(200);

	// // resetDriveSensors = true;
	// // desiredValue = 300;
	// // turnDesiredValue = 600;

	// pros::Task::delay(500);
	// pros::delay(200);

	// // resetDriveSensors = true;
	// // desiredValue = 0;
	// // turnDesiredValue = 600;

	// pros::Task::delay(500);
	// pros::delay(200);

	// // resetDriveSensors = true;
	// // desiredValue = 300;
	// // turnDesiredValue = 0;

	PID(300, 600, true);

	pros::delay(200);

	PID(300, 0, true);

	pros::delay(200);

	PID(0, 600, true);

	pros::delay(200);
}

But I am having an issue with this, as my entire PID code is an infinite while loop until PID gets turned off, but I cannot turn it off if the error is minimal, as if it overshoots, it will not oscillate and correct itself. Do you have any ways that I can use a function without these issues? Otherwise, I think a task would be easier and more beneficial, so could you help me fix my code with tasks?

For turning my code from motor encoder degrees to inches, are you suggesting measuring the diameter and figuring out how many inches that is? After that, I do not understand how to convert into degrees, especially with our geared drivertain. Thanks.

4

@EcstaticPilot Do you know if it is an issue with my code, or with uploading itself?

5

It is likely you are not running it correctly but there’s also a possibility there’s something wrong with your code. Here’s a picture on how to run it:

SelectWithRemote
SelectWithRemote1000×539 306 KB

Make sure to run the autonomous code

6

Thanks, I will try this next time - hopefully it works.