PID Code bugs

[JoethePro]

I have been trying to use @Connor 's PID example code to create my own PID loop. I was able to use it to create a straight line code that works. However, the same template does not work for a turn. The code is below.

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

#include "vex.h"

using namespace vex;
motor(LeftMotorFront) = motor(PORT10, true);
motor(LeftMotorBack) = motor(PORT3);
motor(RightMotorBack) = motor(PORT13, true);
motor(RightMotorFront) = motor(PORT20);





double KdP = 0.01;
double C = 2;
double KdD = 0.007;
double KsP = 0.05;
double KsI = 0.01;
double KsD = 0.01;
double KtP = 1;
double KtI = 1;
double KtD = 0;


float degToMdeg = atan(0.6) * sqrt(76.5);


int robotwidth = 9;

float error = 0;
float preverror = 0;
float deriv = 0;
float integral = 0;

float turnerror = 0;
float turnintegral = 0;
float turnderiv = 0;
float turnpreverror = 0;

float dist = 200;
float theta = 45;
int radius = 0;
int pointturn = 0;



bool resetencoders = false;

bool enablePIDstraight = false;
bool enablePIDpturn = true;



int PIDstraight(){
  while(enablePIDstraight){
if(resetencoders){
  resetencoders = false;
  LeftMotorFront.setPosition(0, rotationUnits::deg);
  LeftMotorBack.setPosition(0, rotationUnits::deg);
  RightMotorFront.setPosition(0, rotationUnits::deg);
  RightMotorBack.setPosition(0, rotationUnits::deg);
}

int LeftMotorPos = (LeftMotorFront.rotation(deg)+LeftMotorBack.rotation(deg))/2;
Brain.Screen.newLine();
Brain.Screen.print(turnerror);
int RightMotorPos = (RightMotorFront.rotation(deg)+RightMotorBack.rotation(deg))/2;

 //Get average of the two motors
int averagePosition = (LeftMotorPos + RightMotorPos)/2;

//Potential
error = dist - averagePosition ;

//Derivative
deriv = error - preverror;

//Integral
integral += error;

double lateralMotorPower = error * KdP + deriv * KdD;
if (error < 0.8 * dist){lateralMotorPower += C;}
/////////////////////////////////////////////////////////////////////


///////////////////////////////////////////
//Turning movement PID
/////////////////////////////////////////////////////////////////////
//Get average of the two motors
int turnDifference = LeftMotorPos - RightMotorPos;

//Potential
turnerror = turnDifference;

//Derivative
turnderiv = turnerror - turnpreverror;


//Integral
turnintegral += turnerror;

if(error < 0.5){
  turnintegral = 0;
}

double turnMotorPower = turnerror * KsP + turnderiv * KsD + turnintegral * KsI;
/////////////////////////////////////////////////////////////////////

LeftMotorFront.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
RightMotorBack.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotorFront.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
LeftMotorBack.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);





preverror = error;
turnpreverror = turnerror;
vex::task::sleep(15);

  }

  return 1;
}

int PIDgradturn(){
  if (resetencoders){
resetencoders = false;
LeftMotorFront.setPosition(0, rotationUnits::deg);
LeftMotorBack.setPosition(0, rotationUnits::deg);
RightMotorFront.setPosition(0, rotationUnits::deg);
RightMotorBack.setPosition(0, rotationUnits::deg);
  }
  int LeftMotorPos = (LeftMotorFront.rotation(deg)+LeftMotorBack.rotation(deg))/2;
  Brain.Screen.newLine();
  Brain.Screen.print(turnerror);
  int RightMotorPos = (RightMotorFront.rotation(deg)+RightMotorBack.rotation(deg))/2;
  
  float Thetaratio = (robotwidth+radius)/radius;

  error = dist - (LeftMotorPos + RightMotorPos)/2;
  
  //BigMotorIdeal = SmallMotorPos * Thetaratio;
  return 1;
}

