GPS PID Help

Programming Support VEXcode Pro V5 Text Tech Support
, ,
#1

I am having trouble with PID code with the GPS sensor. Please tell me what I am doing wrong:

float kP = 3;
float kI = 0;
float kD = 0;
int err;
int pErr = 0;
int tErr = 0;
int der;
int PIDTargetX = 0;
int PIDTargetY = 0;

bool drivePIDEnabled = false

//more code

void setTarget(int x, int y){
  PIDTargetX = x;
  PIDTargetY = y;
}

// drive function (for mecanum drive)

void edrive(double sp3, double sp4, double sp1, double wt = 10){
  LB.spin(forward, sp3 - sp4 + sp1, percent);
  LF.spin(forward, sp3 + sp4 + sp1, percent);
  RF.spin(forward, sp3 - sp4 - sp1, percent);
  RB.spin(forward, sp3 + sp4 - sp1, percent);
  // Controller1.Screen.clearScreen();
  // Controller1.Screen.print("%d", lround(LF.torque(Nm)));
  wait(wt, msec);
}

int drivePID(){
  while (drivePIDEnabled){
    int posX = GPS.xPosition(inches);
    int posY = GPS.yPosition(inches);
    err = sqrt((PIDTargetX - posX)^2 + (PIDTargetY - posY)^2);
    der = err - pErr;
    tErr += err;
    double power = err*kP + tErr*kI + der * kD;
    edrive(power,0,0);
    pErr = err;
    vex::task::sleep(20);
  }
  return 1;
}

/more code

//auton pid code:
GPS.setLocation(0,0);
    vex::task drivePIDTask(drivePID);
    setTarget(48,0);
    drivePIDEnabled = true;

When I run this code, if I put a higher amound in setTarget, the robot goes a shorter distance and vice versa (and I haven’t finished tuning the PID btw). Please tell me if I am making a mistake with this code.

#2

Your code has no way of turning toward its target unless it’s already facing it. I would add that functionality before jumping to any conclusions.

#3

Ok, and actually right after posting this I discovered your JAR-Template library for vexcode and downloaded it and I will probably switch to that instead because it seems to be much easier to work with than coding PID manually, but thank you! Also, the PID I had been using was actually a motor encoder PID that I tried to convert to a GPS PID, but I don’t know if I was doing it correctly anyways. I will implement a turning PID and see if that fixes it.

#4

I have tried to implement turning and I am testing it by making it turn to (0,0), but now it is turning really slowly and going to different rotations but not facing (0,0). I have tried tuning it more, but it either goes way too quickly and overshoots, or it is extremely slow and never makes it. Here is my code:

// Drive PID settings
float kP = 0;
float kI = 0;
float kD = 0;
float turnKP = 1;
float turnKI = 0;
float turnKD = 0;
int err;
int pErr = 0;
int tErr = 0;
int der;
int turnErr;
int turnPErr = 0;
int turnTErr = 0;
int turnDer;
int PIDTargetX = 0;
int PIDTargetY = 0;

bool PIDEnabled = false;

// more code

void setTarget(int x, int y){
  PIDTargetX = x;
  PIDTargetY = y;
}

void edrive(double sp3, double sp4, double sp1, double wt = 10){
  LB.spin(forward, sp3 - sp4 + sp1, percent);
  LF.spin(forward, sp3 + sp4 + sp1, percent);
  RF.spin(forward, sp3 - sp4 - sp1, percent);
  RB.spin(forward, sp3 + sp4 - sp1, percent);
  // Controller1.Screen.clearScreen();
  // Controller1.Screen.print("%d", lround(LF.torque(Nm)));
  wait(wt, msec);
}

void edriveVolts(double sp3, double sp4, double sp1, double wt = 10){
  LB.spin(forward, (sp3 - sp4 + sp1)*120, voltageUnits::mV);
  LF.spin(forward, (sp3 + sp4 + sp1)*120, voltageUnits::mV);
  RF.spin(forward, (sp3 - sp4 - sp1)*120, voltageUnits::mV);
  RB.spin(forward, (sp3 + sp4 - sp1)*120, voltageUnits::mV);
  wait(wt, msec);
}

int PID(){
  while (PIDEnabled){
    int posX = GPS.xPosition(inches);
    int posY = GPS.yPosition(inches);
    float heading1 = GPS.heading(degrees);
    err = sqrt((PIDTargetX - posX)^2 + (PIDTargetY - posY)^2);
    der = err - pErr;
    tErr += err;
    turnErr = (atan2(PIDTargetY - posY, PIDTargetX - posX)*180/pi) - heading1;
    turnDer = turnErr - turnPErr;
    turnTErr += turnErr;
    
    double power = err*kP + tErr*kI + der * kD;
    double turnPower = turnErr*turnKP + turnTErr*turnKI + turnDer*turnKD;
    Controller1.Screen.setCursor(6, 1);
    Controller1.Screen.print("%d",int(heading1));
    edrive(power,0,turnPower*5.0/9.0);
    turnPErr = turnErr;
    pErr = err;
    vex::task::sleep(30);
  }
  return 1;
}

