def _get_gradient(self, delta=0.01) -> (float, float, float):
"""Returns the gradient of the mean squared error of the Simulation with respect to each kp, ki, and kd value.
Arguments:
delta {float} -- The small change in each kp, ki, and kd value.
Returns:
(float, float, float) -- The gradient of the mean squared error of the Simulation with respect to each kp, ki, and kd value.
"""
mse_before = _get_mse(deepcopy(self._controller), len(self._ts),
self._dt)
dp_controller = PIDController(deepcopy(self._sim), self._setpoint,
self._controller.kp + delta,
self._controller.ki, self._controller.kd)
dp = _get_mse(dp_controller, len(self._ts), self._dt) - mse_before
di_controller = PIDController(deepcopy(self._sim), self._setpoint,
self._controller.kp,
self._controller.ki + delta,
self._controller.kd)
di = _get_mse(di_controller, len(self._ts), self._dt) - mse_before
dd_controller = PIDController(deepcopy(self._sim), self._setpoint,
self._controller.kp, self._controller.ki,
self._controller.kd + delta)
dd = _get_mse(dd_controller, len(self._ts), self._dt) - mse_before
return (dp / delta, di / delta, dd / delta)
def pid_thread():
global pos
controller = PIDController()
while True:
lock.acquire()
try:
controller.controll(pos)
finally:
lock.release()
def PID_RootLocusMultiple(self):
self.SM = SpringMassVis(l=1.0, k=40.0, b=8.80) #cm
self.C = PIDController(t=0.01, P=50, I=0, D=25.6)
self.C.sysOpenCE(self.SM.CE)
self.C.sysClosedCE()
f, axx = plt.subplots(1, 1)
self.C.sysRecomp(P=15, I=50, D=1)
out = control.root_locus(self.C.sysCCE / self.C.D, Plot=False)
self.C.plotRootLocus(out, axx, 'x')
def PI_ImpulseResponse(self):
self.SM = SpringMassVis(l=1.0, k=40.0, b=8.80) #cm
self.C = PIDController(t=0.01, P=50, I=100)
self.C.sysOpenCE(self.SM.CE)
self.C.sysClosedCE()
f, axx = plt.subplots(1, 1)
self.C.sysRecomp(P=25, I=75)
out = self.C.plotImpulseResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=50, I=150)
out = self.C.plotImpulseResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=75, I=225)
out = self.C.plotImpulseResponse(self.C.sysCCE, Plot=False, axx=axx)
def P_stepResponse(self):
self.P = PendulumVis(l=1.0, m=1.0, b=0.001, g=9.8)
self.C = PIDController(t=0.01, P=50, I=0, D=0)
self.C.sysOpenCE(self.P.CE)
self.C.sysClosedCE()
f, axx = plt.subplots(1, 1)
self.C.sysRecomp(P=100)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=500)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=2000)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
plt.plot([0, max(out[1])], [1, 1], 'k--')
def P_StepResponse(self):
self.SM = SpringMassVis(l=0.0, m=1, k=40.0, b=8.80) #cm
self.C = PIDController(t=0.01, P=50, I=0, D=0)
self.C.sysOpenCE(self.SM.CE)
self.C.sysClosedCE()
f, axx = plt.subplots(1, 1)
self.C.sysRecomp(P=100)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=500)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=2000)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
plt.plot([0, max(out[1])], [1, 1], 'k--')
def testControlValues(
self): # test a bunch of values and see which one is best
resp = []
for p in np.arange(0, 100, 10):
for d in np.arange(0, 100, 10):
# p = 5.0
i = 0.0
# d = 0.0
self.Controller = PIDController(P=p, I=i, D=d)
self.Controller.setQTarget([0, 0])
resp.append(self.simulate())
print('PID: (%s, %s, %s) has steady state error %s' %
(p, i, d, resp[-1]))
def __init__(self, kp, ki, kd):
self.kp = kp
self.ki = ki
self.kd = kd
self.gyro_vertical_center_x = 5.7
self.gyro_vertical_center_y = 7.052
self.median_filter = [0.0, 0.0, 0.0, 0.0, 0.0]
gyro_scale = 131.0
accel_scale = 16384.0
RAD_TO_DEG = 57.29578
M_PI = 3.14159265358979323846
now = time.time()
K = 0.98
K1 = 1 - K
time_diff = 0.01
gyroAngleX = 0.0
gyroAngleY = 0.0
gyroAngleZ = 0.0
accAngX = 0.0
CFangleX = 0.0
CFangleX1 = 0.0
K = 0.98
FIX = -12.89
self.gyroFilter = GyroFilter()
(self.gyro_scaled_x, self.gyro_scaled_y, self.gyro_scaled_z, self.accel_scaled_x, self.accel_scaled_y,
self.accel_scaled_z) = self.gyroFilter.read_all()
(self.accel_vertical_center_x, self.accel_vertical_center_y,
self.accel_vertical_center_z) = self.gyroFilter.get_accel_center_xyz()
pid_set_point = 0.00
self.pid = PIDController(kp, ki, kd, pid_set_point)
self.DataReaderMain = MyDataReader()
self.data_reader = self.DataReaderMain.get_xyx()
# The angle of the Gyroscope
gyroAngleX += self.gyro_scaled_x * time_diff
gyroAngleY += self.gyro_scaled_y * time_diff
gyroAngleZ += self.gyro_scaled_z * time_diff
accAngX = (math.atan2(self.accel_scaled_x, self.accel_scaled_y) + M_PI) * RAD_TO_DEG
# math.atan2 numeric value between -PI and PI representing the angle theta of an (x, y) point.
CFangleX = K * (CFangleX + self.gyro_scaled_x * time_diff) + (1 - K) * accAngX
accAngX1 = self.get_x_rotation(self.accel_scaled_x, self.accel_scaled_y, self.gyro_scaled_z)
# accAngX1 = get_x_rotation(accel_scaled_x, accel_scaled_y, gyro_scaled_x) or this one - test?
CFangleX1 = (K * (CFangleX1 + self.gyro_scaled_x * time_diff) + (1 - K) * accAngX1)
def PI_RootLocusMultiple(self):
self.P = PendulumVis(l=1.0, m=1.0, b=0.001, g=9.8)
self.C = PIDController(t=0.01, P=500, I=100)
self.C.sysOpenCE(self.P.CE)
self.C.sysClosedCE()
f, axx = plt.subplots(1, 1)
self.C.sysRecomp(I=100)
out = control.root_locus(self.C.sysOCE / self.C.I, Plot=False)
self.C.plotRootLocus(out, axx, 'x')
self.C.sysRecomp(I=500)
out = control.root_locus(self.C.sysOCE / self.C.I, Plot=False)
self.C.plotRootLocus(out, axx, ':')
self.C.sysRecomp(I=2000)
out = control.root_locus(self.C.sysOCE / self.C.I, Plot=False)
self.C.plotRootLocus(out, axx, '--')
def __init__(self):
"""Initializes drone PIDs (x, y, z, yaw) and LP filter."""
# Control attributes
self.__parameters = ControlParameters()
# Filter for manual position setpoint
par = rospy.get_param('~control/sp_filter')
self.__filter = LowPassFilter(par, numpy.zeros(6))
# Loads PID parameters
pars = rospy.get_param('~control/pid')
r, p, y, t = (pars['roll'], pars['pitch'], pars['yaw'], pars['thrust'])
# Roll, pitch, yaw, thrust
self.__pids = [
PIDController(r['kp'], r['ki'], r['kd'], r['min'], r['max'],
r['ff']),
PIDController(p['kp'], p['ki'], p['kd'], p['min'], p['max'],
p['ff']),
PIDController(y['kp'], y['ki'], y['kd'], y['min'], y['max'],
y['ff']),
PIDController(t['kp'], t['ki'], t['kd'], t['min'], t['max'],
t['ff'])
]
def PD_ImpulseResponse(self):
self.P = PendulumVis(l=1.0, m=1.0, b=0.001, g=9.8)
self.C = PIDController(t=0.01, P=50, I=0, D=25.6)
self.C.sysOpenCE(self.P.CE)
self.C.sysClosedCE()
f, axx = plt.subplots(1, 1)
self.C.sysRecomp(P=100, D=10)
out = self.C.plotImpulseResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=270, D=27)
out = self.C.plotImpulseResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=500, D=50)
out = self.C.plotImpulseResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=750, D=75)
out = self.C.plotImpulseResponse(self.C.sysCCE, Plot=False, axx=axx)
axx.plot([0, max(out[1])], [1, 1], 'k--')
def PI_RootLocusMultiple(self):
self.SM = SpringMassVis(l=1.0, k=40.0, b=8.80) #cm
self.C = PIDController(t=0.01, P=50, I=100)
self.C.sysOpenCE(self.SM.CE)
self.C.sysClosedCE()
f, axx = plt.subplots(1, 1)
self.C.sysRecomp(I=100)
out = control.root_locus(self.C.sysOCE / self.C.I, Plot=False)
self.C.plotRootLocus(out, axx, 'x')
self.C.sysRecomp(I=500)
out = control.root_locus(self.C.sysOCE / self.C.I, Plot=False)
self.C.plotRootLocus(out, axx, ':')
self.C.sysRecomp(I=2000)
out = control.root_locus(self.C.sysOCE / self.C.I, Plot=False)
self.C.plotRootLocus(out, axx, '--')
def PID_StepResponse(self):
self.SM = SpringMassVis(l=0.0, k=40.0, b=8.80) #cm
self.C = PIDController(t=0.01, P=50, I=0, D=25.6)
self.C.sysOpenCE(self.SM.CE)
self.C.sysClosedCE()
f, axx = plt.subplots(1, 1)
D = 5
self.C.sysRecomp(P=15 * D, I=50 * D, D=D)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
D = 10
self.C.sysRecomp(P=15 * D, I=50 * D, D=D)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
D = 15
self.C.sysRecomp(P=15 * D, I=50 * D, D=D)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
axx.plot([0, max(out[1])], [1, 1], 'k--')
def test_new_controller_update():
pid = PIDController(kp=2.0, ki=0.5, kd=0.25, limmin=-10.0, limmax=10.0)
assert (pid is not None)
pid.tau = 0.02
pid.limMinInt = -5.0
pid.limMaxInt = 5.0
pid.T = 0.01
setpoint = 1.0
print("Time(s)\tSystemOut\tController Out")
t = 0.0
while t < 4.0:
measurement = _SimulatorUpdate(pid.out)
pid.update(setpoint, measurement)
print(f"{t}\t{measurement}\t{pid.out}")
t += 0.01
def RLChange(self):
self.SM = SpringMassVis(l=0.0, k=40.0, b=8.80) #cm
self.C = PIDController(t=0.01, P=50, I=0, D=0)
self.C.sysOpenCE(self.SM.CE)
self.C.sysClosedCE()
f, axx = plt.subplots(1, 1)
self.C.sysRecomp(P=1, I=0, D=0)
out = control.root_locus(self.C.sysCCE / self.C.P, Plot=False)
self.C.plotRootLocus(out, axx, 'x')
self.C.sysRecomp(P=1, I=3, D=0)
out = control.root_locus(self.C.sysCCE / self.C.P, Plot=False)
self.C.plotRootLocus(out, axx, 'x')
self.C.sysRecomp(P=1, I=0, D=0.1)
out = control.root_locus(self.C.sysCCE / self.C.D, Plot=False)
self.C.plotRootLocus(out, axx, 'x')
self.C.sysRecomp(P=1, I=5, D=0.1)
out = control.root_locus(self.C.sysCCE / self.C.D, Plot=False)
self.C.plotRootLocus(out, axx, 'x')
control.root_locus(self.C.sysCCE / self.C.D, Plot=True)
def __init__(self, webcontroller, photographer, cleaner_side="right"):
print("Initializing CleaningRobot")
self.webcontroller = webcontroller
self.photographer = photographer
self.cleaner_side = cleaner_side
self.FOLLOW_WALL_SET_POINT = 0.40
self.FOLLOW_WALL_SET_POINT_START = 0.35
self.FOLLOW_WALL_SET_POINT_END = 0.50
self.OBSTACLE_DETECTION_SET_POINT_FAR = 1.00
self.STUCK_DETECTION_DISTANCE = 0.20
self.STUCK_MAX_COUNTER = 30
self.PID_controller = PIDController(self.FOLLOW_WALL_SET_POINT)
self.current_speed = Twist()
self.cur_command = "start"
self.previous_searching_time = 0
self.state = 0
self.previous_state = 0
self.robot_stuck_counter = 0
self.state_desc = {
0: 'Idle',
1: 'Finding wall',
2: 'Following wall',
3: 'Stuck',
4: 'Turning'
}
self.directions = {
'left':10,
'fleft':10,
'front1':10,
'front2':10,
'front':10,
'fright':10,
'right':10,
}
self.publisher = rospy.Publisher('/cmd_vel', Twist, queue_size =1 )
self.subscriber = rospy.Subscriber('/scan',LaserScan, self.callback_laser_scan)
def __init__(self, sim: Simulator, setpoint: float, t0: float, t1: float,
dt: float):
"""Constructs a PIDTuner.
Arguments:
sim {Simulator} -- The Simulator to use for the PIDTuner.
setpoint {float} -- The setpoint for the PIDTuner.
t0 {float} -- The initial time to start the simulation at.
t1 {float} -- The final time to end the simulation at.
dt {float} -- The incremental time step in each simulation.
"""
self._sim = sim
self._setpoint = setpoint
self._dt = dt
num_pts = (t1 - t0) / dt
self._ts = np.linspace(t0, t1, num=num_pts)
self._controller = PIDController(
sim, setpoint, 1, 1, 1) # TODO: Optimize initial PID values
self._prev_grad = ()
self._prev_vals = ()
pX = SCREEN_WIDTH / 2 #drone position X
pY = SCREEN_HEIGHT / screenScale #drone position Y
#and drone controls
thrust = 0.0
aileron = 0.0
#initalise some constants
gravity = 9.81
mass = 0.4 #1.0
fuel = 1000.0
maxThrust = 4.5 #20.0
isAltLock = False
altLockHeight = 0
#PIDControllers
heightPID = PIDController(1.0, 0, 0)
################################################################################
"""
drawDrone Not surprisingly, it draws the drone on the screen
@param x
@param y
"""
def drawDrone(x, y):
pts = [
(-9, 1),
(-9, 2),
(9, 2),
(9, 1), #central slice
def main(args=None):
global aX, aY, vX, vY, pX, pY
global thrust, aileron, throttle, theta, thetaDot, isAltLock, altLockHeight
# Setup the clock for a decent framerate
clock = pygame.time.Clock()
lapTimer = 0 #counts game loops during lap e.g. 1/30FPS
completedLaps = [] #list of previous completed lap times
isRaceRunning = False #flips to true when you trip the first timing gate
heightPID = PIDController(0.4, 0.025, 0.35) #5,1.0,0.5 #5,0.5,0.5
rollVelPID = PIDController(0.03, 0.04, 0.000005) #0.03,0.04,0.000005
rollAnglePID = PIDController(0.06, 0.01, 0.005)
throttleNudgePID = PIDController(0.4, 0, 0)
aileronNudgePID = PIDController(0.1, 0.02, 0.005)
#these are the bitmaps for the obstacles
flag = createFlag()
xGate = createXGate()
gate = createGate()
tunnel = createTunnel()
startFinish = createStartFinish()
course = CourseSequencer()
#course.addWaypoint(586,175,40,0,0)
course.addWaypoint(331, 166, 20, -30, 0)
course.addWaypoint(60, 190, 40, 0, 0)
course.addWaypoint(330, 298, 20, 0, 0)
course.addWaypoint(459, 465, 20, -30, 0)
course.addWaypoint(238, 465, 20, -30, 0)
course.addWaypoint(336, 375, 30, 28, 140) #note +100 is reversed
course.addWaypoint(478, 244, 20, 0, 0)
course.addWaypoint(586, 175, 40, 0, 0) #start finish
pX = 5.65 #this puts you on the grass under the start finish grid ready to go
pY = 2.38
course.next = 7 #this sets the waypoint to the start finish straight where timing starts
running = True
animationTimer = 0
while running:
# Fill the background with black
screen.fill(BLACK)
screen.blit(flag, (80, 120))
pygame.draw.rect(screen, GREEN, (70, 188, 30, 10))
screen.blit(flag, (500, 120))
pygame.draw.rect(screen, GREEN, (490, 188, 30, 10))
pygame.draw.rect(screen, GREEN, (610, 188, 40, 10))
screen.blit(flag, (80, 360))
screen.blit(flag, (520, 360))
screen.blit(gate, (420, 200))
screen.blit(xGate, (224, 232))
screen.blit(gate, (180, 420))
screen.blit(gate, (400, 420))
screen.blit(tunnel, (220, 130))
screen.blit(startFinish, (534, 185))
pygame.draw.rect(screen, GREEN, (100, 170, 390, 10))
pygame.draw.rect(screen, GREEN, (0, 306, 255, 10))
pygame.draw.rect(screen, GREEN, (360, 250, 260, 10))
pygame.draw.rect(screen, GREEN, (0, 470, 640, 10))
pygame.draw.rect(screen, GREEN, (140, 380, 360, 10)) #xgate
#next waypoint
#pygame.draw.rect(screen,WHITE,course.getNextRect())
drawWaypoint(course.getNextRect(), animationTimer)
#control inputs
for event in pygame.event.get():
if event.type == QUIT:
running = False
elif event.type == KEYDOWN:
#if event.key==K_LEFT:
#if event.key==K_RIGHT:
#if event.key==K_UP:
#if event.key==K_DOWN:
if event.key == K_ESCAPE:
running = False
elif event.key == K_h:
altLockHeight = pY
isAltLock = not isAltLock
#end if
#end for
#if you're pressing SPACE increase the thrust, otherwise decrease the thrust
keys = pygame.key.get_pressed()
if keys[K_p]: #throttle up
throttle = min(throttle + 0.05, 1.0)
elif keys[K_l]: #throttle down
throttle = max(throttle - 0.05, -1.0)
else:
throttle = 0
#use a PID to link throttle position to a desired vertical speed (vY)
thrust = (mass * gravity) + (maxThrust / 2) * throttleNudgePID.process(
vY, throttle, 1 / 30)
thrust = max(0, thrust)
thrust = min(maxThrust, thrust)
###
if keys[K_q]: #left aileron
aileron = max(aileron - 0.05, -1.0)
elif keys[K_w]: #right aileron
aileron = min(aileron + 0.05, 1.0)
else:
aileron = 0
#altitude lock code
if isAltLock:
#thrust = mass * gravity + 10.0*(altLockHeight - pY)
thrust = (mass * gravity) + (maxThrust / 2) * heightPID.process(
pY, altLockHeight, 1 / 30)
thrust = max(0, thrust)
thrust = min(maxThrust, thrust)
#drone dynamics
#angular dynamics - version 4 angle PID control
#a2 = rollAnglePID.process(theta,aileron*Cla,1/30)
a2 = aileronNudgePID.process(vX, aileron * Cla, 1 / 30)
L = a2 - Cld * thetaDot #L (torque) = roll power (torque) minus drag
pDot = L / Izz #A=F/m
thetaDot = pDot * (1.0 / 30.0
) #integrate pDot to get p, which is thetaDot
theta += thetaDot * (1.0 / 30.0)
#speed and drag
vsq = vX * vX + vY * vY
if (vsq > 0):
v = math.sqrt(vsq)
dx = vX / v
dy = vY / v
else:
v = 0
dx = 0
dy = 0
fX = thrust * math.sin(theta) - Cd * vsq * dx #force +ve=right
fY = -mass * gravity + thrust * math.cos(
theta) - Cd * vsq * dy #force +ve=up
#acceleration and velocity
aX = fX / mass
aY = fY / mass #F=ma, a=F/m
vX += aX * (1.0 / 30.0)
vY += aY * (1.0 / 30.0)
#update position
pX += vX * (1.0 / 30.0)
pY += vY * (1.0 / 30.0) #30 FPS, 1 pixel per metre
if pX < 25 / screenScale: #was 0
#pX = SCREEN_WIDTH/screenScale #wrap left to right
pX = 25 / screenScale
vX = -vX
theta = -theta
if (pX > (SCREEN_WIDTH - 25) / screenScale):
#pX = 0 #wrap right to left
px = (SCREEN_WIDTH - 25) / screenScale
vX = -vX
theta = -theta
if pY <= 11 / screenScale: #drone height below zero line i.e. the collision bounding box
pY = 11 / screenScale
theta = 0
vX = 0.4 * vX
vY = -0.4 * vY #bounce off ground! and lose some energy in the process
#collision detection
sX = int(pX * screenScale)
sY = int(SCREEN_HEIGHT - pY * screenScale)
lowPixel = screen.get_at((sX, sY + 8))
leftPixel = screen.get_at((sX - 20, sY))
rightPixel = screen.get_at((sX + 20, sY))
if lowPixel == GREEN or lowPixel == RED: #crash down
theta = 0
vX = 0.4 * vX
vY = -0.4 * vY
pY += vY * 2.5 / 30 #stop it going through the ground
if leftPixel == RED: #crash left (this is useful, the constant vX PID makes it spin out of control!)
vX = -0.4 * vX
if rightPixel == RED: #crash right
vX = -0.4 * vX
#draw instruments, fuel, vertical, horizontal
#drawInstruments()
drawTiming(lapTimer, completedLaps)
#draw drone
drawDrone(sX, sY, theta)
#waypoint test
(isHit, isLapComplete, jump) = course.testWaypoint(sX, sY)
if isHit:
pX += jump[0] / screenScale
pY += jump[1] / screenScale
isRaceRunning = True
if isLapComplete:
if lapTimer > 1: #you get timer=1 on the initial trigger fist time you cross start - it's not a lap
completedLaps.append(
lapTimer / 30.0) #log the lap time by pushing to list
#print lap here for AI? print("lap complete: "+str(completedLaps[len(completedLaps)-1]))
course.startLap() #and start again
lapTimer = 0
#write out logging data
#print(str(pY)+","+str(thrust)+","+str(altLockHeight)+","+str(aY)+","+str(vY))
animationTimer = (animationTimer + 1) % 32
if isRaceRunning:
lapTimer += 1
#double buffer flip
pygame.display.flip()
# Ensure program maintains a rate of 30 frames per second
clock.tick(30)
#endwhile running
#exit gracefully, shutting down the pygame system cleanly
pygame.quit()
def main(args=None):
global aX,aY,vX,vY,pX,pY
global thrust, aileron, theta, thetaDot, p, isAltLock, altLockHeight
# Setup the clock for a decent framerate
clock = pygame.time.Clock()
heightPID = PIDController(0.4,0.025,0.35) #5,1.0,0.5 #5,0.5,0.5
rollVelPID = PIDController(0.03,0.04,0.000005) #0.03,0.04,0.000005
rollAnglePID = PIDController(0.06,0.01,0.005)
running = True
while running:
# Fill the background with black
screen.fill(BLACK)
#control inputs
for event in pygame.event.get():
if event.type==QUIT:
running=False
elif event.type==KEYDOWN:
#if event.key==K_LEFT:
#if event.key==K_RIGHT:
#if event.key==K_UP:
#if event.key==K_DOWN:
if event.key==K_ESCAPE:
running=False
elif event.key==K_h:
altLockHeight=pY
isAltLock=not isAltLock
#end if
#end for
#if you're pressing SPACE increase the thrust, otherwise decrease the thrust
keys = pygame.key.get_pressed()
if keys[K_p]: #throttle up
thrust+=0.05
if thrust>maxThrust:
thrust = maxThrust
if keys[K_l]: #throttle down
thrust-=0.05
if (thrust<0):
thrust=0
if keys[K_q]: #left aileron
aileron = max(aileron-0.05,-1.0)
if keys[K_w]: #right aileron
aileron = min(aileron+0.05,1.0)
#altitude lock code
if isAltLock:
#thrust = mass * gravity + 10.0*(altLockHeight - pY)
thrust = (mass*gravity) + (maxThrust/2) * heightPID.process(pY,altLockHeight,1/30)
thrust = max(0,thrust)
thrust = min(maxThrust,thrust)
#drone dynamics
#angular dynamics - version 1 direct control
#theta=aileron
###
#angular dynamics - version 2 acceleration control
#L = aileron * Cla - Cld*thetaDot #roll power (torque) minus drag
#pDot = L / Izz #A=F/m
#thetaDot = pDot*(1.0/30.0) #integrate pDot to get p, which is thetaDot
#theta+=thetaDot*(1.0/30.0)
###
#angular dynamics - version 3 velocity PID control
#a2 = rollVelPID.process(thetaDot,aileron*Cla,1/30)
#L = a2 - Cld*thetaDot #L (torque) = roll power (torque) minus drag
#pDot = L / Izz #A=F/m
#thetaDot = pDot*(1.0/30.0) #integrate pDot to get p, which is thetaDot
#theta+=thetaDot*(1.0/30.0)
###
#angular dynamics - version 4 angle PID control
a2 = rollAnglePID.process(theta,aileron*Cla,1/30)
L = a2 - Cld*thetaDot #L (torque) = roll power (torque) minus drag
pDot = L / Izz #A=F/m
thetaDot = pDot*(1.0/30.0) #integrate pDot to get p, which is thetaDot
theta+=thetaDot*(1.0/30.0)
#speed and drag
vsq = vX*vX+vY*vY
if (vsq>0):
v = math.sqrt(vsq)
dx = vX/v
dy = vY/v
else:
v=0
dx=0
dy=0
fX = thrust * math.sin(theta) - Cd*vsq*dx #force +ve=right
fY = -mass*gravity + thrust * math.cos(theta) - Cd*vsq*dy #force +ve=up
#acceleration and velocity
aX = fX/mass
aY = fY/mass #F=ma, a=F/m
vX+= aX*(1.0/30.0)
vY+= aY*(1.0/30.0)
#update position
pX+= vX*(1.0/30.0)
pY+= vY*(1.0/30.0) #30 FPS, 1 pixel per metre
if pX<0:
pX = SCREEN_WIDTH/screenScale #wrap left to right
if (pX>SCREEN_WIDTH/screenScale):
pX = 0 #wrap right to left
if pY<=11/screenScale: #drone height below zero line i.e. the collision bounding box
pY=11/screenScale
theta = 0
vX = 0.4 * vX
vY = -0.4 * vY #bounce off ground! and lose some energy in the process
#draw instruments, fuel, vertical, horizontal
drawInstruments()
#draw drone
drawDrone(pX*screenScale,SCREEN_HEIGHT-pY*screenScale,theta)
#write out logging data
#print(str(pY)+","+str(thrust)+","+str(altLockHeight)+","+str(aY)+","+str(vY))
#double buffer flip
pygame.display.flip()
# Ensure program maintains a rate of 30 frames per second
clock.tick(30)
#endwhile running
#exit gracefully, shutting down the pygame system cleanly
pygame.quit()
def main(args=None):
global aX, aY, vX, vY, pX, pY
global thrust, isAltLock, altLockHeight
# Setup the clock for a decent framerate
clock = pygame.time.Clock()
heightPID = PIDController(0.4, 0.025, 0.35) #5,1.0,0.5 #5,0.5,0.5
running = True
while running:
# Fill the background with black
screen.fill(BLACK)
#control inputs
for event in pygame.event.get():
if event.type == QUIT:
running = False
elif event.type == KEYDOWN:
#if event.key==K_LEFT:
#if event.key==K_RIGHT:
#if event.key==K_UP:
#if event.key==K_DOWN:
if event.key == K_ESCAPE:
running = False
elif event.key == K_h:
altLockHeight = 1.0 #pY
isAltLock = not isAltLock
#end if
#end for
#if you're pressing SPACE increase the thrust, otherwise decrease the thrust
keys = pygame.key.get_pressed()
if keys[K_SPACE]:
thrust += 0.5
if thrust > maxThrust:
thrust = maxThrust
else:
thrust -= 0.05
if (thrust < 0):
thrust = 0
#altitude lock code
if isAltLock:
#thrust = mass * gravity + 10.0*(altLockHeight - pY)
thrust = (mass * gravity) + (maxThrust / 2) * heightPID.process(
pY, altLockHeight, 1 / 30)
thrust = max(0, thrust)
thrust = min(maxThrust, thrust)
#drone dynamics
fY = -mass * gravity + thrust #force +ve=up
aY = fY / mass #F=ma, a=F/m
vY += aY * (1.0 / 30.0)
pY += vY * (1.0 / 30.0) #30 FPS, 1 pixel per metre
if pY <= 11 / screenScale: #drone height below zero line i.e. the collision bounding box
pY = 11 / screenScale
vY = -0.4 * vY #bounce off ground! and lose some energy in the process
#draw instruments, fuel, vertical, horizontal
drawInstruments()
#draw drone
drawDrone(pX, SCREEN_HEIGHT - pY * screenScale)
#write out logging data
#print(str(pY)+","+str(thrust)+","+str(altLockHeight)+","+str(aY)+","+str(vY))
#double buffer flip
pygame.display.flip()
# Ensure program maintains a rate of 30 frames per second
clock.tick(30)
#endwhile running
#exit gracefully, shutting down the pygame system cleanly
pygame.quit()
def test_new_controller_init():
pid = PIDController()
assert (pid is not None)
assert (pid.Kp == 0.0)
assert (pid.Kd == 0.0)
assert (pid.Ki == 0.0)
def test_new_controller():
pid = PIDController()
assert (pid is not None)
if processor:
yield processor.stream
processor.event.set()
else:
# When the pool is starved, wait a while for it to refill
print("pool is starved")
time.sleep(0.02)
except KeyboardInterrupt:
print("Ctrl-c pressed ...")
done = True
# - - - - - - - - - - - - - - - - - -
# - - - - - Main Program - - - - - -
# - - - - - - - - - - - - - - - - - -
pidCon = PIDController(camWidth, camHeight)
pidCon.sendData()
with picamera.PiCamera() as camera:
pool = [ImageProcessor(camWidth, camHeight, 25) for i in range(4)]
camera.resolution = (camWidth, camHeight)
camera.framerate = 90
time.sleep(2)
# Now fix the values
camera.shutter_speed = 2720 #int(camera.exposure_speed/4)
print(str(camera.shutter_speed))
camera.exposure_mode = 'off'
g = camera.awb_gains
camera.awb_mode = 'off'
camera.awb_gains = g
camera.capture_sequence(streams(), 'rgb', use_video_port=True)
# T.PI_RootLocusMultiple()
# T.PI_StepResponse()
# T.PI_ImpulseResponse()
# T.PD_RootLocusMultiple()
# T.PD_StepResponse()
# T.PID_RootLocusMultiple()
# T.PID_StepResponse()
# T.RLChange()
# T.P_Bode()
A = AnalyzeSystem()
q_targ = [9.0, 0.0]
Ku = 2.15e2
Tu = 0.4
C1 = PIDController(t=0.01, P=-0.6 * Ku, I=-Tu / 2, D=-Tu / 8) #ZN Rule
C1.setQTarget(q_targ)
C2 = PIDController(t=0.01, P=-0.5 * Ku, I=-0, D=0) #P
C2.setQTarget(q_targ)
C3 = PIDController(t=0.01, P=-0.2 * Ku, I=-Tu / 2,
D=-Tu / 3) # no overshoot
C3.setQTarget(q_targ)
sim_o = {'dt': 0.001, 't_total': 5.0, 'q_zero': [10.0, 0]}
Sim1 = Simulate(SM, C1, sim_options=sim_o)
Sim2 = Simulate(SM, C2, sim_options=sim_o)
Sim3 = Simulate(SM, C3, sim_options=sim_o)
# A.PI_analyze(SM, C)
q1 = Sim1.simulate(RTPlot=False)
q2 = Sim2.simulate(RTPlot=False)
q3 = Sim3.simulate()
Sim1.plotPhases((q1, q2, q3), q_targ)
plt.show()
Tmin = np.min(TModelOut)-273
Tmax = np.max(TModelOut)-273
Tmean = np.mean(TModelOut)-273
Tstdev = np.std(TModelOut)
print("Min: %0.1f" % Tmin, "Mean: %0.1f" % Tmean, "Max: %0.1f" % Tmax, "Stdev: %0.1f"% Tstdev)
"""
PID Controller Testing
"""
from PIDController import PIDController
MyPID = PIDController(1,0,1)
MyPID.set_SP(273+21)
PID_output = np.zeros(len(TModelOut))
PID_err = np.zeros(len(TModelOut))
PID_P = np.zeros(len(TModelOut))
PID_I = np.zeros(len(TModelOut))
PID_D = np.zeros(len(TModelOut))
# return [PID_out, self.Err, self.P_val, self.D_val, self.I_val]
for i in range(len(TModelOut)):
PID_output[i], PID_err[i], PID_P[i], PID_I[i], PID_D[i] = MyPID.update(TModelOut[i])
plt.figure()
plt.subplot(511).plot(PID_output)
axes = plt.gca()
self.controller2, self.driver.mapper,
self.ultrasonic)
self.current_index += 1
else:
self.follower.step()
except:
psm.BAM1.setSpeed(0)
psm.BAM2.setSpeed(0)
self.done = True
gyro = Gyro(psm.BAS2)
psm.BBM1.setSpeed(100)
gyro.calibrate(1)
print "Calibrate done error=", gyro.error_rate
controller = PIDController(1, 0, 0.1)
controller2 = PIDController(0.5, 0, 0.1)
driver = Driver(psm.BAM1, psm.BAM2, gyro, 3)
POINTS = [[1000, 1000], [3000, 1000], [3000, -750], [1000, -750], [1000, 1100],
[3000, 1120], [5000, 0]]
POINTSX = []
POINTSY = []
for i in POINTS:
POINTSX.append(i[0])
POINTSY.append(i[1])
follower = PathFollower(POINTS, driver, gyro, controller, controller2,
psm.BAS1)
#try:
while not follower.done:
follower.step()
#except:
def step(self):
self.follow()
def done(self):
if self.output < 10 and abs(self.get_error()) < 20:
return True
else:
return False
try:
gyro = Gyro(psm.BAS2)
gyro.calibrate(1)
print "Calibrate done error=", gyro.error_rate
controller = PIDController(1, 0, 0.1)
driver = Driver(psm.BAM1, psm.BAM2, gyro, 3)
follower = Follower(controller, driver, psm.BAS1)
while not follower.done():
follower.step()
print "done"
t = 1
driver.turn_right(20)
driver.step()
time.sleep(t)
while not follower.done():
follower.step()
print "done"
t = 1.2
import time
import datetime
from PixyCamget import *
from PIDController import PIDController
from Gyro import Gyro
from Driver import *
from PiStorms import PiStorms
psm = PiStorms()
gyro = Gyro(psm.BAS2)
TurnPID = PIDController(1, 0, 0, 1)
DrivePID = PIDController(1, 0, 0)
pixy = Pixy()
gyro.calibrate(1)
driver = Driver(psm.BAM1, psm.BAM2, gyro, 3)
while True:
blocks = pixy.get_blocks()
found_block = None
largest_block_size = 0
for block in blocks:
if block[1] != 1: # Check signature type
continue
current_size = block[4] * block[5] # Find block with largest area
if current_size > largest_block_size:
largest_block_size = current_size
found_block = block
PV.addMotor(1.0 / 10)
PV.addEncoder((2 * np.pi) / 180)
# PV.freeResponse()
# PV.plotRootLocus()
# PV.plotImpulseResponse()
# PV.plotStepResponse()
# T = Tests()
# T.P_stepResponse()
# T.PI_RootLocusMultiple()
# T.PD_StepResponse()
# T.PD_ImpulseResponse()
q_targ = [0.0, 0.0]
Ku = 50.0
Tu = 0.000
# C0 = PIDController(t = 0.01, P = 0, I = 0, D = 0) #Free response
# C1 = PIDController(t = 0.01, P = -0.6*Ku, I = -Tu/2, D = -Tu/8) #ZN Rule
C1 = PIDController(t=0.01, P=-1, I=-.02, D=-100) #ZN Rule
# C1 = GeneralController(t = 0.01)
CE = control.tf([1, -10], [1, -1]) * 25
# C1.setCE(CE)
# C0.setQTarget(q_targ)
C1.setQTarget(q_targ)
sim_o = {'dt': 0.001, 't_total': 25.0, 'q_zero': [np.pi / 20, 0]}
# Sim0 = Simulate(PV, C0, sim_options = sim_o)
Sim1 = Simulate(PV, C1, sim_options=sim_o)
# q0 = Sim0.simulate(RTPlot = False, Plot = True)
q1 = Sim1.simulate(RTPlot=False, Plot=True)
# Sim1.plotPhases((q1), q_targ)
# Sim1.plotPhases((q1, q2, q3), q_targ)
plt.show()
pdb.set_trace()