int PIDpointturn(){
  while(enablePIDpturn){
if (resetencoders){
  resetencoders = false;
  LeftMotorFront.setPosition(0, rotationUnits::deg);
  LeftMotorBack.setPosition(0, rotationUnits::deg);
  RightMotorFront.setPosition(0, rotationUnits::deg);
  RightMotorBack.setPosition(0, rotationUnits::deg);
}

  float LeftMotorPos = (LeftMotorFront.rotation(rotationUnits::deg)+std::abs(LeftMotorBack.rotation(rotationUnits::deg)))/2;
  float RightMotorPos = (std::abs(RightMotorFront.rotation(deg))+RightMotorBack.rotation(deg))/2;
  Brain.Screen.newLine();
  Brain.Screen.print(RightMotorPos);
  
  error = dist - (std::abs(LeftMotorPos) + std::abs(RightMotorPos))/2;

  integral += error;

  deriv = error - preverror;


  float lateralMotorPower = error * KtP + integral * KtI + deriv * KtD + C ;


  LeftMotorFront.spin(forward, -lateralMotorPower , voltageUnits::volt);
  RightMotorBack.spin(forward, -lateralMotorPower, voltageUnits::volt);
  RightMotorFront.spin(forward, lateralMotorPower, voltageUnits::volt);
  LeftMotorBack.spin(forward, lateralMotorPower , voltageUnits::volt);

  task::sleep(15);
  preverror = error;
  }
  return 1;

}


void Autonomous(){
  task DriveStraightTask (PIDstraight);
  //task GradTurn(PIDgradturn);
  task PointTurn(PIDpointturn);
  dist = 45 * degToMdeg;
  task::sleep(1000);
  waitUntil( error < 0.5);
  enablePIDpturn = false;

  

  
  



  


}
int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
  Autonomous();
  


  
}

Note: The only reason I posted my whole code is so that is there is an error outside of my function, we can find it.

So basically, when I try to execute PIDpointturn (which is supposed to be a 45 degree turn), the robot turns forever. I have tried for the past 4 hours to get it to stop at the right value, but with no success.

Edit: I really need to fix this problem as my team needs a reliable autonomous in 3 days for State.

[Connor]

If you read the PID tutorial main post, it explains what to do in those type of cases:

VEXCode PID Tutorial

I will be allocating this thread to answering any questions in regards to the tutorial Fair Warning: I am unsure if I have the positive and negative motor values correct, as this is all done on the top of my head. So If the robot is continuing to go even after the lateral position is reached, change error = averagePosition - desiredValue; to error = desiredValue - averagePosition; If the robot continues to go even after the turning desired is reached, change turnError = turnDifference - de…

If the robot continues to go even after the turning desired is reached, change

turnError = turnDifference - desiredTurnValue;

to

turnError = desiredTurnValue - turnDifference;

[langchen]

given you spent 4 hours on debugging code and state in three days, I would suggest use

rotatefor 
startRotateFor

Those are much easy to use. or you can use drive train.

[Connor]

I agree, since their states is in a couple of days, programming with PID is time consuming and I would advise against utilizing PID if you are so close to competition.

[JoethePro]

I already tried that, and it did not work. I do not remember exactly what I did though.

[JoethePro]

I remember what happened. The wheels would stay still, and when I manually moved one wheel a little bit, the wheels on both sides would turn in opposite directions endlessly.

@langchen We already have a 50% consistent 5 point that uses RotateFor. We really need the consistent autonomous commands so we can get a good autonomous/ skills.

[langchen]

I see there is [quote=“ChingChong, post:1, topic:77077”]
+= C
[/quote] in your straight PID and +C term in your turning PID. I wonder why you add this term of constant of 2.

You have two tasks running on the same motors. It may cause problem.

[JoethePro]

The plus C in my straight line code is to basically add speed to my drive. It is basically a PCD loop. I didn’t want the integral element for my drive. In my turn, the + C was to actually get the robot moving. I don’t really think it is necessary. The robot wouldn’t even turn and I just assumed it needed more power.

[rpm]

Those with more experience with PID please correct me if i’m wrong…

I have to agree with the others. Three days before a tournament is not the time to learn and test out a PID. There are multiple issues with the code. Some of them are:

Right side motors are usually set as reversed and left side are not.

vex::motor.rotation() return a double not an int…
https://help.vexcodingstudio.com/#cpp/namespacevex/classvex_1_1motor/rotation

It would probably be easier to use/visualize if rotation units were in rev since the circumference of the wheel is 1 rev. revs = dist/pi*d (of wheel)

The i-term can have serious integral windup b/c it is not clamp to max power. It needs to be summed with ki so it can be clamped to either max or min power.

Derivative kick is not a problem if you don’t change the set point (dist). But to handle that, typically the d-term only looks at the change in the senor value.

0.8 * dist can be a large value if dist equates to more than an inch. since the code is not looking at the ABS of error, it cannot handle negative values of dist. Not driving backwards.
why is lateralMotorPower not set to zero when close enough to dist?

