Trouble With PID Help

[Micah_7157X]

using namespace vex;

// A global instance of competition
competition Competition;

// define your global instances of motors and other devices here

extern motor LeftMotor;
extern motor RightMotor;

extern drivetrain Drivetrain ;

extern controller Controller1 ;




void pre_auton(void) {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

// Settings
double kP = 0.0; // Error
double kI = 0.0; // Last, Increase Precision
double kD = 0.0; // To Fast or Too Slow

double turnkP = 0.0 ;
double turnkI = 0.0;
double turnkD = 0.0;

// Auton Settings
int desiredValue = 200;
int desiredTurnValue = 0; 

int error = 0; // SensorValue - DesireValue : Positional Value 
int previouserror = 0; // Position 20 miliseconds ago
int derivative = 0; // error - previouserror : Speed
int totalerror= 0; // totalerror = totalerror + error

int turnerror; // SensorValue - DesireValue : Positional Value 
int turnpreviouserror = 0; // Position 20 miliseconds ago
int turnderivative; // error - previouserror : Speed
int turntotalerror= 0; // totalerror = totalerror + error

bool resetDriveSensors = false;

// Variation modified for use
bool enableDrivePID = true;

int drivePID(){

  while(enableDrivePID){

    if (resetDriveSensors) {
      resetDriveSensors = false;
      LeftMotor.setPosition(0,degrees);
      RightMotor.setPosition(0,degrees);
    }


    // Get Position of Both Motors
    int leftMotorPosition = LeftMotor.position(degrees);
    int rightMotorPosition = RightMotor.position(degrees);

    //////////////////////////////////////////////////////////////////////////
    //Lateral Movement PID
    //////////////////////////////////////////////////////////////////////////

    // Average Of the Motors
    int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

    // Potential
    error = averagePosition - desiredValue;

    // Derivative
    derivative = error - previouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    totalerror += error;

    double lateralMotorPower = (error * kP + derivative * kD + totalerror *kI);
    ////////////////////////////////////////////////////////////////////////

    //////////////////////////////////////////////////
    //Turning movement PID
    //////////////////////////////////////////////////

    // Average Of the Motors
    int turnDifference = leftMotorPosition - rightMotorPosition;

    // Potential
    turnerror = turnDifference - desiredTurnValue;

    // Derivative
    turnderivative = turnerror - turnpreviouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    turntotalerror += turnerror;

    double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI);

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

    LeftMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);



    //code
    previouserror = error;
    turnpreviouserror = turnerror;
    vex::task::sleep(20); 

  }



  return 1; 
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void autonomous(void) {

  vex::task whynot(drivePID);

  desiredValue = 300; 
  desiredTurnValue = 600;

  vex::task::sleep(1000);

  resetDriveSensors = true;
  desiredValue = 300;
  desiredTurnValue = 600;

  




  


}

void usercontrol(void) {
  // User control code here, inside the loop
  while (1) {
    
    enableDrivePID = false ;

   

    wait(20, msec); // Sleep the task for a short amount of time to
                    // prevent wasted resources.
  }
}

//
// Main will set up the competition functions and callbacks.
//
int main() {
  // Set up callbacks for autonomous and driver control periods.
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // Run the pre-autonomous function.
  pre_auton();

  // Prevent main from exiting with an infinite loop.
  while (true) {
    wait(100, msec);
  }
}



  return 1; 
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void autonomous(void) {

  task whynot(drivePID);

  resetDriveSensors = true;
  desiredValue = 300; 
  desiredTurnValue = 600;

  vex::task::sleep(1000);
  resetDriveSensors = true;
  desiredValue = 300;
  desiredTurnValue = 600;




  


}

void usercontrol(void) {
  // User control code here, inside the loop
  while (1) {
    
    enableDrivePID = false ;

   

    wait(20, msec); // Sleep the task for a short amount of time to
                    // prevent wasted resources.
  }
}

[trontech569]

First of all, what problems are you facing? What does the code do now?
Second, it really helps to read code when it is in [code] ... [/code] tags.
Third, I feel like guessing the problem. If the code does nothing, then I might have an idea why. Since PID multiplies each element by its k value, you might want to set the k values to something other than 0 to test. Otherwise, you will likely get no result, with the motors not moving.

[Connor]

By merely pasting your code @Micah_7157X you are causing the system to be confused as it’s trying to identify what you are doing. Please use [code] and [/code] to make it a more readable format.

[Micah_7157X]

The code does nothing…

[Micah_7157X]

using namespace vex;

// A global instance of competition
competition Competition;

// define your global instances of motors and other devices here

