C++ Autonomous Code Inaccurate Using VEX Drivetrain Object

Programming Support
1

I have been experimenting with making accurate autonomous code without external sensors (using only the motor encoder values). I’ve seen multiple calculations and formulas to convert motor degrees to actual inches of movement or degrees of the robot turning. Having not found a formula that worked, I turned to the VEX drivetrain object that takes in the necessary values for motor degree conversion and has inbuilt driving and turning functions.

Below is my code that is incredibly inaccurate (the drive function massively overshoots and the turn functions spins well over 360 degrees for a 90 degree turn).

motor_group leftDriveMotors = motor_group(leftFront, leftBack);
motor_group rightDriveMotors = motor_group(rightFront, rightBack);
drivetrain robotDrive = drivetrain(leftDriveMotors, rightDriveMotors, 220, 317.5, 317.5, vex::distanceUnits::mm, 0.75); 
 
//NON FUNCTIONAL AUTON

void drive(int inches, std::string direction){
  if(direction == "fwd"){
    robotDrive.driveFor(vex::directionType::fwd, inches, vex::distanceUnits::in);
  }
  else{
    robotDrive.driveFor(vex::directionType::rev, inches, vex::distanceUnits::in);
  }
}

void turn(int degrees, std::string direction){
  if(direction == "left"){
    robotDrive.turnFor(vex::turnType::left, degrees, vex::rotationUnits::deg);
  }
  else{
    robotDrive.turnFor(vex::turnType::right, degrees, vex::rotationUnits::deg);
  }
}

This is the constructor: vex::drivetrain::drivetrain(motor_group &l, motor_group &r, double wheelTravel=320, double trackWidth=320, double wheelBase=130, distanceUnits unit=distanceUnits::mm, double externalGearRatio=1.0)

The wheel travel comes from VEX’s website (2.75 inch wheels); the trackWidth and wheelBase were all measured in inches (then converted to millimeters); and our gear ratio is 48teeth gear motor powered and 36teeth gear on the wheel (thus a 36/48 or 0.75 gear ratio).

Any ideas what I’m doing wrong? Or any ideas on how to use another approach? Last year I just used a trial and error approach that was accurate enough for driving but pretty inaccurate for turning, so I want a more consistent approach (again no sensors).

Let me know if you need more details (I recorded a video of it failing but the forum won’t let me upload videos here).

2

wheelBase is not necessarily distance between front and back wheels, it really means twice distance between driven wheels and center of rotation, in other words, it depends on which, and how many, wheels are driven by motors.

In general, drivetrain will not work well for large competition robots, it’s designed for classroom size robots. However, it should be at least close in terms of turning.

2 Likes
3

All right. I will send a picture of the bot later this week to see if I could get help figuring out the wheel base.

Is there a better, more consistent, and applicable for competitive use, way to convert motor degrees to actual robot rotation degrees or inches of movement? I’ve seen multiple formulas using wheelBase, trackWidth, and gearRatios but none of them seem to be accurate at all.

4

I wouldn’t recommend using the drivetrain class for competition at all.

1 Like
5

Thanks for the advice! Do you have a better method for converting inches of robot movement or degrees of robot turning to motor degrees? This would be used for the autonomous part of head to head matches and autonomous skills. Our team does not have access to external sensors so we are limited to using the motor encoders. Our drive setup is as follows from front to back on each side: 2.75" omni wheel with a 36tooth gear, green motor with a 48tooth gear, 2.75" traction wheel with a 36tooth gear, green motor with a 48tooth gear, 2.75" omni wheel with a 36tooth gear.

6

Motor encoders are really not accurate enough for competition. To be competitive you’ll need an IMU with PID control running.

But while you don’t have access to one, my best advice is to create a PID loop for controlling your motors rather than raw turnFor commands.

4 Likes
7

This is the one full tutorial for coding PID in vex that I found: https://www.youtube.com/watch?v=_Itn-0d340g

