Jerky PD Loop

Programming Support VEXcode IQ Technical Support
1

Can anyone tell me why this PD loop is jerky?

#region VEXcode Generated Robot Configuration
from vex import *
import urandom

# Brain should be defined by default
brain = Brain()

# Robot configuration code
brain_inertial = Inertial()
left_drive_smart = Motor(Ports.PORT6, 0.75, True)
right_drive_smart = Motor(Ports.PORT7, 0.75, False)

drivetrain = SmartDrive(left_drive_smart, right_drive_smart, brain_inertial, 200)
Lifter = Motor(Ports.PORT4, False)
H_Wheel = Motor(Ports.PORT5, False)
Combine = Motor(Ports.PORT10, False)



# Make random actually random
def setRandomSeedUsingAccel():
    wait(100, MSEC)
    xaxis = brain_inertial.acceleration(XAXIS) * 1000
    yaxis = brain_inertial.acceleration(YAXIS) * 1000
    zaxis = brain_inertial.acceleration(ZAXIS) * 1000
    urandom.seed(int(xaxis + yaxis + zaxis))
    
# Set random seed 
setRandomSeedUsingAccel()

vexcode_initial_drivetrain_calibration_completed = False
def calibrate_drivetrain():
    # Calibrate the Drivetrain Inertial
    global vexcode_initial_drivetrain_calibration_completed
    sleep(200, MSEC)
    brain.screen.print("Calibrating")
    brain.screen.next_row()
    brain.screen.print("Inertial")
    brain_inertial.calibrate()
    while brain_inertial.is_calibrating():
        sleep(25, MSEC)
    vexcode_initial_drivetrain_calibration_completed = True
    brain.screen.clear_screen()
    brain.screen.set_cursor(1, 1)

#endregion VEXcode Generated Robot Configuration

global distTraveled
def posPID(setPoint):
    wTime = 15
    distTraveled = 0
    prevError = 0
    power = 0
    derivative = 0
    while True: #some condition 
        error = setPoint - distTraveled 
        derivative = error - prevError 
        prevError = error            
        kP = 0.1
        kD = 0.1
        power = error*kP + derivative*kD
        drivetrain.set_drive_velocity(power, PERCENT)
        mRPM = (power / 100) * 127
        toTravel = mRPM * 200 / 60 / wTime
        #print("\033[2J")
        #brain.screen.print(toTravel)
        #wait(20000000)
        drivetrain.drive_for(FORWARD, toTravel, MM)
        drivetrain.stop()
        distTraveled = distTraveled + toTravel
        if (error <= 1) and (error >= -1):
            break
        wait(wTime)
        
     
    pass

pass    

brain_inertial.calibrate()
brain_inertial.set_heading(0, DEGREES)
brain_inertial.set_rotation(0, DEGREES)
drivetrain.set_stopping(COAST)

posPID(276) # Drives 1 tile length
2

I think it is just you proportional and derivative is too small or to big.
Try setting d to 0 then increasing p until it is oscillating, then increase derivative until it fixes it.

2 Likes
3

The wait(15) at the end of the loop may be causing the issue

4

If I set kP and kD to 0, the power output will be 0

5

removing this dosen’t affect the jerkyness

6

oh, I think I may know why.

    drivetrain.drive_for(FORWARD, toTravel, MM)
    drivetrain.stop()

Every time you go through the loop, it’ll attempt to drive the distance, but then it will stop.

7

I just checked, no difference.

8

First of all I would recommend reading this article - Introduction to PID as well as this pdf - Documents. In your code you are making the velocity for the motors way more complicated than it needs to be. You can completely get rid of the mPRM rariable and toTravel variable. All you need to do is feed the power variable to the motor velocity. Another problem is that your wait time is way to long, I would recommend 5-10 milliseconds. You are waiting 15 seconds for your loop which means that if you set the left motor velocity to let’s say 50% from the PID loop and -50% for the right motor velocity. This will cause your robot to turn right. If you want to go to for example 90 degrees then your robot might turn to 90 degrees in 2 seconds. For the next 13 seconds the robot will continue turning causing error. I would also add the I part of the PID loop so that your robot isn’t too slow. This is also an unused variable that I would get rid of distTraveled = distTraveled + toTravel

9

Nevermind, I see what the distanceTraveled variable does. However I don’t know how this works

        mRPM = (power / 100) * 127
        toTravel = mRPM * 200 / 60 / wTime

It looks like it is converting the rpm to distance in some sort of unit, for my PID loop I use the motor encoders directy, here is how I do it -

    -- Note this code is in C++
    leftwheeldist = leftdrivemotor.position(degrees) * (wheel_circumference / 360) / gear_ratio;
    rightwheeldist = rightdrivemotor.position(degrees) * (wheel_circumference / 360) / gear_ratio;

I would also add another PID loop for error with heading so that if your robot gets hit off in the wrong direction by a certain amount of degrees it can correct itself. I would also add a function for turning.

10

This is replying to Banana Pi, it is probably because it is running into an infinite loop error, try setting the wait time to something very small, see to my other post

11

I was using mRPM as the RPM value of the velocity, toTravel was used as the distance to travel per iteration.

I’ll try that. thank you

12

You’re welcome, I would also read up on the articles in my post from before

13

How should I calculate the pover varible to be used as a percentage or RPM?

14

percentage (20 char)