extern motor LeftMotor;
extern motor RightMotor;

extern drivetrain Drivetrain ;

extern controller Controller1 ;

void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();

// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, …
}

// Settings
double kP = 0.0; // Error
double kI = 0.0; // Last, Increase Precision
double kD = 0.0; // To Fast or Too Slow

double turnkP = 0.0 ;
double turnkI = 0.0;
double turnkD = 0.0;

// Auton Settings
int desiredValue = 200;
int desiredTurnValue = 0;

int error = 0; // SensorValue - DesireValue : Positional Value
int previouserror = 0; // Position 20 miliseconds ago
int derivative = 0; // error - previouserror : Speed
int totalerror= 0; // totalerror = totalerror + error

int turnerror; // SensorValue - DesireValue : Positional Value
int turnpreviouserror = 0; // Position 20 miliseconds ago
int turnderivative; // error - previouserror : Speed
int turntotalerror= 0; // totalerror = totalerror + error

bool resetDriveSensors = false;

// Variation modified for use
bool enableDrivePID = true;

int drivePID(){

while(enableDrivePID){

if (resetDriveSensors) {
  resetDriveSensors = false;
  LeftMotor.setPosition(0,degrees);
  RightMotor.setPosition(0,degrees);
}


// Get Position of Both Motors
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);

//////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
//////////////////////////////////////////////////////////////////////////

// Average Of the Motors
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

// Potential
error = averagePosition - desiredValue;

// Derivative
derivative = error - previouserror;

// Integral : Velocity -> Position -> Absenent ( Position x Time )
totalerror += error;

double lateralMotorPower = (error * kP + derivative * kD + totalerror *kI);
////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////
//Turning movement PID
//////////////////////////////////////////////////

// Average Of the Motors
int turnDifference = leftMotorPosition - rightMotorPosition;

// Potential
turnerror = turnDifference - desiredTurnValue;

// Derivative
turnderivative = turnerror - turnpreviouserror;

// Integral : Velocity -> Position -> Absenent ( Position x Time )
turntotalerror += turnerror;

double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI);

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

LeftMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);



//code
previouserror = error;
turnpreviouserror = turnerror;
vex::task::sleep(20); 
}

return 1;
}

/---------------------------------------------------------------------------/
/* /
/ Autonomous Task /
/ /
/ This task is used to control your robot during the autonomous phase of /
/ a VEX Competition. /
/ /
/ You must modify the code to add your own robot specific commands here. /
/---------------------------------------------------------------------------*/

void autonomous(void) {

vex::task whynot(drivePID);

desiredValue = 300;
desiredTurnValue = 600;

vex::task::sleep(1000);

resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;

}

void usercontrol(void) {
// User control code here, inside the loop
while (1) {

enableDrivePID = false ;



wait(20, msec); // Sleep the task for a short amount of time to
                // prevent wasted resources.
}
}

//
// Main will set up the competition functions and callbacks.
//
int main() {
// Set up callbacks for autonomous and driver control periods.
Competition.autonomous(autonomous);
Competition.drivercontrol(usercontrol);

// Run the pre-autonomous function.
pre_auton();

// Prevent main from exiting with an infinite loop.
while (true) {
wait(100, msec);
}
}

return 1;
}

/---------------------------------------------------------------------------/
/* /
/ Autonomous Task /
/ /
/ This task is used to control your robot during the autonomous phase of /
/ a VEX Competition. /
/ /
/ You must modify the code to add your own robot specific commands here. /
/---------------------------------------------------------------------------*/

void autonomous(void) {

task whynot(drivePID);

resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;

vex::task::sleep(1000);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;

}

void usercontrol(void) {
// User control code here, inside the loop
while (1) {

enableDrivePID = false ;



wait(20, msec); // Sleep the task for a short amount of time to
                // prevent wasted resources.
}
}```

[Micah_7157X]

nevermind now it just doesnt stop once it reaches driver control

[Connor]

Your ki, kp, and kd values are all 0. Those values need to be tuned in order to actually make your robot move.

[rpm]

Look here for some critics by @Coded.

PID Question

int desiredValue = 200; It represents the target value so how far you want the robot to drive. Set it to the desired distance (in your case measured in motor degrees since that is what you use). PID simply takes in sensor values and translates them to motor power. If you add tracking wheels in the same general orientation, then it is as simple as using those values instead (you will have to adjust the PID constants). One error I did notice was… LeftMotor.spin(forward, lateralMotorPower +…

I saw the turnMotorPower issues yesterday and thought I responded but I did not hit replay. So I canceled it and linked to his post in the other thread in case you did not see it.