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_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 pid_thread():
global pos
controller = PIDController()
while True:
lock.acquire()
try:
controller.controll(pos)
finally:
lock.release()
def test_PIDControl():
target = 50
pidController = PIDController(target, 0.6, 0.1, 0.02)
time = np.linspace(0, 100, 100)
position = [0] # starting position
for i in range(1, len(time)):
velocity = pidController.getFeedback(position[i - 1])
position.append(position[i - 1] + velocity)
pylab.ylim(0, 60)
pylab.plot(time, [target for i in range(100)], "r", time, position, "b.")
pylab.show()
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 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 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 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 __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 __init__(self, targetDistance=1, targetAngle=90):
self.targetDistance = targetDistance
self.targetAngle = targetAngle
self._velocityPublisher = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=10)
# self._velocityPublisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self._scanSubscriber = rospy.Subscriber('scan', LaserScan, self._onScan)
self._angleController = PIDController(self.targetAngle, 0.02, 0.0001, 0.00)
self._distanceController = PIDController(self.targetDistance, 0.5, 0.001, 0.00)
self._velocityMessage = Twist()
self._states = {"followWall":self._followWall, "teleop":self._teleop}
self._currentState = "followWall"
self._keyDirection = None
self._stop = False
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 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 _get_mse(controller: PIDController, num_pts: float, dt: float) -> float:
"""Returns the mean squared error of the PIDController.
Arguments:
controller {PIDController} -- The PIDController to use.
num_pts {float} -- The number of points to use for the PIDController.
dt {float} -- The time step for the PIDController.
Returns:
float -- The root squared error of the PIDController.
"""
mse = 0
for _ in range(num_pts):
controller.step(dt)
err = controller.get_error()
mse += err**2
mse /= num_pts
return mse
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, targetAngle=0.0, pullForce=10.0, obstacleScaleInverseFactor=1.0):
self._velocityPublisher = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=10)
self._scanSubscriber = rospy.Subscriber('scan', LaserScan, self._onScan)
self._odomSubscriber = rospy.Subscriber('odom', Odometry, self._onOdom)
self._velocityMessage = Twist()
self._pullForce = pullForce
self._obstacleScaleFactor = obstacleScaleInverseFactor
self._currentAngle = None
self._angleController = PIDController(targetAngle, 0.2, 0.001, 0.02)
self._stop = False
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):
"""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 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 __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 = ()
class WallFollower(object):
""" Class for handling wall following behavior
"""
def __init__(self, targetDistance=1, targetAngle=90):
self.targetDistance = targetDistance
self.targetAngle = targetAngle
self._velocityPublisher = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=10)
# self._velocityPublisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self._scanSubscriber = rospy.Subscriber('scan', LaserScan, self._onScan)
self._angleController = PIDController(self.targetAngle, 0.02, 0.0001, 0.00)
self._distanceController = PIDController(self.targetDistance, 0.5, 0.001, 0.00)
self._velocityMessage = Twist()
self._states = {"followWall":self._followWall, "teleop":self._teleop}
self._currentState = "followWall"
self._keyDirection = None
self._stop = False
def stop():
self._stop = True
@property
def state(self):
return self._currentState
@state.setter
def state(self, state):
self._currentState = state
@property
def key(self):
return self._keyDirection
@key.setter
def key(self, key):
self._keyDirection = key
@staticmethod
def _filterReadings(data, lowerBound=0.0, upperBound=10.0):
return map(lambda x: x if x > lowerBound and x <= upperBound else float("inf") , data)
@staticmethod
def _sampleReadings(data, startIndex, endIndex):
""" returns the mean of a sample of readings
startIndex -- Number starting index of sample
endIndex -- Number end index of sample (end index is NOT included in result)
"""
if (endIndex > len(data)):
endIndex = len(data)
validData = [data[i] for i in range(startIndex, endIndex) if data[i] != float("inf")]
dataLength = endIndex - startIndex
if len(validData) == dataLength:
return sum(validData) / float(dataLength)
else:
return None
@staticmethod
def _wallAngle(data):
""" returns the angle of the wall relative to the robot in degrees
"""
if (len(data)):
return data.index(min(data))
def _onScan(self, msg):
""" callback handler for scan event
"""
readings = self._filterReadings(msg.ranges)
angle = self._wallAngle(readings)
distance = self._sampleReadings(readings, angle, angle + 5)
distance = readings[angle]
if distance:
self._states[self._currentState](angle, distance)
def _followWall(self, angle, distance):
""" wall following behavior. Uses two complementary PID controllers to
keep the robot along the wall
"""
parallelFeedback = self._angleController.getFeedback(angle)
distanceFeedback = self._distanceController.getFeedback(distance)
if angle <= 180:
parallelFeedback *= -1
distanceFeedback *= -1
self._velocityMessage.angular.z = parallelFeedback + 2.25 * distanceFeedback
self._velocityMessage.linear.x = 0.35
def _teleop(self, angle, distance):
char = self.key
if char == 'w':
self._velocityMessage = Twist(linear=Vector3(x=0.5))
elif char == 's':
self._velocityMessage = Twist(linear=Vector3(x=-0.5))
if char == 'a':
self._velocityMessage = Twist(angular=Vector3(z=0.5))
elif char == 'd':
self._velocityMessage = Twist(angular=Vector3(z=-0.5))
def run(self):
""" main behavior of wall follower class
"""
self._stop = False
rospy.init_node('wall_follower', anonymous=True)
r = rospy.Rate(10)
while not rospy.is_shutdown():
if not self._stop:
self._velocityPublisher.publish(self._velocityMessage)
r.sleep()
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)
class ObstacleAvoider(object):
""" Class for handling wall following behavior
"""
def __init__(self, targetAngle=0.0, pullForce=10.0, obstacleScaleInverseFactor=1.0):
self._velocityPublisher = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=10)
self._scanSubscriber = rospy.Subscriber('scan', LaserScan, self._onScan)
self._odomSubscriber = rospy.Subscriber('odom', Odometry, self._onOdom)
self._velocityMessage = Twist()
self._pullForce = pullForce
self._obstacleScaleFactor = obstacleScaleInverseFactor
self._currentAngle = None
self._angleController = PIDController(targetAngle, 0.2, 0.001, 0.02)
self._stop = False
def stop():
self._stop = True
@staticmethod
def _filterReadings(data, lowerBound=0.0, upperBound=10.0):
return map(lambda x: x if x > lowerBound and x <= upperBound else float("inf"), data)
@staticmethod
def _odomToAngle(value, leftMin, leftMax, rightMin, rightMax):
leftSpan = leftMax - leftMin
rightSpan = rightMax - rightMin
valueScaled = float(value - leftMin) / float(leftSpan)
return rightMin + (valueScaled * rightSpan)
def _sumComponentForces(self, angleDistances):
""" Sum the force components
"""
pullAngle = self._odomToAngle(self._currentAngle, -1.0, 1.0, -3.14, 3.14)
pullX, pullY = math.cos(pullAngle), math.sin(pullAngle)
xComponents = pullX*self._pullForce
yComponents = pullY*self._pullForce
for angle, reading in angleDistances:
if reading == float("inf"):
continue
angle = self._odomToAngle(angle, 0, 359, 0, 2*3.14) - pullAngle
xComponents -= (self._obstacleScaleFactor / reading)*math.cos(angle)
yComponents -= (self._obstacleScaleFactor / reading)*math.sin(angle)
return xComponents, yComponents
def _onOdom(self, msg):
""" Odometry handler
"""
self._currentAngle = msg.pose.pose.orientation.z
def _onScan(self, msg):
if self._currentAngle:
readings = self._filterReadings(msg.ranges)
desiredAngle = self._sumComponentForces(zip(range(360), readings))
desiredAngle = -1.0 * math.atan2(desiredAngle[1],desiredAngle[0])
self._angleController.target = desiredAngle
parallelFeedback = -1.0 * self._angleController.getFeedback(self._currentAngle)
print parallelFeedback
self._velocityMessage = Twist(angular=Vector3(z=20*parallelFeedback), linear=Vector3(x=0.5))
def run(self):
""" main behavior of obstacle avoider class
"""
self._stop = False
rospy.init_node('obstacle_avoider', anonymous=True)
r = rospy.Rate(10)
while not rospy.is_shutdown():
if not self._stop:
self._velocityPublisher.publish(self._velocityMessage)
r.sleep()
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()
class Tests(object):
def __init__(self):
i = 1
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 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 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 PI_StepResponse(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.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=50, I=150)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=75, I=225)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
axx.plot([0, max(out[1])], [1, 1], 'k--')
def PD_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=100, D=10)
out = control.root_locus(self.C.sysCCE / self.C.D, Plot=False)
self.C.plotRootLocus(out, axx, 'x')
# self.C.sysRecomp(P = 250, D = 25); out = control.root_locus(self.C.sysCCE/self.C.I, Plot = False); self.C.plotRootLocus(out, axx, ':')
# self.C.sysRecomp(P = 1000, D = 100); out = control.root_locus(self.C.sysCCE/self.C.I, Plot = False); self.C.plotRootLocus(out, axx, '--')
def PD_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)
self.C.sysRecomp(P=100, D=10)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=270, D=27)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=500, D=50)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=750, D=75)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
axx.plot([0, max(out[1])], [1, 1], 'k--')
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 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 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 P_Bode(self):
s = control.tf([1, 0], [1])
CE = 1 / (s**2 + 8.8 * s + 40)
P = control.tf([1], [1])
OL = P * CE
CL = control.feedback(OL, 1, sign=-1)
control.root_locus(CL, Plot=True)
plt.figure()
P = control.tf([100], [1])
OL1 = P * CE
CL1 = control.feedback(OL1, 1, sign=-1)
P = control.tf([500], [1])
OL2 = P * CE
CL2 = control.feedback(OL2, 1, sign=-1)
P = control.tf([2000], [1])
OL3 = P * CE
CL3 = control.feedback(OL3, 1, sign=-1)
print("Closed Loop")
print([CL1, CL2, CL3])
control.bode_plot([OL1, OL2, OL3], Plot=True)
print(CL)
# 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)
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:
class CleaningRobot:
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 _sendStatusMessage(self, msg):
self.webcontroller.SendStatus(msg)
def callback_laser_scan(self,msg):
self.directions = {
'right':min(min(msg.ranges[270:305]),10),
'fright':min(min(msg.ranges[306:341]),10),
'front1':min(min(msg.ranges[342:360]),10),
'front2':min(min(msg.ranges[0:17]),10),
'front':min(min(min(msg.ranges[342:360]),min(msg.ranges[0:17])),10),
'fleft':min(min(msg.ranges[18:53]),10),
'left':min(min(msg.ranges[54:90]),10),
}
def _wall_at_right(self,distance_set_point):
distance = min(self.directions['right'], self.directions['fright'])
return distance - distance_set_point <= 0
def _wall_at_front(self,distance_set_point):
distance = self.directions['front']
return distance - distance_set_point <= 0
def _wall_at_left(self,distance_set_point):
distance = min(self.directions['left'], self.directions['fleft'])
return distance - distance_set_point <= 0
def wall_is_found(self,distance_set_point):
return self._wall_at_right(distance_set_point) or self._wall_at_front(distance_set_point) or self._wall_at_left(distance_set_point)
def _is_in_corner(self):
if self.cleaner_side == "right":
if self.directions["right"] < self.FOLLOW_WALL_SET_POINT\
and self.directions["fright"] < self.FOLLOW_WALL_SET_POINT\
and self.directions["front1"] < self.FOLLOW_WALL_SET_POINT\
and self.directions["front2"] < self.FOLLOW_WALL_SET_POINT\
and not self.directions["left"] < self.FOLLOW_WALL_SET_POINT:
return True
if self.cleaner_side == "left":
if self.directions["left"] < self.FOLLOW_WALL_SET_POINT\
and self.directions["fleft"] < self.FOLLOW_WALL_SET_POINT\
and self.directions["front1"] < self.FOLLOW_WALL_SET_POINT\
and self.directions["front2"] < self.FOLLOW_WALL_SET_POINT\
and not self.directions["right"] < self.FOLLOW_WALL_SET_POINT:
return True
return False
def robot_is_stuck(self):
minimum_distance = min(self.directions.values())
print '\nstuck counter: %f , minimun distance: %f ' %(self.robot_stuck_counter,minimum_distance)
if minimum_distance <= self.STUCK_DETECTION_DISTANCE:
self.robot_stuck_counter += 1
else:
self.robot_stuck_counter = 0
if self.robot_stuck_counter >= self.STUCK_MAX_COUNTER:
return True
return False
def vortex_searching(self,increment_delay,search_speed_increment,search_speed_max):
current_time = time()
if current_time - self.previous_searching_time >= increment_delay:
self.current_speed.linear.x += search_speed_increment
self.previous_searching_time = current_time
self.current_speed.angular.z = 0.2
self.current_speed.linear.x = min(self.current_speed.linear.x,search_speed_max)
def finding_wall(self,distance_set_point):
#detecting wall position
#act based on the position
"""
if self._wall_at_front(distance_set_point):
#at far front, approaching slowly
self.current_speed.linear.x = 0.1
self.current_speed.angular.z = 0
elif self._wall_at_right(distance_set_point):
#at far right, rotate right
self.current_speed.linear.x = 0
self.current_speed.angular.z = -0.1
elif self._wall_at_left(distance_set_point):
#at far left, rotate left
self.current_speed.linear.x = 0
self.current_speed.angular.z = 0.1
"""
if self._is_in_corner():
self.state = STATES["TURNING"]
else:
#no obstacles, vortex searching
self.vortex_searching(10,0.05,1.0)
def state_work(self):
if self.cur_command == "stop":
print '\nstop command received, to idle'
self.current_speed.linear.x = 0
self.current_speed.angular.z = 0
self.publisher.publish(self.current_speed)
self.state = 0
self._sendStatusMessage(self.state_desc[self.state])
#idle state
if self.state == STATES["IDLE"]:
if self.cur_command == "start":
#start finding wall
print '\nstart command received to find wall'
self.state = STATES["FIND_WALL"]
self._sendStatusMessage(self.state_desc[self.state])
#find wall state
elif self.state == STATES["FIND_WALL"]:
if self.robot_is_stuck():
print '\nrobot is stuck, to idle'
self.current_speed.linear.x = 0
self.current_speed.angular.z = 0
self.publisher.publish(self.current_speed)
self.cur_command = "stop"
self.state = STATES["STUCK"]
self._sendStatusMessage(self.state_desc[self.state])
self.photographer.CaptureImage()
elif self.wall_is_found(self.FOLLOW_WALL_SET_POINT_START):
#change to follow wall
print '\nto follow wall'
self.state = STATES["FOLLOW_WALL"]
self._sendStatusMessage(self.state_desc[self.state])
else:
print '\nfinding wall'
self.finding_wall(self.OBSTACLE_DETECTION_SET_POINT_FAR)
self.publisher.publish(self.current_speed)
#print '\nfleft: %f ,front: %f ,fright: %f ' %(self.directions['front'], self.directions['fleft'], self.directions['fright'])
#follow wall state
elif self.state == STATES["FOLLOW_WALL"]:
if self.robot_is_stuck():
print '\nrobot is stuck, to idle'
self.current_speed.linear.x = 0
self.current_speed.angular.z = 0
self.cur_command = "stop"
self.publisher.publish(self.current_speed)
self.state = STATES["STUCK"]
self._sendStatusMessage(self.state_desc[self.state])
self.photographer.CaptureImage()
elif self._is_in_corner():
print "In a corner"
self.state = STATES["TURNING"]
self.current_speed.linear.x = 0
self.current_speed.angular.z = 1 if self.cleaner_side == "right" else -1
self.publisher.publish(self.current_speed)
self._sendStatusMessage(self.state_desc[self.state])
else:
print '\nfollowing wall'
if self.cleaner_side == "right":
distance_to_wall = min(self.directions['right'],self.directions['fright'],self.directions['front1'])
elif self.cleaner_side == "left":
distance_to_wall = min(self.directions['left'],self.directions['fleft'],self.directions['front2'])
self.current_speed = self.PID_controller.GetPV(distance_to_wall)
if self.cleaner_side == "left":
self.current_speed.angular.z = self.current_speed.angular.z * -1
self.publisher.publish(self.current_speed)
elif self.state == STATES["STUCK"]:
if self.cur_command == "start":
state = STATES["IDLE"]
self._sendStatusMessage(self.state_desc[self.state])
elif self.state == STATES["TURNING"]:
if not self._is_in_corner():
self.state = STATES["FOLLOW_WALL"]
self._sendStatusMessage(self.state_desc[self.state])
else:
self.current_speed.linear.x = 0
self.current_speed.angular.z = 0
self.publisher.publish(self.current_speed)
self.state = STATES["IDLE"]
print '\nunknown state, to idle'
self._sendStatusMessage(self.state_desc[self.state])
print '\nleft: %f ,front: %f ,right: %f ' %(self.directions['left'],self.directions['front'],self.directions['right'])
print '\nlinear: %f ,angular: %f ' %(self.current_speed.linear.x, self.current_speed.angular.z )
class Tests(object):
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 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 PD_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=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.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=270, D=27)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=500, D=50)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
self.C.sysRecomp(P=750, D=75)
out = self.C.plotStepResponse(self.C.sysCCE, Plot=False, axx=axx)
axx.plot([0, max(out[1])], [1, 1], 'k--')
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 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
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()
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()
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
class Simulate(object): #class for simulating the whole system
def __init__(self, system, controller, sim_options=None):
self.SM = system
self.Controller = controller
if not sim_options:
self.sim = {'dt': 0.001, 't_total': 5.0, 'q_zero': [0.01, 0]}
else:
self.sim = sim_options # a dictionary with settings for simulation
def simulate(self, Plot=False, RTPlot=False):
t_cur = 0
q = [self.sim['q_zero']]
enc = [0]
u = [0.0]
u_P = [0.0]
u_D = [0.0]
u_I = [0.0]
e = [0]
if RTPlot: frt, axrt = plt.subplots(1, 1)
t_plot = 0.1
t_plot_last = 0
while t_cur < self.sim['t_total']:
if (t_cur - self.Controller.last_t) < self.Controller.dt:
u_cur, e_cur = self.Controller.update(q[-1])
u.append(u_cur)
# u_P.append(self.Controller.u_P)
# u_D.append(self.Controller.u_D)
# u_I.append(self.Controller.u_I)
e.append(e_cur)
self.Controller.last_t = t_cur
q.append(self.SM.deltaTSim(q[-1], u[-1], self.sim['dt']))
enc.append(self.SM.Encoder.update(q[-1][0]))
if t_cur - t_plot_last > t_plot and RTPlot:
self.SM.plotSystem(q[-1], u[-1], axrt, show=False)
axrt.plot([self.Controller.q_target[0]] * 2, [0, 2], 'g-.')
plt.draw()
plt.pause(0.001)
t_plot_last = t_cur
t_cur += self.sim['dt']
if Plot:
print("Starting to Plot points")
f, axarr = plt.subplots(3, 2)
axarr[0, 1].plot(np.array(q)[:, 0], 'r-', label='Theta')
axarr[0, 1].plot(np.array(q)[:, 1], 'b-', label='Theta Dot')
lim_y = max(abs(np.array(q))[:, 0])
axarr[0, 1].plot([0, len(q)], [lim_y, lim_y], 'k-')
axarr[0, 1].plot([0, len(q)], [-lim_y, -lim_y], 'k-')
axarr[0, 1].legend()
axarr[0, 1].set_title('Phase')
axarr[1, 1].plot(np.array(enc), 'ko')
axarr[1, 1].set_title('encoder')
# pdb.set_trace()
xs = -self.SM.l * np.sin(np.array(q)[:, 0])
ys = self.SM.l * np.cos(np.array(q)[:, 0])
axarr[1, 0].plot(xs, ys, 'o')
axarr[1, 0].set_title('Pendulum Position')
axarr[1, 0].set_ylim([-self.SM.l, self.SM.l])
axarr[1, 0].set_xlim([-self.SM.l, self.SM.l])
axarr[0, 0].set_title('Control Params')
axarr[0, 0].plot(np.array(q)[:, 0], 'r-', label='Theta')
axarr[0, 0].plot(np.array(e), 'g.', label='Error')
axarr[0, 0].autoscale(True)
axarr[0, 0].legend()
axarr[2, 0].set_title('Control Input')
axarr[2, 0].autoscale(True)
axarr[2, 0].plot(np.array(u), 'b.', label='Input')
axarr[2, 0].legend()
print('Average Input: %s' % np.mean(np.array(u)))
steady_state_error = self.Controller.q_target[0] - np.mean(
np.array(q[-100:])[:, 0])
return np.array(q)
def plotPhases(
self,
phase_tuple,
q_targ=None
): #phase tuple is (q1, q2, q3) with increasing derivates in each q
f, axx = plt.subplots(2, 1)
for i, q in enumerate(phase_tuple):
axx[0].plot(q[:, 0], label='%s' % i)
axx[1].plot(q[:, 1], label='%s' % i)
for i in range(2):
if q_targ:
axx[i].plot([0, len(q[:, i])], [q_targ[i], q_targ[i]], 'k:')
axx[i].legend()
plt.show()
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]))
