New to PID help

we are trying PID for the first time and we followed a youtube tutorial by Connor 1814D and our robot will not move for whatever reason, any help or sources would be very useful

here is the code:

/----------------------------------------------------------------------------/
/* /
/
Module: main.cpp /
/
Author: VEX /
/
Created: Thu Sep 26 2019 /
/
Description: Competition Template /
/
/
/
----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// ---- END VEXCODE CONFIGURED DEVICES ----

#include “vex.h”

using namespace vex;

vex::motor BLeftMotor = vex::motor( vex::PORT8,true );
vex::motor FLeftMotor = vex::motor( vex::PORT16,true );
vex::motor BRightMotor = vex::motor( vex::PORT2,true );
vex::motor FRightMotor = vex::motor( vex::PORT11,true );

vex::motor IntakeR = vex::motor( vex::PORT20,true );
vex::motor IntakeL = vex::motor( vex::PORT17,true );
vex::motor TopBoi = vex::motor( vex::PORT19,true );

vex::motor BallCarry = vex::motor( vex::PORT12,true );
vex::motor Scorer = vex::motor( vex::PORT4,true );

//vex::limit ClickyBoi =vex::limit( Brain.ThreeWirePort.A);

vex::controller Controller = vex::controller();

// A global instance of competition
competition Competition;

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

/---------------------------------------------------------------------------/
/* Pre-Autonomous Functions /
/
/
/
You may want to perform some actions before the competition starts. /
/
Do them in the following function. You must return from this function /
/
or the autonomous and usercontrol tasks will not be started. This /
/
function is only called once after the V5 has been powered on and /
/
not every time that the robot is disabled. /
/
---------------------------------------------------------------------------*/

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, …
}
//PID
//////////////////////////////////////////////
//Constants
double kP = 0.00001;
double kI = 0;
double kD = 0;
double TurnkP = 0;
double TurnkI = 0;
double TurnkD = 0;

//AutonStoof
///////////////////////////////
int desiredValue = 300;
int desiredTurnValue = 0;

int error; //SensorValue - DesireValue : Position
int prevError = 0; //Position 20 milliseconds ago
int derivative; //error - prevError : Speed
int totalError = 0; //totalError = totalError + error

int Turnerror; //SensorValue - DesireValue : Position
int TurnprevError = 0; //Position 20 milliseconds ago
int Turnderivative; //error - prevError : Speed
int TurntotalError = 0; //totalError = totalError + error

bool resetDriveSensors = false;
//Varibles
//////////////////////////////////////

int drivePID(){

if (resetDriveSensors) {
resetDriveSensors = true;

BLeftMotor.setPosition(0, degrees);
BRightMotor.setPosition(0, degrees);
FLeftMotor.setPosition(0, degrees);
FRightMotor.setPosition(0, degrees);

}

//Get the position of both motors

int BleftMotorPosition = BLeftMotor.position(degrees);
int BrightMotorPosition = BRightMotor.position(degrees);
int FleftMotorPosition = FLeftMotor.position(degrees);
int FrightMotorPosition = FRightMotor.position(degrees);

///////////////////////////////////////////////
// Lateral movement PID
/////////////////////////////////////////////////////
//average of 4 motors
int averagePostion = (BleftMotorPosition + BrightMotorPosition + FleftMotorPosition + FrightMotorPosition );

//Potential
error = averagePostion - desiredValue;

//Derivative
derivative = error - prevError;

//Intengral
totalError += error;

double LateralMotorPower = (error * kP + derivative * kD + totalError * kI);

double TurnMotorPower = (Turnerror * TurnkP + Turnderivative * TurnkD + TurntotalError * TurnkI);

BLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
BRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);
FLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
FRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);

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

int TurnaveragePostion = (BleftMotorPosition + BrightMotorPosition + FleftMotorPosition + FrightMotorPosition );

//Potential
Turnerror = TurnaveragePostion - desiredTurnValue;

//Derivative
Turnderivative = Turnerror - TurnprevError;

//Intengral
TurntotalError += Turnerror;

BLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
BRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);
FLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
FRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);

//code
prevError = error;
TurnprevError = Turnerror;

vex::task::sleep(1);

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 Drive(drivePID);
  
  resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;

vex::task::sleep(3);

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

}

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