the drive straight control has the same issues but at least kills turnintegral when close. note that integral is what keeps the bot straight if there is bias in encoders or chassis. I would also think that the code should apply half the turnMotorPower to each side but maybe the k-terms fix that.

turnMotorPower does not get set to 0 when at/near dist.

finally laterMotorPower needs to be clamped to a value less than the actual motor max so that turnMotorPower can achieve adding or subtracting as needed.

This is not meant to be harsh but to let you know what you are getting yourself into.

plus tuning k-terms is not a fast process.

It is probably a better solution to use rotateFor in steps as the bot starts and stops. Increase and decrease power in 4 to 6 steps to prevent slippage and have accurate stops.

[ThirdDegree]

A very useful tool you have at your disposal and it seems you aren’t using is terminal output. std::cout and printf should both work (VexCode users please correct me if I’m wrong).

With these you have the ability to look at what your code is actually doing to fill the gap of what you expect and what you observe. As a first step with PIDs, try logging both error and post-gain motor power. If error has unexpected behavior, you’ve narrows it down to a much smaller section of code to worry about. Same applies to post-gain motor power.

This should be your goto for trying to solve code issues where the robot is simply not doing what you’re expecting. Move them around and change what they log, get creative.

[JoethePro]

I applied some of those changes to the code. However, I not sure if I understand what you meant by summing the integral term by ki. Did you mean that you should add the error *ki to the integral every time it loops?

here is my improved code.

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

#include "vex.h"

using namespace vex;
// changed reversed motors to right side only.
motor(LeftMotorFront) = motor(PORT10);
motor(LeftMotorBack) = motor(PORT3);
motor(RightMotorBack) = motor(PORT13, true);
motor(RightMotorFront) = motor(PORT20, true);





double KdP = 0.01;
double C = 2;
double KdD = 0.007;
double KsP = 0.05;
double KsI = 0.01;
double KsD = 0.01;
double KtP = 1;
double KtI = 1;
double KtD = 0;


float degToMdeg = atan(0.6) * sqrt(76.5);


int robotwidth = 9;

float error = 0;
float preverror = 0;
float deriv = 0;
float integral = 0;

float turnerror = 0;
float turnintegral = 0;
float turnderiv = 0;
float turnpreverror = 0;

float dist = 200;
float theta = 45;
int radius = 0;
int pointturn = 0;



bool resetencoders = false;

bool enablePIDstraight = false;
bool enablePIDpturn = true;



int PIDstraight(){
  while(enablePIDstraight){
    if(resetencoders){
      resetencoders = false;
      LeftMotorFront.setPosition(0, rotationUnits::deg);
      LeftMotorBack.setPosition(0, rotationUnits::deg);
      RightMotorFront.setPosition(0, rotationUnits::deg);
      RightMotorBack.setPosition(0, rotationUnits::deg);
    }
    //changed both variables to doubles
    double LeftMotorPos = (LeftMotorFront.rotation(deg)+LeftMotorBack.rotation(deg))/2;
    Brain.Screen.newLine();
    Brain.Screen.print(turnerror);
    double RightMotorPos = (RightMotorFront.rotation(deg)+RightMotorBack.rotation(deg))/2;
    //changed variable tyrp to double
     //Get average of the two motors
    double averagePosition = (LeftMotorPos + RightMotorPos)/2;

    //Potential
    error = dist - averagePosition ;

    //Derivative
    deriv = error - preverror;

    //Integral
    integral += error;
    //clamped Motor Power to 8 Volts
    double lateralMotorPower = std::min(error * KdP + deriv * KdD, 8.0);
    // Changed error to absolute value of error
    if (std::abs(error) < 0.8 * dist){lateralMotorPower += C;}
    /////////////////////////////////////////////////////////////////////


    ///////////////////////////////////////////
    //Turning movement PID
    /////////////////////////////////////////////////////////////////////
    //Get average of the two motors
    float turnDifference = LeftMotorPos - RightMotorPos;

    //Potential
    turnerror = turnDifference;

    //Derivative
    turnderiv = turnerror - turnpreverror;
    

    //Integral
    turnintegral += turnerror;

    if(error < 0.5){
      turnintegral = 0;
    }

    double turnMotorPower = turnerror * KsP + turnderiv * KsD + turnintegral * KsI;
    /////////////////////////////////////////////////////////////////////

    LeftMotorFront.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    RightMotorBack.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightMotorFront.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    LeftMotorBack.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);



    

    preverror = error;
    turnpreverror = turnerror;
    vex::task::sleep(15);

  }

  return 1;
}

