Help Needed with PID

Hi everyone,
I recently decided to write a PID for my robot, but there are a few issues with it. Before people ask, I have checked the search bar, and I have also read the George Gillard guide to PIDs. Further, I have watched the 30-minute PID video on YouTube. I do not quite know what the issue is although I am aware that there are problems. Currently the Robot will not drive at all. I am trying to control the forward motion with the internal motor encoders and the turning with the inertial sensor. If anyone can help fix the issues it would be really helpful.

#include "vex.h"
using namespace vex;
// A global instance of competition
competition Competition;
float myVariable, convspin;
float yourVariable, flyspin;
float ourVariable, index1;
void Lstrafe2(){
  Rback.spin(reverse);
  Rfront.spin(forward);
  Lback.spin(forward);
  Lfront.spin(reverse);}
void Rstrafe2(){
  Rback.spin(forward);
  Rfront.spin(reverse);
  Lback.spin(reverse);
  Lfront.spin(forward);
}
/////////////////////////////////Conveyor
void ConveyorButton() {
if (convspin == 0.0) {
   Conveyor.spin(forward);
   convspin = 1.0;
}
else {
   Conveyor.stop();
   convspin = 0.0;
}
}
int whenStarted1() {
convspin = 0.0;
return 0;
}
////////////////////////////////Flywheel
void FlyButton() {
if (flyspin == 0.0) {
   Flywheel.spin(forward);
   flyspin = 1.0;
}
else {
   Flywheel.stop();
   flyspin = 0.0;
}
}
int whenStarted2() {
flyspin = 0.0;
return 0;
}
///////////////////////////////////indexer
void IndexButton() {
  if (index1 == 0.0) {
   Indexer.set(true);
   index1 = 1.0;
  }
  else {
   Indexer.set(false);
   index1 = 0.0;
  }
}
int whenStarted3() {
  index1 = 0.0;
  return 0;
}
//All other voids 
// Strafe Left
void Lstrafe(int numTurns){
  Rback.spinFor(reverse,numTurns,turns,false);
  Rfront.spinFor(forward,numTurns,turns,false);
  Lback.spinFor(forward,numTurns,turns,false);
  Lfront.spinFor(reverse,numTurns,turns);}
void Rstrafe(int numTurns){
  Rback.spinFor(forward,numTurns,turns,false);
  Rfront.spinFor(reverse,numTurns,turns,false);
  Lback.spinFor(reverse,numTurns,turns,false);
  Lfront.spinFor(forward,numTurns,turns);}
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
}
int distanceError;
int angleError;


int desiredValue;
int desiredTurnValue;
int error; // Sensor value - desired value: position
 
// variables modified for use
bool enableTurnPID = true;
bool enableDrivePID = true;
bool resetDriveSensors = false;


int drivePID(){
while (enableDrivePID){
 
if (resetDriveSensors){
  resetDriveSensors = false;
  Lside.setPosition(0,turns);
  Rside.setPosition(0,turns);
}
 
int forwardDistanceController;
int angleController;
 
forwardDistanceController = desiredValue;
angleController = desiredTurnValue;
 
int LsidePosition = Lside.position(turns);
int RsidePosition = Lside.position(turns);
 
int averagePosition = (LsidePosition + RsidePosition)/2;
int inertialAngle = Inertial.heading();
 
//Potential
distanceError = averagePosition - desiredValue;
angleError = inertialAngle - desiredTurnValue;
 
Lside.spinFor(forward,error,turns,false);
Rside.spinFor(forward,error,turns);
 
double straightOut = forwardDistanceController - (distanceError);
double angleOut = angleController - (desiredTurnValue);
 
int leftPower;
int rightPower;
 
leftPower = straightOut + angleOut;
rightPower = straightOut - angleOut;
 
Lside.setVelocity(leftPower,percent);
Rside.setVelocity(rightPower,percent);
 
vex::task::sleep(20);
}
return(1);
}
// Autonomous code
void autonomous(void) {
enableTurnPID = true;
vex::task start(drivePID);
desiredValue = 30;
desiredTurnValue = 0;
Drivetrain.stop();
while(1<2){
Controller1.Screen.clearScreen();
Controller1.Screen.print(error); 
}
}
 
///////////////////DRIVER CONTROL
 