This is the code it had me write and I have a couple of questions as it is my first time coding PID.

  • Do you know if this code should work?

  • What values for kP, kD, and kI (tutorial said I shouldn’t include this for drive PID, should I?) to start?

  • How should I modify these values based on what I see during testing?

  • Is there a way for me to convert motor degrees (which this PID loop takes) to inches of movement or degrees of robot rotation?

// Constant Settings
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;

double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;

//Dynamic Settings
int desiredValue = 600;
int desiredTurnValue = 0;

int error; //Sensor Value - Desired Value : Position
int prevError = 0; //Position 20msec ago
int derivative;
int totalError = 0;//totalError += error

int turnError; //Sensor Value - Desired Value : Position
int turnPrevError = 0; //Position 20msec ago
int turnDerivative;
int turnTotalError = 0;//totalError += error

bool resetDriveSensors = false;

bool enableDrivePID = true;

int drivePID(){
  while(enableDrivePID){

    if(resetDriveSensors){
      resetDriveSensors = false;
      driveMotors.resetPosition();
    }
    //Get Motor Positions
    int lfPosition = leftFront.position(vex::rotationUnits::deg);
    int rfPosition = rightFront.position(vex::rotationUnits::deg);
    int lbPosition = leftBack.position(vex::rotationUnits::deg);
    int rbPosition = rightBack.position(vex::rotationUnits::deg);

  ////////////////////////////////////////////////////////////////
  //                  Lateral Movement PID                      //
  ////////////////////////////////////////////////////////////////


    //Get Avg Motor Positions
    int frontMotorAvgPosition = (lfPosition + rfPosition)/2;
    int backMotorAvgPosition = (lbPosition + rbPosition)/2;

    //Potential
    error = backMotorAvgPosition - desiredValue;

    //Derivative
    derivative = error - prevError;

    //Integral
    totalError += error;

    double lateralMotorDegrees = (error * kP + derivative * kD + totalError * kI)/360;

  ////////////////////////////////////////////////////////////////
  //                  Turn Movement PID                         //
  ////////////////////////////////////////////////////////////////

    //Get Avg Motor Positions
    int frontMotorTurnDifference= lfPosition - rfPosition;
    int backMotorTurnDifference = lbPosition - rbPosition;

    //Potential
    turnError = backMotorTurnDifference - desiredValue;

    //Derivative
    turnDerivative = turnError - turnPrevError;

    //Integral
    turnTotalError += turnError;

    double turnMotorDegrees = (turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI)/360;

    leftDriveMotors.spin(vex::directionType::fwd, lateralMotorDegrees + turnMotorDegrees, vex::voltageUnits::volt);
    rightDriveMotors.spin(vex::directionType::fwd, lateralMotorDegrees - turnMotorDegrees, vex::voltageUnits::volt);


    prevError = error;
    turnPrevError = turnError;
    vex::task::sleep(20);
  }
  return 1;
}


void autonomous(void) {
  vex::task PIDTask(drivePID);

  resetDriveSensors = true;
  desiredValue = 600;
  vex::task::sleep(100);

  resetDriveSensors = true;
  desiredTurnValue = 300;
}

Thank you so much for your help so far!

8

to change the kp, make it a little lower than the highest you can put it without the robot oscillating. For ki, it isn’t necessary and can lead to a lot of complications, so I usually leave it at 0. For kd, (after kp is tuned), have the brain/controllers print the error and repeatedly increase or decrease kd until the error is as close to zero as possible.

1 Like
9

But I can’t start everything at 0 or the motors won’t turn at all. This forum post recommends that I start kP at 1 (How to set KP value of a pid program - #3 by ZachDaChampion) and then increment until it overshoots or starts oscillating. Is it fine to start kD at 0 until I have tuned kP, and then increment kD until the error is almost 0.

Also any tips on how to convert robot movement or turning to motor degree rotations. Right now, this PID control takes in motor degrees, but I don’t know how many degrees the motors need to turn to drive say 24" or turn 45 degrees.

10

This is in essence what I was trying to say.

For the turning sensor, I would recommend getting an inertial sensor and replace your turn difference variables with a variable that stores the inertial sensor’s rotation. (Also if you go the route of an inertial sensor, don’t forget to calibrate it)

11

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.