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]'