int PIDgradturn(){
  if (resetencoders){
    resetencoders = false;
    LeftMotorFront.setPosition(0, rotationUnits::deg);
    LeftMotorBack.setPosition(0, rotationUnits::deg);
    RightMotorFront.setPosition(0, rotationUnits::deg);
    RightMotorBack.setPosition(0, rotationUnits::deg);
  }
  int LeftMotorPos = (LeftMotorFront.rotation(deg)+LeftMotorBack.rotation(deg))/2;
  Brain.Screen.newLine();
  Brain.Screen.print(turnerror);
  int RightMotorPos = (RightMotorFront.rotation(deg)+RightMotorBack.rotation(deg))/2;
  
  float Thetaratio = (robotwidth+radius)/radius;

  error = dist - (LeftMotorPos + RightMotorPos)/2;
  
  //BigMotorIdeal = SmallMotorPos * Thetaratio;
  return 1;
}

int PIDpointturn(){
  while(enablePIDpturn){
    if (resetencoders){
      resetencoders = false;
      LeftMotorFront.setPosition(0, rotationUnits::deg);
      LeftMotorBack.setPosition(0, rotationUnits::deg);
      RightMotorFront.setPosition(0, rotationUnits::deg);
      RightMotorBack.setPosition(0, rotationUnits::deg);
    }

  float LeftMotorPos = (LeftMotorFront.rotation(rotationUnits::deg)+std::abs(LeftMotorBack.rotation(rotationUnits::deg)))/2;
  float RightMotorPos = (std::abs(RightMotorFront.rotation(deg))+RightMotorBack.rotation(deg))/2;
  Brain.Screen.newLine();
  Brain.Screen.print("rightposition",RightMotorPos);
  Brain.Screen.newLine();
  Brain.Screen.print("Error", error);
  //reversed dist - postion
  error = (std::abs(LeftMotorPos) + std::abs(RightMotorPos))/2 - dist;

  integral += error;

  deriv = error - preverror;


  float lateralMotorPower = error * KtP + integral * KtI + deriv * KtD ;


  //Set Motor Power to 0 if close to target value
  if(std::abs(error) < 0.5){
    lateralMotorPower = 0;
  }


  LeftMotorFront.spin(forward, -lateralMotorPower , voltageUnits::volt);
  RightMotorBack.spin(forward, -lateralMotorPower, voltageUnits::volt);
  RightMotorFront.spin(forward, lateralMotorPower, voltageUnits::volt);
  LeftMotorBack.spin(forward, lateralMotorPower , voltageUnits::volt);

  task::sleep(15);
  preverror = error;
  }
  return 1;

}


void Autonomous(){
  task DriveStraightTask (PIDstraight);
  //task GradTurn(PIDgradturn);
  task PointTurn(PIDpointturn);
  dist = 45 * degToMdeg;
  task::sleep(100);
  waitUntil( error < 0.5);
  enablePIDpturn = false;

  

  
  



  


}
int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
  Autonomous();
  


  
}

I have not been able to test it yet because I cannot access the robot now, I may be able to later today. If there is anythink I did wrong, please tell me.
@rpm thank you for your reply, it definitely clarified some issues. I did not just start learning about PID, I have been working on this code for two weeks already with limited success. However, I am obviously still very new to PIDs.

@ThirdDegree I have kind of been doing that. I tried printing out the error, and it was basically showing zero for a little bit and then went negative. I did not understand why it did this because I though that the error should start out as a large number and then get smaller (or more negative), not at zero and then get smaller.

[rpm]

Yes, summing the integral is this.
turnintergral += KtI * turnerror;

Its not necessary but, the reason you do this is so you can tune ki live. having ki in the loop means when you adjust it the change is only for the current error and not all prior accumulated errors. the dist PID is probably going to be a lot easier to tune which is good since it is not as easy to tune live. drive straight is really suited for live tuning since it is continuously attempting to hit a center point.

I can’t look in depth at the new code until later, and I’m not familiar with vexcode, but here are some more thoughts. Is this a competition template? Does VCt require tasks to be void functions? Tasks usually need sleep(50) at the end of the loop, where 50 is whatever you need as your delta time. It is still no clamp for turnMotorPower. I would think that ABS(turnMotorPower + turnMotorPower) should be < 12. the code looks like it intends to split it up between the two. I would think it could support lateral power at +/- 10 and turn at +/- 2.

