def test_integrate(self): pid = PID(None, None, None, None, None, None, 'test_pid') err = ([], []) for i in np.arange(0, 9, 0.001): err[0].append(i) err[1].append(i * 2) ans = pid.integrate(*err)
def test_integrate(self): pid = PID(None, None, None, None, None, None, 'test_pid') err = ([],[]) for i in np.arange(0,9,0.001): err[0].append(i) err[1].append(i*2) ans = pid.integrate(*err)
MASS = 1.24324 # lbs TIME = 0 STEP = 0.01 def acceleration(velocity, density, input): cd = CD + (0.75 * input) return -GRAV - (cd * AREA * density * (velocity**2) / (2 * MASS)) AtmosEngine = Atmosphere() altitude = 1000 velocity = 700 accel = acceleration(velocity, AtmosEngine.density(altitude), 0) pid = PID(0.00025, 0.001, 0.1, 3500, 0, 1) pid_outputs = [] while velocity > 0: altitude += velocity * STEP velocity += accel * STEP p_alt = projected_altitude(accel, velocity, altitude) pid_output = pid.output(p_alt, STEP) accel = acceleration(velocity, AtmosEngine.density(altitude), pid_output) pid_outputs.append(pid_output) print(f"Altitude: {altitude}") plt.plot(pid_outputs) plt.show()
# PID ############################################### actposx = np.zeros(len(bb_path[0])) actposy = np.zeros(len(bb_path[0])) actvelx = np.zeros(len(bb_path[0])) actvely = np.zeros(len(bb_path[0])) pidvelx = np.zeros(len(bb_path[0])) pidvely = np.zeros(len(bb_path[0])) #controlx = PID(0.8*10.0, 1.5*0.4, 0.0, 1.0, -1.0) #controly = PID(0.8*10.0, 1.5*0.4, 0.0, 1.0, -1.0) controlx = PID(1.0, 0.01, 0.0, 1.0, -1.0) controly = PID(1.0, 0.01, 0.0, 1.0, -1.0) # Loop for i in xrange(0, len(bb_path[0])): if(i == 0): actposx[i] = -9.0 actposy[i] = -9.0 actvelx[i] = 0.0 actvely[i] = 0.0 else: actposx[i] = actposx[i-1] + actvelx[i-1]*0.1 + np.random.normal(0.0 ,0.05) actposy[i] = actposy[i-1] + actvely[i-1]*0.1 + np.random.normal(0.0 ,0.05) pidvelx[i] = controlx.calculate(actposx[i], bb_path[0,i])
def test_pid(self): # math for test: # target: 1 1 1 # current: 0 0.25 1 # err: 1 0.75 0 # P: 0.5 0.375 0 # I: 0 0.875 0.875 # D: 0 -0.125 -0.375 # out: 0.5 1.125 0.5 rospy.init_node('test_pid') target = Bunch() self.testOut = 0 def pidOut(out): diff = self.testOut - out self.assertEquals(abs(diff) < TOLERANCE, True) pid = PID(pidOut, 0.5, 0.5, 0.5, [-2, 2], [-1, 1], 'test_pid') target = 3 self.assertEquals(False, pid.set_target(target)) pid.update(0) target = 1 self.assertEquals(True, pid.set_target(target)) self.testOut = 0.5 pid.update(0) time.sleep(0.01) self.testOut = 1 pid.update(0.25) time.sleep(0.01) self.testOut = 0.5 pid.update(1.0) time.sleep(0.01)
from src.robot import Robot import numpy as np import threading import time as t #PID variables KP = 0.3 KI = 0.0 KD = 0.0 MAXOUTPUT = 0.08 MINOUTPUT = -0.08 #PID objects pidPosX = PID(KP, KI, KD, MAXOUTPUT, MINOUTPUT) pidPosY = PID(KP, KI, KD, MAXOUTPUT, MINOUTPUT) #ballbot R_MARKER = 12 R_PORT = '/dev/ttyACM0' ballbot = Robot(R_PORT) #Pyre node P_NODE_SELF = 'RPi' P_NODE_EXTERN = 'PC' P_GROUP_CAM = 'EAGLE' field = field(P_NODE_SELF, P_GROUP_CAM) print("Searching...") while (field.assignExternalUuid(P_NODE_EXTERN) == False):
def test_pid(self): # math for test: # target: 1 1 1 # current: 0 0.25 1 # err: 1 0.75 0 # P: 0.5 0.375 0 # I: 0 0.875 0.875 # D: 0 -0.125 -0.375 # out: 0.5 1.125 0.5 rospy.init_node('test_pid') target = Bunch() self.testOut = 0 def pidOut(out): diff = self.testOut - out self.assertEquals(abs(diff) < TOLERANCE, True) pid = PID(pidOut, 0.5, 0.5, 0.5, [-2,2], [-1,1], 'test_pid') target = 3 self.assertEquals(False, pid.set_target(target)) pid.update(0) target = 1 self.assertEquals(True, pid.set_target(target)) self.testOut = 0.5 pid.update(0) time.sleep(0.01) self.testOut = 1 pid.update(0.25) time.sleep(0.01) self.testOut = 0.5 pid.update(1.0) time.sleep(0.01)
# Other LOOPTIME = 0.1 # Globals running = True #set the running flag # Objects. ballbot = Robot(R_PORT) solver = solver(LOOPTIME) field = field(P_NODE_SELF, P_GROUP_CAM, R_MARKER) watchdog = watchdog(LOOPTIME) pidPosX = PID(C_KP_X, C_KI_X, C_KD_X, C_MAX_X, C_MIN_X) pidPosY = PID(C_KP_Y, C_KI_Y, C_KD_Y, C_MAX_Y, C_MIN_Y) def flush(q): """ Empty queue. """ while (q.empty() == False): q.get_nowait() def receiveState(): """ Function where the ballbot receives info about its state. state = [x, y, z, r, p, y]'