void usercontrol(void) {
while (1) {
// This is the main execution loop for the user control program.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.

// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................

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

I believe you need a while loop around your PID so it is constantly updating.

1 Like

we put it in a while loop and it still does nothing.

Here is the code:

/----------------------------------------------------------------------------/
/* /
/
Module: main.cpp /
/
Author: VEX /
/
Created: Thu Sep 26 2019 /
/
Description: Competition Template /
/
/
/
----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// ---- END VEXCODE CONFIGURED DEVICES ----

#include “vex.h”

using namespace vex;

vex::motor BLeftMotor = vex::motor( vex::PORT8,true );
vex::motor FLeftMotor = vex::motor( vex::PORT16,true );
vex::motor BRightMotor = vex::motor( vex::PORT2,true );
vex::motor FRightMotor = vex::motor( vex::PORT11,true );

vex::motor IntakeR = vex::motor( vex::PORT20,true );
vex::motor IntakeL = vex::motor( vex::PORT17,true );
vex::motor TopBoi = vex::motor( vex::PORT19,true );

vex::motor BallCarry = vex::motor( vex::PORT12,true );
vex::motor Scorer = vex::motor( vex::PORT4,true );

//vex::limit ClickyBoi =vex::limit( Brain.ThreeWirePort.A);

vex::controller Controller = vex::controller();

// A global instance of competition
competition Competition;

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

/---------------------------------------------------------------------------/
/* Pre-Autonomous Functions /
/
/
/
You may want to perform some actions before the competition starts. /
/
Do them in the following function. You must return from this function /
/
or the autonomous and usercontrol tasks will not be started. This /
/
function is only called once after the V5 has been powered on and /
/
not every time that the robot is disabled. /
/
---------------------------------------------------------------------------*/

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, …
}
//PID
//////////////////////////////////////////////
//Constants
double kP = 0.00001;
double kI = 0;
double kD = 0;
double TurnkP = 0;
double TurnkI = 0;
double TurnkD = 0;

//AutonStoof
///////////////////////////////
int desiredValue = 300;
int desiredTurnValue = 300;

int error; //SensorValue - DesireValue : Position
int prevError = 0; //Position 20 milliseconds ago
int derivative; //error - prevError : Speed
int totalError = 0; //totalError = totalError + error

int Turnerror; //SensorValue - DesireValue : Position
int TurnprevError = 0; //Position 20 milliseconds ago
int Turnderivative; //error - prevError : Speed
int TurntotalError = 0; //totalError = totalError + error

bool resetDriveSensors = false;
//Varibles
//////////////////////////////////////

bool enableDrivePID = true;

int drivePID(){

while(enableDrivePID){

if (resetDriveSensors) {
resetDriveSensors = true;

BLeftMotor.setPosition(0, degrees);
BRightMotor.setPosition(0, degrees);
FLeftMotor.setPosition(0, degrees);
FRightMotor.setPosition(0, degrees);

}

//Get the position of both motors

int BleftMotorPosition = BLeftMotor.position(vex::degrees);
int BrightMotorPosition = BRightMotor.position(vex::degrees);
int FleftMotorPosition = FLeftMotor.position(vex::degrees);
int FrightMotorPosition = FRightMotor.position(vex::degrees);

///////////////////////////////////////////////
// Lateral movement PID
/////////////////////////////////////////////////////
//average of 4 motors
int averagePostion = (BleftMotorPosition + BrightMotorPosition + FleftMotorPosition + FrightMotorPosition );

//Potential
error = averagePostion - desiredValue;

//Derivative
derivative = error - prevError;

//Intengral
totalError += error;

double LateralMotorPower = (error * kP + derivative * kD + totalError * kI);

double TurnMotorPower = (Turnerror * TurnkP + Turnderivative * TurnkD + TurntotalError * TurnkI);

BLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
BRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);
FLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
FRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);

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

int TurnaveragePostion = (BleftMotorPosition + BrightMotorPosition + FleftMotorPosition + FrightMotorPosition );

//Potential
Turnerror = TurnaveragePostion - desiredTurnValue;

//Derivative
Turnderivative = Turnerror - TurnprevError;

//Intengral
TurntotalError += Turnerror;

BLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
BRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);
FLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
FRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);

//code
prevError = error;
TurnprevError = Turnerror;

vex::task::sleep(1);

}

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) {

  enableDrivePID = true;
  


  resetDriveSensors = true;

desiredValue = 300;
desiredTurnValue = 600;

vex::task::sleep(3);

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

  }

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

void usercontrol(void) {
while (1) {
// This is the main execution loop for the user control program.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.

// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................

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

}

You can format your code by putting it in [code] ... [/code] brackets. Like so:

/*----------------------------------------------------------------------------* /
/*  /
/  Module: main.cpp  <em>/
/  Author: VEX  <em>/
/  Created: Thu Sep 26 2019  <em>/
/  Description: Competition Template  <em>/
/   /
/ ----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// ---- END VEXCODE CONFIGURED DEVICES ----

#include “vex.h”

using namespace vex;

vex::motor BLeftMotor = vex::motor( vex::PORT8,true );
vex::motor FLeftMotor = vex::motor( vex::PORT16,true );
vex::motor BRightMotor = vex::motor( vex::PORT2,true );
vex::motor FRightMotor = vex::motor( vex::PORT11,true );

vex::motor IntakeR = vex::motor( vex::PORT20,true );
vex::motor IntakeL = vex::motor( vex::PORT17,true );
vex::motor TopBoi = vex::motor( vex::PORT19,true );

vex::motor BallCarry = vex::motor( vex::PORT12,true );
vex::motor Scorer = vex::motor( vex::PORT4,true );

//vex::limit ClickyBoi =vex::limit( Brain.ThreeWirePort.A);

vex::controller Controller = vex::controller();

// A global instance of competition
competition Competition;

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

/ *---------------------------------------------------------------------------* /
/* Pre-Autonomous Functions                                                                         /
/                                                                                                                              /
/  You may want to perform some actions before the competition starts.  /
/  Do them in the following function. You must return from this function  /
/  or the autonomous and usercontrol tasks will not be started. This  /
/  function is only called once after the V5 has been powered on and  /
/  not every time that the robot is disabled.                                         /
/ ---------------------------------------------------------------------------*/

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, …
}
//PID
//////////////////////////////////////////////
//Constants
double kP = 0.00001;
double kI = 0;
double kD = 0;
double TurnkP = 0;
double TurnkI = 0;
double TurnkD = 0;

