Pid programing help with 3 wire encoders

Programming Support VEXcode V5 Tech Support
1 
  double kP = 0.1;
  double kI = 0.1;
  double kD = 0.1;
  double turnkP = 0.1;
  double turnkI = 0.1;
  double turnkD = 0.1;

  // autonomous settings
  int desiredvalue = 0;
  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;

          EncoderL.setPosition(0,degrees);
          EncoderR.setPosition(0,degrees);
          
      }

      
      //get the positions of all encoders
      int leftmotorposition = EncoderL.position(degrees);
      int rightmotorposition = EncoderR.position(degrees);
      
      ///////////////////////////////////////////////////////////
      ///// lateral movement pid
      ///////////////////////////////////////////////////////////
      // get the average of the motors

      int averageposition = (leftmotorposition + rightmotorposition)/2;

      //potential

      error = desiredvalue - averageposition;

      //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 =leftmotorposition - rightmotorposition;

      //potential

      turnerror = turndifference - desiredturnvalue;

      //derivitive

      turnderivitive = turnerror - turnpreverror;

      //integral

      turntotalerror += turnerror;

      int turnmotorpower = turnerror * turnkP + turnderivitive * turnkD + turntotalerror * turnkI;      

      frontleft.spin(forward, turnmotorpower,  velocityUnits::pct);
      backleft.spin(forward, turnmotorpower,  velocityUnits::pct);
      frontright.spin(forward, turnmotorpower,  velocityUnits::pct); 
      backright.spin(forward, turnmotorpower,  velocityUnits::pct);


      ////////////////////////////////////////////////////////////////////////
      frontleft.spin(forward, lateralmotorpower,  velocityUnits::pct);
      backleft.spin(forward, lateralmotorpower,  velocityUnits::pct);
      frontright.spin(forward, lateralmotorpower,  velocityUnits::pct); 
      backright.spin(forward, lateralmotorpower,  velocityUnits::pct);

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

  return 1;

}

I know its towards the end of this season but I just added 3 wire encoders to my robot, because I could not get the ones built into the V5 motors to work correctly. I am super new to this and I was wondering if anyone would be able to help me because what I have now doesn’t stop my robot at all

2

It looks like you never set your variable enabledrivePID to false, resulting in the while loop looping infinitely.

Maybe you intended to have it set to false when the error is close to 0?

3

So your saying that i just need to set the variable as false in my while loop

4

No, you need to have a statement in the loop that says (in psuedocode)

if(abs(error)<a very small number)
enableDrivePID=false;

This means that the loop will exit once the motion is very near the target.

1 Like
5 

    while(enabledrivePID){

      if(abs(error)<5)
        enabledrivePID=false;

ok so I put that in the code like shown above and the robot still does not want to stop

6

You might need to make the 5 bigger. You’re saying that the wheel needs to be within 5 degrees of it’s target. Depending on your constants, it may never reach that precise of a value.

Side-note: What exactly do you mean by “does not stop?” Do you mean the robot keeps moving slower and slower and never gets to a stopping point, or do you mean it just charges forward full speed?

1 Like
7

the robot completely starts off at full speed and does not slow down or stop. Do you have any ideas how i would be able to make it take off gradualy and slow down the same way?

8

The only thing that I can see that potentially may be incorrect is that you did the spin functions for your motors as a percentage.

I would switch them into either rpm or volts.
frontleft.spin(forward, lateralmotorpower, voltageUnits::volts);

9

What ports do you have your encoder plugged into?

It makes difference how you connect, define, and initialize 3-wire devices:

https://vexforum.com/t/vexcode-v5-text-version-1-0-release/67420/23?u=weilin

https://vexforum.com/t/defining-three-wire-devices-in-text/70888/11?u=weilin

Then you will need to print onto screen (or controller) encoder value to debug if they are turning.

1 Like
10

so ive never used volts before is their anything else i would have to do to go along with it or just state that

11

To me, this issue seems like one have having improperly tuned constants. You set them all to 0.1. I’m no expert, but that could be part of the problem…

12

i have my left encoder plugged into ports c and d and my right encoder plugged into e and f

13

from everyone that i asked they said that those did not mater and that the robot should have stopped because those are just to tune it