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;
}