(I was printing the heading of the robot for debugging, and it seems to be fluctuating a lot. It might be because of the lighting, but I am not sure)

#5

Hi, have been able to fix turning (it turns out there were some offset GPS sensor problems and I wasn’t processing the output of atan2 correctly. I was starting to tune the PID when I came across a problem. kD (turnKD in my code) doesnt seem to be working properly, and not dampening the turn or slowing down until I set it to really large numbers higher than kP (turnKP in my code). The reason kP,kI,and kD are 0 is because I’m only tuning turning first. Lastly, I was logging results when I noticed that the heading and turn error was changing and seemed to update after the robot overshot, sometimes updating correctly, sometimes not. I am not sure why. Also the desired heading seemed to jump around a bit, but am more concerned about the heading. Here is my code:

float kP = 0;
float kI = 0;
float kD = 0;
float turnKP = 0.8;
float turnKI = 0;
float turnKD = 0;
int err;
int pErr = 0;
int tErr = 0;
int der;
int turnErr;
int turnPErr = 0;
int turnTErr = 0;
int turnDer;
int PIDTargetX = 0;
int PIDTargetY = 0;

bool PIDEnabled = false;


float pi = 3.14159;
float D = 10.0;
float G = 7.0/5.0;

float DriveRev = 0.8;

float totalRotation = 0.0;
float steadyDeg;

double flywheelPower;

bool flywheel = false;
bool flywheelTog = false;
bool expansionReady = false;

void setTarget(int x, int y){
  PIDTargetX = x;
  PIDTargetY = y;
}

void edrive(double sp3, double sp4, double sp1, double wt = 10){
  LB.spin(forward, sp3 - sp4 + sp1, percent);
  LF.spin(forward, sp3 + sp4 + sp1, percent);
  RF.spin(forward, sp3 - sp4 - sp1, percent);
  RB.spin(forward, sp3 + sp4 - sp1, percent);
  // Controller1.Screen.clearScreen();
  // Controller1.Screen.print("%d", lround(LF.torque(Nm)));
  wait(wt, msec);
}

void edriveVolts(double sp3, double sp4, double sp1, double wt = 10){
  LB.spin(forward, (sp3 - sp4 + sp1)*120, voltageUnits::mV);
  LF.spin(forward, (sp3 + sp4 + sp1)*120, voltageUnits::mV);
  RF.spin(forward, (sp3 - sp4 - sp1)*120, voltageUnits::mV);
  RB.spin(forward, (sp3 + sp4 - sp1)*120, voltageUnits::mV);
  wait(wt, msec);
}

void driveBrake(int wt=0) {
  LB.stop(brake);
  LF.stop(brake);
  RB.stop(brake);
  RF.stop(brake);
  wait(wt,msec);
}

int PID(){
  while (PIDEnabled){
    int posX = GPS.xPosition(inches);
    int posY = GPS.yPosition(inches);
    if (turnTErr == 0){
      Inertial1.setRotation(GPS.heading(degrees),degrees);
    }
    float heading1 = Inertial1.rotation(degrees);
    float desHeading = atan2(PIDTargetY - posY, PIDTargetX - posX)*180/pi;
    if (desHeading < 0){
      desHeading += 360;
    }
    desHeading = (360 - desHeading) - 90;
    if (desHeading < 0){
      desHeading += 360;
    }
    if (auton == 2){
      desHeading = int(desHeading + 180) % 360;
    }
    err = sqrt((pow(PIDTargetX - posX,2) + pow(PIDTargetY - posY,2)));
    der = err - pErr;
    tErr += err;
    turnErr = (desHeading - heading1);
    turnDer = turnErr - turnPErr;
    turnTErr += turnErr;
    if (abs(turnErr) > 1){
      err = 0;
    }
    printf("%d %d\n",int(heading1),int(desHeading));
    
    double power1 = err*kP + tErr*kI + der*kD;
    double turnPower;
    if (abs(turnErr) > 5){
      turnPower = (turnErr*turnKP) + (turnTErr*turnKI) + (turnDer*turnKD);
    }else{
      turnPower = (turnErr*turnKP * 1.5) + (turnTErr*turnKI) + (turnDer*turnKD);
    }
    if (abs(turnErr) == 0){
      driveBrake();
      return 1;
    }
    edrive(power1,0,turnPower*5.0/9.0);
    turnPErr = turnErr;
    pErr = err;
    vex::task::sleep(10);
  }
  return 1;
}

If anyone has any suggestions, please tell them to me. States for us is on Sunday, and I really want to have PID working by then. Thank you!