使用PID控制底盤前進

方法1：使用PROS API:

pros::Motor left_motor(1);
pros::Motor right_motor(-2);

float wheel_diameter = 0.1; // 10 cm

// PID constants
float Kp = 1;
float Ki = 0.01;
float Kd = 0.1;

/**
 * @brief Move the robot with a specified distance
 * 
 * @param distance forward distance in meters
 */
void move_distance(float distance) {
	// PID stored errors
	float total_error = 0;
	float prev_error = 0;
	
	// calculate the angle needed to rotate
	float revs = wheel_diameter / (M_PI * wheel_diameter);
	float actual_angle = (left_motor.get_position() + right_motor.get_position()) / 2;
	float target_angle = revs * 360 + actual_angle;
	
	prev_error = target_angle - actual_angle;
	
	// while not arriving the destination
	while (abs(target_angle - actual_angle) >= 10) {
		float actual_angle = (left_motor.get_position() + right_motor.get_position()) / 2;
    		float error = actual_angle - target_angle;
    		total_error += error;
    		float deriv_error = error - prev_error;
    
    		float output = Kp * error + Ki * total_error + Kd * deriv_error;
    		left_motor.move_voltage(output);
    		right_motor.move_voltage(output);
    		prev_error = error;
	}  
}

void opcontrol() {

	while (true) {
		move_distance(2);
		pros::delay(10);
	}
}

方法2：使用Okapilib:

okapi::Motor left_motor(1);
okapi::Motor right_motor(-2);

float wheel_diameter = 0.1; // 10 cm

// PID constants
float Kp = 1;
float Ki = 0.01;
float Kd = 0.1;

/**
 * @brief Move the robot with a specified distance
 * 
 * @param distance forward distance in meters
 */
void move_distance(float distance) {
	// PID stored errors
	float total_error = 0;
	float prev_error = 0;
	
	// calculate the angle needed to rotate
	float revs = wheel_diameter / (M_PI * wheel_diameter);
	float actual_angle = (left_motor.getPosition() + right_motor.getPosition()) / 2;
	float target_angle = revs * 360 + actual_angle;
	
	prev_error = target_angle - actual_angle;
	
	// while not arriving the destination
	while (abs(target_angle - actual_angle) >= 10) {
		float actual_angle = (left_motor.getPosition() + right_motor.getPosition()) / 2;
    		float error = actual_angle - target_angle;
    		total_error += error;
    		float deriv_error = error - prev_error;
    
    		float output = Kp * error + Ki * total_error + Kd * deriv_error;
    		left_motor.moveVoltage(output);
    		right_motor.moveVoltage(output);
    		prev_error = error;
	}  
}

void opcontrol() {

	while (true) {
		move_distance(2);
		pros::delay(10);
	}
}