[JoethePro]

The code is not in a competition template right now. I just callback the autonomous function in the main function.

If I am correct, tasks in vexcode need to return integers. I will add the clamp to turnMotorPower and the integral summation.

[rpm]

you are correct, task are of type int. a little searching here found its prototype:
https://api.vexcode.cloud/v5/html/classvex_1_1task.html

[JoethePro]

Heres my updated code
The turn is just not turning consistently. It now stops but is not entirely consistent. We wanted it to turn 45 degrees, but it goes from 25 - 55 degrees. Is this just because we have to tune the constants more? Or is there logic that would cause it to be so inconsistent?
/----------------------------------------------------------------------------/
/* /
/ Module: main.cpp /
/ Author: C:\Users\jdhan /
/ Created: Mon Mar 02 2020 /
/ Description: V5 project /
/ /
/----------------------------------------------------------------------------*/

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

#include "vex.h"

using namespace vex;
// changed reversed motors to right side only.
motor(LeftMotorFront) = motor(PORT10, true);
motor(LeftMotorBack) = motor(PORT3);
motor(RightMotorBack) = motor(PORT13, true);
motor(RightMotorFront) = motor(PORT20);





double KdP = 0.01;
double C = 2;
double KdD = 0.007;
double KsP = 0.05;
double KsI = 0.01;
double KsD = 0.01;
double KtP = 0.75;
double KtI = 0.014;
double KtD = 0.053;


float degToMdeg = atan(0.6) * sqrt(76.5)/2;


int robotwidth = 9;

float error = 0;
float preverror = 0;
float deriv = 0;
float integral = 0;

float turnerror = 0;
float turnintegral = 0;
float turnderiv = 0;
float turnpreverror = 0;

float dist = 200;
float theta = 45;
int radius = 0;
int pointturn = 0;



bool resetencoders = false;

bool enablePIDstraight = true;
bool enablePIDpturn = false;



int PIDstraight(){
  while(enablePIDstraight){
    if(resetencoders){
      resetencoders = false;
      LeftMotorFront.setPosition(0, rotationUnits::deg);
      LeftMotorBack.setPosition(0, rotationUnits::deg);
      RightMotorFront.setPosition(0, rotationUnits::deg);
      RightMotorBack.setPosition(0, rotationUnits::deg);
    }
    //changed both variables to doubles
    double LeftMotorPos = (LeftMotorFront.rotation(deg)+LeftMotorBack.rotation(deg))/2;
    Brain.Screen.newLine();
    Brain.Screen.print(turnerror);
    double RightMotorPos = (RightMotorFront.rotation(deg)+RightMotorBack.rotation(deg))/2;
    //changed variable type to double
     //Get average of the two motors
    double averagePosition = (LeftMotorPos + RightMotorPos)/2;

    //Potential
    error = dist - averagePosition ;

    //Derivative
    deriv = error - preverror;

    //Integral
    integral += error;
    //clamped Motor Power to 8 Volts
    double lateralMotorPower = std::min(error * KdP + deriv * KdD, 10.0);
    // Changed error to absolute value of error
    if (std::abs(error) < 0.8 * dist){lateralMotorPower += C;}
    /////////////////////////////////////////////////////////////////////


    ///////////////////////////////////////////
    //Turning movement PID
    /////////////////////////////////////////////////////////////////////
    //Get average of the two motors
    float turnDifference = LeftMotorPos - RightMotorPos;

    //Potential
    turnerror = turnDifference;

    //Derivative
    turnderiv = turnerror - turnpreverror;
    

    //Integral
    turnintegral += turnerror;

    if(error < 0.5){
      turnintegral = 0;
    }
    //Capped turn power to 2 Volts
    double turnMotorPower = std::min(turnerror * KsP + turnderiv * KsD + turnintegral * KsI, 2.0);
    //Eliminated Turn power at 95 percent of distance
    if(error < 0.05 * dist){
      turnMotorPower = 0;
    }
     /////////////////////////////////////////////////////////////////////

    LeftMotorFront.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightMotorBack.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightMotorFront.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    LeftMotorBack.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);



    

    preverror = error;
    turnpreverror = turnerror;
    vex::task::sleep(15);

  }

  return 1;
}