void driver(){
  enableDrivePID = false;
  enableTurnPID = false;
  Controller2.Screen.clearScreen();
  while(1<2){
    Eye.setLight(ledState::on);
    Eye.brightness(100);
    Drivetrain.setStopping(hold); 
  // Prints the temperature of the Drivetrain
    Controller1.Screen.clearLine(3);
    Controller1.Screen.setCursor(3, 1);
    Controller1.Screen.print(Drivetrain.temperature(percent));
  // Prints the temperature of the Flywheel
    Controller2.Screen.clearLine(2);
    Controller2.Screen.setCursor(2, 1);
    Controller2.Screen.print(Flywheel.temperature(percent));
  // Prints the temperature of the Conveyor
    Controller2.Screen.clearLine(3);
    Controller2.Screen.setCursor(3, 1);
    Controller2.Screen.print(Conveyor.temperature(percent));
  //Drivetrain Control
    Rfront.setVelocity((Controller1.Axis2.position() - (Controller1.Axis1.position() + Controller1.Axis4.position()/2)), percent);
    Lfront.setVelocity((Controller1.Axis2.position() + (Controller1.Axis1.position() + Controller1.Axis4.position()/2)), percent);
    Rback.setVelocity((Controller1.Axis2.position() + (Controller1.Axis1.position() - Controller1.Axis4.position()/2)), percent);
    Lback.setVelocity((Controller1.Axis2.position() - (Controller1.Axis1.position() - Controller1.Axis4.position()/2)), percent);
    Rfront.spin(forward);
    Lfront.spin(forward);
    Rback.spin(forward);
    Lback.spin(forward);  
  // Conveyor
    Conveyor.setStopping(brake);
    Conveyor.setVelocity(100,percent);
    if ((Controller1.ButtonB.pressing)()||(Controller2.ButtonB.pressing)()){
      Conveyor.spin(reverse);
    }
  // Flywheel
    Flywheel.setStopping(coast);
    Flywheel.setVelocity(100,percent);
  //Endgame
    if (((Controller1.ButtonLeft.pressing)()&&(Controller1.ButtonA.pressing)())||
    ((Controller2.ButtonLeft.pressing)()&&(Controller2.ButtonA.pressing)())){
      Endgame.set(true);
    }
    else{
      Endgame.set(false);
    }
  //Flicker
    if ((Controller1.ButtonR2.pressing)()||(Controller2.ButtonR2.pressing)()){
      Flicker.set(true);
    }
    else{
      Flicker.set(false);
    }
  }
  //Eye Control
    if (Eye.color()==red){
      Conveyor.stop();
    }
wait(0.02,seconds);
}
int main() {
  Controller1.ButtonR1.pressed(ConveyorButton);
  Controller2.ButtonR1.pressed(ConveyorButton);
  Controller1.ButtonL1.pressed(FlyButton);
  Controller2.ButtonL1.pressed(FlyButton);
  Controller1.ButtonL2.pressed(IndexButton);
  Controller2.ButtonL2.pressed(IndexButton);
 
// Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
  Competition.autonomous(autonomous);
  Competition.drivercontrol(driver);
  whenStarted1();
  whenStarted2();
  whenStarted3();
}

Hey can you make a minimal example that doesn’t work? Remove all the code not related to your driving straight PID controller. (Intake, strafe etc) Then make sure it all runs still and doesn’t do what you want.

This will make it easier for others to spot the problem and also often results in you solving the problem yourself.

3 Likes

I would recommend making your PID into a function rather than a task. Also, add an accuracy variable that exits the loop when the abs(error) is less than accuracy.

1 Like

OK here is the reduced code. Sorry about that.

#include "vex.h"
using namespace vex;
// A global instance of competition
competition Competition;
int distanceError;
int angleError;

int desiredValue;
int desiredTurnValue;
int error; // Sensor value - desired value: position
 
// variables modified for use
bool enableTurnPID = true;
bool enableDrivePID = true;
bool resetDriveSensors = false;

int drivePID(){
while (enableDrivePID){
 
if (resetDriveSensors){
  resetDriveSensors = false;
  Lside.setPosition(0,turns);
  Rside.setPosition(0,turns);
}
 
int forwardDistanceController;
int angleController;
 
forwardDistanceController = desiredValue;
angleController = desiredTurnValue;
 
int LsidePosition = Lside.position(turns);
int RsidePosition = Lside.position(turns);
 
int averagePosition = (LsidePosition + RsidePosition)/2;
int inertialAngle = Inertial.heading();
 
//Potential
distanceError = averagePosition - desiredValue;
angleError = inertialAngle - desiredTurnValue;
 
Lside.spinFor(forward,error,turns,false);
Rside.spinFor(forward,error,turns);
 
double straightOut = forwardDistanceController - (distanceError);
double angleOut = angleController - (desiredTurnValue);
 
int leftPower;
int rightPower;
 
leftPower = straightOut + angleOut;
rightPower = straightOut - angleOut;
 
Lside.setVelocity(leftPower,percent);
Rside.setVelocity(rightPower,percent);
 
vex::task::sleep(20);
}
return(1);
}
// Autonomous code
void autonomous(void) {
enableTurnPID = true;
vex::task start(drivePID);
desiredValue = 30;
desiredTurnValue = 0;
Drivetrain.stop();
while(1<2){
Controller1.Screen.clearScreen();
Controller1.Screen.print(error); 
}
}

Can you provide an example of that? I still don’t completely understand the intricacies of VexCode Pro. Also, if anyone else knows how to help, provide an example if you can, because it is a lot easier to understand that way.

Here’s some psuedo code I wrote for another person’s question

Void PID(double target) { 
   double kp = 1; 
   double ki = .1; 
   double kd = .01; 
   double sum = 0; 
   double prevError = 0; 
   double error = target - sensor value; 
   double accuracy = .1; 
   double speed; 
   while (fabs(error) > accuracy) { 
     error = targetAngle - sensor value; 
     speed = (error * kp) + (ki * sum) + (kd * (error - prevError));
      motor.spin(fwd, speed, pct);

    wait(10, msec); 
     prevError = error; 
     sum += error; 
   }
motor.stop();
}
1 Like

So how would I declare this in my Autonomous, and how would I set the drive distance and turn heading? Also, if for whatever reason I needed to get out of the PID loop, how would I do that? Finally, people talk about tuning the PID. How does that work? Sorry, I ask a lot of questions…

You would declare this as a function before driver control and use target as whatever you want to turn to (example:PID(50);) You would need to make a separate PID for driving a distance. If you wanted to brake from the loop you would need to add an or conditional to the fabs(error)>accuracy . Tuning the PID is where you adjust the P,I,and D values until the PID works well for you.

3 Likes