PID simulation

PID-sim.py

import sys, itertools, matplotlib.pyplot as plt
from random import *
from math import copysign
from matplotlib import animation

MAX_MOTOR=10
MAX_VELOCITY=100

bound = lambda n, b: copysign(min(abs(n), b), n)

class Robot(object):
	def __init__(self):
		self.position=self.velocity=self.acceleration=self.prev=0.0

	def get_sensor(self):
		return self.position

	def set_motor(self, value):
		self.acceleration=bound(value, MAX_MOTOR)

	def physics(self):
		self.velocity=bound(self.velocity+self.acceleration, MAX_VELOCITY)
		self.position+=self.velocity

	def add_noise(self):
		self.acceleration=bound(uniform(.8, 1.2)*self.acceleration+uniform(-2, 2), MAX_MOTOR)
		#self.velocity=bound(uniform(.8, 1.2)*self.velocity, MAX_VELOCITY)
		#self.position=uniform(.8, 1.2)*self.position+uniform(-5, 5)

	def update(self):
		self.physics()
		self.controller()
		self.add_noise()

	def controller(self):
		pass

	def __str__(self):
		return str(self.position) + "\t\t" + str(self.velocity) + "\t\t" + str(self.acceleration)

class DoNothing(Robot):
	pass

class SimpleLoop(Robot):
	def controller(self):
		self.set_motor(MAX_MOTOR if self.get_sensor()<0 else -MAX_MOTOR if self.get_sensor() else 0)

class PLoop(Robot):
	def controller(self):
		self.set_motor(-self.get_sensor())

class PILoop(Robot):
	def __init__(self):
		Robot.__init__(self)
		self.integral=0

	def controller(self):
		self.integral+=self.get_sensor()
		self.set_motor(-self.get_sensor()-self.integral)

class GoodPILoop(Robot):
	def __init__(self):
		Robot.__init__(self)
		self.integral=0

	def controller(self):
		self.integral=self.integral*(2/3)+self.get_sensor()
		self.integral=bound(self.integral, 2)
		self.set_motor(-self.get_sensor()-.5*self.integral)

class PIDLoop(Robot):
	def __init__(self):
		Robot.__init__(self)
		self.integral=0

	def controller(self):
		self.integral=self.integral*(2/3)+self.get_sensor()
		self.integral=bound(self.integral, 2)
		self.derivative=self.prev-self.get_sensor()
		self.set_motor(-self.get_sensor()-.5*self.integral+.5*self.derivative)

def animate(f):
	if f:
		for i in robots:
			plt.plot([f-1, f], [i.prev, i.position], '-o')
			i.prev=i.get_sensor()
			i.update()



if __name__=="__main__":
	robots=[DoNothing(), SimpleLoop, PLoop()]
	fig=plt.figure()
	plt.gca().set_color_cycle('b')
	plt.title("Movement of Robots")
	plt.xlabel("Time (Ticks)")
	plt.ylabel("Position (units)")
	plt.axis([0,200,-25,25])
	plt.axhline(0, color='black', lw=3)
	[plt.plot(0,0,label=type(i).__name__+" Robot") for i in robots]
	plt.legend(loc="lower right")
	ani=animation.FuncAnimation(fig=fig, func=animate, frames=200, interval=500, repeat=False)
	plt.show()

-1 for Python