Our Robot is Drifting left in the PID

Programming Support VEXcode V5 Tech Support
#1

I was wondering if there is a way to make my robot auto correct in a PID loop. I have tried for a week now and have seen to not have been successful.

This is a video of what my robot does: Programming PID - Robot Drifting to Left_ 2/24/2023 - YouTube

And this what my code looks like:

#include "vex.h"
#include "iostream"
#include <cmath>
using namespace vex;
using namespace std;

using signature = vision::signature;
using code = vision::code;

brain Brain;
controller Controller1;

//motor definitions
motor Index (vex::PORT1, vex::gearSetting::ratio18_1); 
motor Intake (vex::PORT2, vex::gearSetting::ratio6_1, true); 
motor left_back (vex::PORT15, vex::gearSetting::ratio18_1,true); 
motor left_front (vex::PORT16, vex::gearSetting::ratio18_1,true); 
motor right_back (vex::PORT20, vex::gearSetting::ratio18_1); 
motor right_front (vex::PORT19, vex::gearSetting::ratio18_1); 
motor Fly (vex::PORT10, vex::gearSetting::ratio6_1); 
motor Roll (vex::PORT3, vex::gearSetting::ratio18_1, true); 

//sensor defintions
inertial Inertial(vex::PORT9); 

//motor_group/drivetrain definitions
motor_group leftGroup(left_back, left_front);
motor_group rightGroup(right_back, right_front);
drivetrain driveTrain(leftGroup, rightGroup);
smartdrive smartDrive(leftGroup, rightGroup, Inertial, 8.539, 12, 12, distanceUnits::in);

//piston definitions
digital_out Flap(Brain.ThreeWirePort.F);
digital_out RightEnd(Brain.ThreeWirePort.H);
digital_out LeftEnd(Brain.ThreeWirePort.D);

competition Competition;

const int scale = 1;
const int scale2 = 2;
void flywheel(int speed){
  Fly.spin(fwd, speed*scale2, velocityUnits::pct);
}
void indexer(int speed){
  Index.spin(fwd, speed*scale, velocityUnits::pct);
}
void intake(int speed){
  Intake.spin(fwd, speed*scale, velocityUnits::pct);
}
void roller(int speed){
  Roll.spin(fwd, speed*scale, velocityUnits::pct);
}
void resetRotations(){
  leftGroup.setRotation(0,deg);
  rightGroup.setRotation(0,deg);
}
void calibrate(){
  Inertial.calibrate();
  while(Inertial.isCalibrating()){
    wait(100,msec);
  }
}
void drivePID(double distance, double degrees){
  double kP = 0.5;
  double kI = 0.0001;
  double kD = 0.75;

  double Err;
  double It = 0;
  double pErr = 0;

  int dt = 20;
  bool complete = true;
  
  //////////////////////////////////
  //Resets rotation before running loop
  resetRotations();
  ///////////////////////////////////

  while(complete) {
    //Proportional
    Err = distance - ((leftGroup.rotation(deg) + rightGroup.rotation(deg))/2);
    double P = kP * Err;
    //Integral
    It += (kI * Err * dt);
    //Derivative
    double D = kD * (Err - pErr)/dt;    
    //Moves robot laterally
    double output = P + It + D;

    // leftGroup.spin(fwd, output/2, pct);
    // rightGroup.spin(fwd, output/2, pct);
    driveTrain.drive(fwd, output/2, rpm);

    // cout << "Error = " << Err << ", It = " << It << ", D = " << D << ", output = " << output << endl;
    cout << "Inertial = " << Inertial.yaw() << endl;

    //conditional end
    if(fabs(Err) < fabs(distance)*0.0247){
      //checks to see if condiion is met
      cout << "We are in condition" << endl;
      
      //Stops motors
      // leftGroup.stop(brake);
      // rightGroup.stop(brake);
      driveTrain.drive(fwd, 0, rpm);

      //ends loop
      complete = false;
    }

    //prepares to rerun loop
    pErr = Err;

    vex::task::sleep(dt);
  }  
  wait(500, msec);
}
void pre_auton(void){
}
void autonomous(void){
  calibrate();
  drivePID(1000, 0);
  // drivePID(-365, 0);
  wait(1000, sec);
}
void usercontrol(void){
 while(true){
  //Drive
    int power = Controller1.Axis3.value();
    int turn = Controller1.Axis1.value();

    right_front.spin(fwd, power - turn, velocityUnits::pct);
    right_back.spin(fwd, power - turn, velocityUnits::pct);
    left_front.spin(fwd, power + turn, velocityUnits::pct);
    left_back.spin(fwd, power + turn, velocityUnits::pct);

    //Flywheel or Indexer
    if (Controller1.ButtonL1.pressing()){
      flywheel(100); 
    }
    else{
      flywheel(0);
    }
    if (Controller1.ButtonL2.pressing()){
      indexer(100);
    }
    else{
      indexer(0);
    }
    //Intake and Roller
    if (Controller1.ButtonR1.pressing()){
      intake(100);
      roller(100);
    }
    else if (Controller1.ButtonR2.pressing()){
      intake(-100);
      roller(-100);
    }
    else{
      intake(0);
      roller(0);
    }
    //Flap
    if (Controller1.ButtonUp.pressing()){
      Flap.set(true);
    }
    else if (Controller1.ButtonB.pressing()){
      Flap.set(false);
    }
    //Endgame
    while(Controller1.ButtonA.pressing()){
      if(Controller1.ButtonUp.pressing()){
        RightEnd.set(true);
        LeftEnd.set(true);
      }
      else if (Controller1.ButtonLeft.pressing()){
        LeftEnd.set(true);
      }
      else if (Controller1.ButtonRight.pressing()){
        RightEnd.set(true);
      }
    }
    wait(20,msec);
 }
}

int main() {
  pre_auton();

  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  while(true){
    wait(100,msec);
  }
}
#2

I think you might just have to tune the PID a bit more. Increased kD should help with error correction. Also, if this does not work, you might consider aiming your robot differently before the PID begins.

#3

no matter how I adjust the kD, it still drifts left.

#4

Check for friction at the drive train level. Put robot in drive mode and drive it forward in a straight line. If it drifts left instead of going perfectly straight, there’s your answer - it’s mechanical. Figure out where the friction is and get rid of it. Otherwise, since your PID function acts on the drivetrain class directly and not individual motors, I don’t see how that could make robot drift to the side.

1 Like