int PIDgradturn(){
  if (resetencoders){
    resetencoders = false;
    LeftMotorFront.setPosition(0, rotationUnits::deg);
    LeftMotorBack.setPosition(0, rotationUnits::deg);
    RightMotorFront.setPosition(0, rotationUnits::deg);
    RightMotorBack.setPosition(0, rotationUnits::deg);
  }
  int LeftMotorPos = (LeftMotorFront.rotation(deg)+LeftMotorBack.rotation(deg))/2;
  Brain.Screen.newLine();
  Brain.Screen.print(turnerror);
  int RightMotorPos = (RightMotorFront.rotation(deg)+RightMotorBack.rotation(deg))/2;
  
  float Thetaratio = (robotwidth+radius)/radius;

  error = dist - (LeftMotorPos + RightMotorPos)/2;
  
  //BigMotorIdeal = SmallMotorPos * Thetaratio;
  return 1;
}

int PIDpointturn(){
  while(enablePIDpturn){
    if (resetencoders){
      resetencoders = false;
      LeftMotorFront.setPosition(0, rotationUnits::deg);
      LeftMotorBack.setPosition(0, rotationUnits::deg);
      RightMotorFront.setPosition(0, rotationUnits::deg);
      RightMotorBack.setPosition(0, rotationUnits::deg);
    }

  float LeftMotorPos = (LeftMotorFront.rotation(rotationUnits::deg)-LeftMotorBack.rotation(rotationUnits::deg))/2;
  float RightMotorPos = (RightMotorFront.rotation(deg)-RightMotorBack.rotation(deg))/2;
  Brain.Screen.newLine();
  Brain.Screen.print("Rightposition: %f", RightMotorFront.rotation(deg));
  Brain.Screen.newLine();
  Brain.Screen.print("Error: %f", error);
  //reversed dist - postion
  error = dist -  (std::abs(LeftMotorPos) + std::abs(RightMotorPos))/2;

  integral += error * KtI;

  if(std::abs(error) > dist * 0.8){
    integral = 0;
  }

  deriv = error - preverror;


  float lateralMotorPower = std::min(error * KtP + integral + deriv * KtD, 10.5) ;


  //Set Motor Power to 0 if close to target value
  if(std::abs(error) < 1){
    lateralMotorPower = 0;
  }


  LeftMotorFront.spin(forward, lateralMotorPower , voltageUnits::volt);
  RightMotorBack.spin(forward, lateralMotorPower, voltageUnits::volt);
  RightMotorFront.spin(forward, -lateralMotorPower, voltageUnits::volt);
  LeftMotorBack.spin(forward, -lateralMotorPower , voltageUnits::volt);

  task::sleep(15);
  preverror = error;
  }
  return 1;

}


void Autonomous(){
  task DriveStraightTask (PIDstraight);
  //task GradTurn(PIDgradturn);
  task PointTurn(PIDpointturn);
  enablePIDpturn = true;
  dist = 45 * degToMdeg;

  waitUntil( std::abs(error) < 1);
  enablePIDpturn = false;

  

  
  



  


}
int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
  Autonomous();
  


  
}

[rpm]

if you are asking about issues with PIDpointturn the easy ones i see are:

reusing global vars between the PID function could cause problems especially for those that maintain a previous value. there is no need to share variables between this functions/tasks.

enablePIDstraight is never set to false so it is doing something with the same variables.
enablePIDpturn is never set to true.

Some code i’m not sure what you are intending:

  float LeftMotorPos = (LeftMotorFront.rotation(rotationUnits::deg)-LeftMotorBack.rotation(rotationUnits::deg))/2;
  float RightMotorPos = (RightMotorFront.rotation(deg)-RightMotorBack.rotation(deg))/2;

plus the same as mentioned about the PIDstraight code.

[JoethePro]

Those lines calculate the average of the encoders on the left and right side of the robot. I then use them to calculate the error in the next lines of the code. I subtracted the code because two of the motors are reversed, so to keep the sign the same I added the negative of the reversed IME values, which is the same as subtracting them. However, I just noticed that I am not subtracting the reversed motors so I don’t know why my code is even partially working.

[rpm]

agreed on that last part. and on the first part…

well that makes your code unreadable w/o some serious comments. why not make the direction of the motors workout so that forward is positive for both sides?

[JoethePro]

Hey guys who responded, thanks for helping me with these issues. Even though it didn’t exactly work, I got the logic working. The constants need to be tuned though. At this point it doesn’t really matter because Tower takeover is done.