Reputation: 11
So the goal for this project is to create a robot that is able to complete a course in a predetermined time. I attempted to create a PID controller and setup a python script to test the code. Right now in place of robot controls I have a time.sleep() that the pid controller adjust the sleep time to make the robot finish the list of instructions on time. Could someone just look over my code and tell me how bad this code is? I cannot figure out how any of the math should work and I have spent way to much time on this. Thanks in advance.
import re
import time
import random
import matplotlib.pyplot as plt
import numpy as np
#from buildhat import MotorPair
probe_distance = 10 # distance of probe from center in mm
average_time = 6 # average time of the robot per function
class PID:
def __init__(self, Kp, Ki, Kd):
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.last_error = 0
self.integral = 0
def update(self, error, dt):
derivative = (error - self.last_error) / dt
self.integral += error * dt
output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
self.last_error = error
return output
class Move():
def __init__(self, time_target, file_path, motor_a, motor_b):
with open(file_path, 'r') as f:
self.file = f.readlines()
self.time_target = time_target
self.motor_a = motor_a
self.motor_b = motor_b
self.pid = PID(Kp=1.0, Ki=0.1, Kd=0.05)
#self.pair = MotorPair(motor_a, motor_b)
def run(self):
self.start_time = time.time()
#self.pair.set_default_speed(50) # this is the default speed of the robot
# The Pid controller will determine the speed of the robot based on the time it is taking to complete the tasks
m.xp = []
m.yp = []
i = 0
adjustment = 1
last_time_remaining = self.time_target
while True:
try:
line = self.file[i]
except IndexError:
break
# Execute command
time.sleep((60/len(self.file))-(60/len(self.file)*adjustment))
line = line.strip().removesuffix('\n').lower()
fchar = line[0]
time_remaining = self.time_target - (time.time() - self.start_time )
target = (self.time_target / len(self.file)) * (i + 1)
error = (self.time_target - target) - time_remaining # error = target - actual
dt = time_remaining / last_time_remaining
last_time_remaining = time_remaining
adjustment = self.pid.update(error, dt)
self.yp.append(error)
self.xp.append(time.time() - self.start_time)
print(f'Error: {error}, Adjustment: {adjustment}')
# 1. Initialize the system
m = Move(60, 'sequence.txt', 'C', 'D')
######################
### Begin Course ###
######################
# 2. Run the system
m.run()
print(m.xp)
print(m.yp)
xpoints = np.array(m.xp)
ypoints = np.array(m.yp)
plt.plot(xpoints, ypoints)
plt.show()
Upvotes: 0
Views: 35