I am looking for some advice on my PID code I have been working on. I am very new to using PID, so I very well could be doing this very wrong. But as of right now I am trying to tune the robot. When trying to adjust the variables it will either: 1 move the drive train an inch then stop, or 2 oscillate back and forth frantically. I have tried adjusting many things apart of my code, not just the variables, to see if it would help but nothing has worked. I will be attaching my whole code below this paragraph for anyone who is willing to look over it.
VexV5 python Code:
#region VEXcode Generated Robot Configuration
from vex import *
import urandom
import time
import math
# Brain should be defined by default
brain=Brain()
# Robot configuration code
left_motor_a = Motor(Ports.PORT16, GearSetting.RATIO_6_1, True)
left_motor_b = Motor(Ports.PORT20, GearSetting.RATIO_6_1, True)
left_drive_smart = MotorGroup(left_motor_a, left_motor_b)
right_motor_a = Motor(Ports.PORT18, GearSetting.RATIO_6_1, False)
right_motor_b = Motor(Ports.PORT17, GearSetting.RATIO_6_1, False)
right_drive_smart = MotorGroup(right_motor_a, right_motor_b)
drivetrain_inertial = Inertial(Ports.PORT14)
drivetrain = SmartDrive(left_drive_smart, right_drive_smart, drivetrain_inertial, 299.24, 320, 40, MM, 0.6)
piston_2 = Led(brain.three_wire_port.h)
Piston_1 = Led(brain.three_wire_port.g)
motor_19 = Motor(Ports.PORT19, GearSetting.RATIO_6_1, False)
motor_9 = Motor(Ports.PORT9, GearSetting.RATIO_36_1, False)
motor_10 = Motor(Ports.PORT10, GearSetting.RATIO_6_1, False)
# wait for rotation sensor to fully initialize
wait(30, MSEC)
# Make random actually random
def initializeRandomSeed():
wait(100, MSEC)
random = brain.battery.voltage(MV) + brain.battery.current(CurrentUnits.AMP) * 100 + brain.timer.system_high_res()
urandom.seed(int(random))
# Set random seed
initializeRandomSeed()
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")
drivetrain_inertial.calibrate()
while drivetrain_inertial.is_calibrating():
sleep(25, MSEC)
vexcode_initial_drivetrain_calibration_completed = True
brain.screen.clear_screen()
brain.screen.set_cursor(1, 1)
# Calibrate the Drivetrain
calibrate_drivetrain()
def play_vexcode_sound(sound_name):
# Helper to make playing sounds from the V5 in VEXcode easier and
# keeps the code cleaner by making it clear what is happening.
print("VEXPlaySound:" + sound_name)
wait(5, MSEC)
# add a small delay to make sure we don't print in the middle of the REPL header
wait(200, MSEC)
# clear the console to make sure we don't have the REPL in the console
print("\033[2J")
#endregion VEXcode Generated Robot Configuration
#Drive PID Constants
Kp_drive = 0.35
Ki_drive = 0.005
Kd_drive = 0.01
#Turn PID Constants
Kp_turn = 0.5
Ki_turn = 0
Kd_turn = 0
drivetrain.set_stopping(COAST)
#PID Drive Function
def drive_pid(target_distance):
drivetrain.stop()
drivetrain.set_stopping(COAST)
error = 0
integral = 0
derivative = 0
last_error = 0
threshold = 1
integral_limit = 10
left_motor_a.set_position(0, DEGREES)
left_motor_b.set_position(0, DEGREES)
right_motor_a.set_position(0, DEGREES)
right_motor_b.set_position(0, DEGREES)
wait(100, MSEC)
loop_timeout = 10000
start_time = time.time()
while True:
current_position = abs((left_motor_a.position(DEGREES) + left_motor_b.position(DEGREES) + right_motor_a.position(DEGREES) + right_motor_b.position(DEGREES))/4)
error = target_distance - current_position
if abs(error) < integral_limit:
integral += error
integral = max(min(integral, 50), -50)
else:
integral *= 0.9
derivative = error - last_error
last_error = error
speed = (Kp_drive * error) + (Ki_drive * integral) + (Kd_drive * derivative)
speed = max(min(speed, 100), -100)
voltage_output = (speed/100) * 12.0
if abs(voltage_output) < 6.0 and voltage_output != 0:
voltage_output = 6.0
left_motor_a.spin(FORWARD if voltage_output > 0 else REVERSE, abs(voltage_output), VOLT)
left_motor_b.spin(FORWARD if voltage_output > 0 else REVERSE, abs(voltage_output), VOLT)
right_motor_a.spin(FORWARD if voltage_output > 0 else REVERSE, abs(voltage_output), VOLT)
right_motor_b.spin(FORWARD if voltage_output > 0 else REVERSE, abs(voltage_output), VOLT)
if abs(error) < threshold:
break
wait(20, MSEC)
brain.screen.clear_screen()
brain.screen.set_cursor(1,1)
brain.screen.print("Error: " + str(error))
brain.screen.set_cursor(2,1)
brain.screen.print("Current Pos: " + str(current_position))
brain.screen.set_cursor(3,1)
brain.screen.print("Current Vol: " + str(voltage_output))
brain.screen.set_cursor(4,1)
brain.screen.print("Current int: " + str(integral))
brain.screen.set_cursor(5,1)
brain.screen.print("Current speed: " + str(speed))
left_motor_a.stop()
left_motor_b.stop()
right_motor_a.stop()
right_motor_b.stop()
#PID Turn Function
def turn_pid(target_angle):
drivetrain.stop()
drivetrain.set_stopping(COAST)
error = 0
integral = 0
derivative = 0
last_error = 0
threshold = 4
integral_limit = 10
drivetrain.set_heading(0, DEGREES)
while True:
current_angle = drivetrain.heading(DEGREES)
error = target_angle - current_angle
if abs(error) < integral_limit:
integral += error
else:
integral = 0
derivative = error - last_error
last_error = error
speed = (Kp_turn * error) + (Ki_turn * integral) + (Kd_turn * derivative)
speed = max(min(speed, 50), -50)
drivetrain.set_turn_velocity(speed, PERCENT)
drivetrain.turn(RIGHT) if error > 0 else drivetrain.turn(LEFT)
if abs(error) < threshold:
break
wait(20, MSEC)
drivetrain.stop()
#Autonomous Routine
def autonomous():
drivetrain.stop()
drivetrain.set_stopping(COAST)
drive_pid(24)
#left_motor_a.set_position(0, DEGREES)
#left_motor_b.set_position(0, DEGREES)
#right_motor_a.set_position(0, DEGREES)
#right_motor_b.set_position(0, DEGREES)
#for i in range(10):
#brain.screen.clear_screen()
#brain.screen.set_cursor(1,1)
#brain.screen.print("L1:" +str(left_motor_a.velocity(PERCENT)) + "degrees")
#brain.screen.set_cursor(2,1)
#brain.screen.print("L2:" +str(left_motor_b.velocity(PERCENT)) + "degrees")
#brain.screen.set_cursor(3,1)
#brain.screen.print("R1:" +str(right_motor_a.velocity(PERCENT)) + "degrees")
#brain.screen.set_cursor(4,1)
#brain.screen.print("R2:" +str(right_motor_b.velocity(PERCENT)) + "degrees")
#wait(500, MSEC)
#Code Running Area
autonomous()