//AutonStoof
///////////////////////////////
int desiredValue = 300;
int desiredTurnValue = 300;

int error; //SensorValue - DesireValue : Position
int prevError = 0; //Position 20 milliseconds ago
int derivative; //error - prevError : Speed
int totalError = 0; //totalError = totalError + error

int Turnerror; //SensorValue - DesireValue : Position
int TurnprevError = 0; //Position 20 milliseconds ago
int Turnderivative; //error - prevError : Speed
int TurntotalError = 0; //totalError = totalError + error

bool resetDriveSensors = false;
//Varibles
//////////////////////////////////////

bool enableDrivePID = true;

int drivePID(){

while(enableDrivePID){

if (resetDriveSensors) {
resetDriveSensors = true;

BLeftMotor.setPosition(0, degrees);
BRightMotor.setPosition(0, degrees);
FLeftMotor.setPosition(0, degrees);
FRightMotor.setPosition(0, degrees);

}

```
//Get the position of both motors
```

int BleftMotorPosition = BLeftMotor.position(vex::degrees);
int BrightMotorPosition = BRightMotor.position(vex::degrees);
int FleftMotorPosition = FLeftMotor.position(vex::degrees);
int FrightMotorPosition = FRightMotor.position(vex::degrees);

///////////////////////////////////////////////
// Lateral movement PID
/////////////////////////////////////////////////////
//average of 4 motors
int averagePostion = (BleftMotorPosition + BrightMotorPosition + FleftMotorPosition + FrightMotorPosition );

//Potential
error = averagePostion - desiredValue;

//Derivative
derivative = error - prevError;

//Intengral
totalError += error;

double LateralMotorPower = (error * kP + derivative * kD + totalError * kI);

double TurnMotorPower = (Turnerror * TurnkP + Turnderivative * TurnkD + TurntotalError * TurnkI);

BLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
BRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);
FLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
FRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);

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

int TurnaveragePostion = (BleftMotorPosition + BrightMotorPosition + FleftMotorPosition + FrightMotorPosition );

//Potential
Turnerror = TurnaveragePostion - desiredTurnValue;

//Derivative
Turnderivative = Turnerror - TurnprevError;

//Intengral
TurntotalError += Turnerror;

BLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
BRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);
FLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
FRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);

//code
prevError = error;
TurnprevError = Turnerror;

vex::task::sleep(1);

}

return 1;
}

/ *---------------------------------------------------------------------------* /
/*  /
/  Autonomous Task  <em>/
//
/  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) {

```
  enableDrivePID = true;
  


  resetDriveSensors = true;

desiredValue = 300;
desiredTurnValue = 600;

vex::task::sleep(3);

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

  }
```

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

void usercontrol(void) {
while (1) {
// This is the main execution loop for the user control program.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.


// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................

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

}

I see two problems (with more possible problems, but let’s start with these). Firstly, you create a task called Drive in the first code you posted, but not in the second one. Whassup with that? You need to make a task with the function, so definitely do that.

Secondly, your kP is VERY small. It may work for you, but you may want to try making it larger if you have problems with the robot not moving.

3 Likes

To add on to this, often times the reason PID loops don’t work is out-of-bounds constants. Try messing around with them a little.

You may need to increase your kP value as the values you are getting are two low to overcome the friction of existing. As a starting point use 0.01 as a kP value and either raise or lower it depending on whats happening.

Also, you may need to include vex::task taskName(drivePID); into your code for it to work.
Hope this helps

1 Like

this code is frightfully long. is it just a drive function?

No it’s a PID loop
20 char

1 Like

Another think I noticed was you arent taking the average of the 4 motor values, you forgot to add / 4 to the end of the line like this.

int TurnaveragePostion = (BleftMotorPosition + BrightMotorPosition 
+ FleftMotorPosition + FrightMotorPosition ) / 4;
1 Like

https://wiki.purduesigbots.com/software/control-algorithms/pid-controller

3 Likes

It helps to learn PID incrementally, these resources are well worth reading. Copying code is alright until you need to fix it

3 Likes