autonomous gyro pid issue

VEX V5 Technical Support
1

Recently, I’ve been working on a function that controls my robot’s drivetrain in autonomous using two encoders on each side for position PID (albeit only using the proportional part) control and a gyro to keep the robot from drifting. While this function works as intended, once I called the function again it just malfunctioned and turned (video for reference). I’m using PROS, and this is a Cortex robot

This is the code I was running in


auto.c

:

#include "main.h"

void autonomous() {
   autoSmartRobot(1800, 1800, 127, 0, 0);
    autoSmartRobot(-1800, -1800, 127, 0, 0); //problematic part

}

And this is the


autoSmartRobot()

function definition:

void autoSmartRobot(int driveLeft, int driveRight, int limit, int ballIntake, int flyWheelRPM) {
    const int driveThreshold = 180;
    //reset encoders and gyro
    encoderReset(leftDriveEncoder);
    encoderReset(rightDriveEncoder);
    gyroReset(gyro);
   
    int leftDiff = driveLeft - encoderGet(leftDriveEncoder);
    int rightDiff = driveRight - encoderGet(rightDriveEncoder);

     //run robot subsystems, and when drivetrain target is reached stop drivetrain
    while (abs(leftDiff) > driveThreshold && abs(rightDiff) > driveThreshold) {
        autoStraightDriveTrain(driveLeft, driveRight);
        autoBallIntake(ballIntake);
        autoManualFlyWheelControl(flyWheelRPM);
        printf("left %d right %d\n", encoderGet(leftDriveEncoder), encoderGet(rightDriveEncoder));
        leftDiff = driveLeft - encoderGet(leftDriveEncoder);
        rightDiff = driveRight - encoderGet(rightDriveEncoder);
        delay(20);
    }
    driveTrainSetSpeed(0, 0);
}

And my


autoStraightDriveTrain()

function definition:

void autoStraightDriveTrain(int leftTarget, int rightTarget) {
    int leftCurrent = encoderGet(leftDriveEncoder);
    int rightCurrent = encoderGet(rightDriveEncoder);
    int robotHeading = gyroGet(gyro);

    int leftSpeed = positionPID(leftCurrent, leftTarget, POS_PID_DRIVETRAIN_L);
    int rightSpeed = positionPID(rightCurrent, rightTarget, POS_PID_DRIVETRAIN_R);

    leftSpeed = min(127, max(-127, leftSpeed));
    rightSpeed = min(127, max(-127, rightSpeed));

    leftSpeed += robotHeading * 25;
    rightSpeed -= robotHeading * 25;

    driveTrainSetSpeed(leftSpeed, rightSpeed);
}

Any ideas on how to fix this issue? I was thinking it was due to the negative drivetrain parameters in the second


autoSmartRobot()

call, but when I ran the second


autoSmartRobot()

call by itself (ie I commented out the first call) it worked fine.

2

Totally unrelated, but I couldn’t help but notice your profile pic. Greetings from the Philippines! Good luck with your code.

3

Can I assume this is Cortex/Robot C?

WAY over my head but will suggest less can be more:

If you need an auto-straightening routine you may want to fix what’s causing this in the first place. Many times it’s either assembly or a weak motor. Pick up the robot and go as slow as you can - if all wheel move at same speed you’re good.

Chances are you will see some wheels move and others not. Slowly increasing speed will give you an idea which ones are worse.

If you’re lucky you can prob. minimize autoStraighten()'s workload or preferably (IMHO) eliminate it.

Which encoders you are using - IME or shaft? We find IMEs too unreliable and get much better results with timed moves.

4

Sorry for not clarifying earlier, I’m using PROS with Cortex. We use shaft encoders on the drivetrain.

Thanks

5

Have you tried setting the motor values directly, without the gyro to see if it makes a difference?

6

Yes, one of my previous iterations of


autoSmartRobot()

had drivetrain code without the gyro, only using the shaft encoders for position PID targeting, which worked fine

7

Be sure that both encoders are properly reading positive and negative values. Also, might you try reducing your 25 multiplier down to just 1 or maybe 2 to see if that helps at all, as your math for driving straight may be overcompensating for a small amount of difference.

8

So I took your advice on reducing the multiplier, and the robot now works as intended in autonomous. Thank you everyone for your help!

9

It is usually a good idea to try to tune the multiplier to find out what works best, so make sure you try out other values as well, to make sure your robot is correcting just the right amount, and not too much. Good luck!