def createWorld(self):
self._isLiving = True
self._auto = True
self._pid_control = PIDControl(1, 0.05, 0.1) # kp, ki, kd
self.ground = self.world.CreateBody(
shapes=b2EdgeShape(vertices=[(-25, 0), (25, 0)])
)
self.carBody = self.world.CreateDynamicBody(
position=(0, 3),
fixtures=b2FixtureDef(
shape=b2PolygonShape(box=(5, 1)), density=1)
)
self.carLwheel = self.world.CreateDynamicBody(
position=(-3, 1),
fixtures=b2FixtureDef(
shape=b2CircleShape(radius=1), density=2, friction=1)
)
self.carRwheel = self.world.CreateDynamicBody(
position=(3, 1),
fixtures=b2FixtureDef(
shape=b2CircleShape(radius=1), density=2, friction=1)
)
self.pendulum = self.world.CreateDynamicBody(
position=(0, 13),
fixtures=b2FixtureDef(
shape=b2PolygonShape(box=(0.5, 10)), density=1),
)
self.pendelumJoin = self.world.CreateRevoluteJoint(
bodyA=self.carBody,
bodyB=self.pendulum,
anchor=(0, 3),
maxMotorTorque=1,
enableMotor=True
)
self.pendelumRJoin =self.world.CreateRevoluteJoint(
bodyA=self.carBody,
bodyB=self.carRwheel,
anchor=(3, 1),
maxMotorTorque=1,
enableMotor=True,
)
self.pendelumLJoin = self.world.CreateRevoluteJoint(
bodyA=self.carBody,
bodyB=self.carLwheel,
anchor=(-3, 1),
maxMotorTorque=1,
enableMotor=True,
)
from pyconsys.PIDControl import PIDControl
from pyconsys.PT2 import PT2
from pyconsys.Rating import Rating
from random import randint
import matplotlib.pyplot as plt
pid_p = 10
pid_i = 20
pid_d = 0.9
pt2_a2 = 0.2
pt2_a1 = 0.1
pt2_a0 = 1
pt2_b0 = 1
pid_control = PIDControl(pid_p, pid_i, pid_d)
pt2 = PT2(pt2_a2, pt2_a1, pt2_a0, pt2_b0)
rating = Rating()
time_steps = [x * Control.DELTA_T for x in range(0, 500)]
w_lst = [1 for x in time_steps]
e_lst = []
y_lst = []
x_lst = []
z_lst = []
best_pid_p = best_pid_i = best_pid_d = best_rating = 0
for _ in range(0, 100):
e = y = x = z = 0
from pyconsys.Control import Control
from pyconsys.PIDControl import PIDControl
from pyconsys.PT2 import PT2
import matplotlib.pyplot as plt
pid_p = 10
pid_i = 20
pid_d = 0.9
pt2_a2 = 0.2
pt2_a1 = 0.5
pt2_a0 = 1
pt2_b0 = 1
pid_control = PIDControl(pid_p, pid_i, pid_d)
pt2 = PT2(pt2_a2, pt2_a1, pt2_a0, pt2_b0)
time_steps = [x * Control.DELTA_T for x in range(0, 500)]
w_lst = [1 if x > 1 else 0 for x in time_steps]
e_lst = []
y_lst = []
x_lst = []
z_lst = []
e = y = x = z = i = 0
for w in w_lst:
e = w - x
y = pid_control.get_xa(e)
if i > 250:
z = 0.5
class BodyPendulum(Framework):
name = "Inverted Pendulum"
description = "(n) new world"
def __init__(self):
super(BodyPendulum, self).__init__()
self.createWorld()
self.createFuzzy()
def createFuzzy(self):
self.fuzz_pend1 = ctrl.Antecedent(np.arange(-1, 1.1, 0.1), 'join1')
self.fuzz_motor = ctrl.Consequent(np.arange(-300, 300, 1), 'motor')
self.fuzz_pend1.automf(7)
self.fuzz_motor.automf(7)
#self.fuzz_pend1['average'].view()
#self.fuzz_motor['average'].view()
self.rule1 = ctrl.Rule(self.fuzz_pend1['dismal'], self.fuzz_motor['dismal'])
self.rule2 = ctrl.Rule(self.fuzz_pend1['poor'], self.fuzz_motor['poor'])
self.rule3 = ctrl.Rule(self.fuzz_pend1['mediocre'], self.fuzz_motor['mediocre'])
self.rule4 = ctrl.Rule(self.fuzz_pend1['average'], self.fuzz_motor['average'])
self.rule5 = ctrl.Rule(self.fuzz_pend1['decent'], self.fuzz_motor['decent'])
self.rule6 = ctrl.Rule(self.fuzz_pend1['good'], self.fuzz_motor['good'])
self.rule7 = ctrl.Rule(self.fuzz_pend1['excellent'], self.fuzz_motor['excellent'])
self.pendulum_ctrl = ctrl.ControlSystem([self.rule1, self.rule2, self.rule3, self.rule4, self.rule5, self.rule6, self.rule7])
self.pendulum_fuzz = ctrl.ControlSystemSimulation(self.pendulum_ctrl)
#self.rule1.view()
def createWorld(self):
self._isLiving = True
self._auto = True
self._pid_control = PIDControl(1, 0.05, 0.1) # kp, ki, kd
self.ground = self.world.CreateBody(
shapes=b2EdgeShape(vertices=[(-25, 0), (25, 0)])
)
self.carBody = self.world.CreateDynamicBody(
position=(0, 3),
fixtures=b2FixtureDef(
shape=b2PolygonShape(box=(5, 1)), density=1)
)
self.carLwheel = self.world.CreateDynamicBody(
position=(-3, 1),
fixtures=b2FixtureDef(
shape=b2CircleShape(radius=1), density=2, friction=1)
)
self.carRwheel = self.world.CreateDynamicBody(
position=(3, 1),
fixtures=b2FixtureDef(
shape=b2CircleShape(radius=1), density=2, friction=1)
)
self.pendulum = self.world.CreateDynamicBody(
position=(0, 13),
fixtures=b2FixtureDef(
shape=b2PolygonShape(box=(0.5, 10)), density=1),
)
self.pendelumJoin = self.world.CreateRevoluteJoint(
bodyA=self.carBody,
bodyB=self.pendulum,
anchor=(0, 3),
maxMotorTorque=1,
enableMotor=True
)
self.pendelumRJoin =self.world.CreateRevoluteJoint(
bodyA=self.carBody,
bodyB=self.carRwheel,
anchor=(3, 1),
maxMotorTorque=1,
enableMotor=True,
)
self.pendelumLJoin = self.world.CreateRevoluteJoint(
bodyA=self.carBody,
bodyB=self.carLwheel,
anchor=(-3, 1),
maxMotorTorque=1,
enableMotor=True,
)
def destroyWorld(self):
self.world.DestroyBody(self.carBody)
self.world.DestroyBody(self.carLwheel)
self.world.DestroyBody(self.carRwheel)
self.world.DestroyBody(self.pendulum)
self._isLiving = False
def Keyboard(self, key):
if key == Keys.K_a:
if self._isLiving:
self.pendelumLJoin.motorSpeed = 0
self.pendelumLJoin.maxMotorTorque = 1000
self.pendelumRJoin.motorSpeed = 0
self.pendelumRJoin.maxMotorTorque = 1000
self._auto = True
elif key == Keys.K_n:
if self._isLiving:
self.destroyWorld()
self.createWorld()
def Step(self, settings):
super(BodyPendulum, self).Step(settings)
self.pendelumLJoin.maxMotorTorque = 1000
self.pendelumRJoin.maxMotorTorque = 1000
self.pendulum_fuzz.input['join1'] = self.pendulum.angle
self.pendulum_fuzz.compute()
if abs(self.pendulum.angle) > 0.01:
xa = self._pid_control.get_xa(self.pendulum_fuzz.output['motor'])
else:
xa = self.pendulum_fuzz.output['motor']
self.pendelumLJoin.motorSpeed = xa
self.pendelumRJoin.motorSpeed = xa
class BodyPendulum(Framework):
name = "Inverted Pendulum (PyConSys)"
description = "(m) manual, (a) automatic, (n) new world"
speed = 3
def __init__(self):
super(BodyPendulum, self).__init__()
self.createWorld()
def createWorld(self):
self._isLiving = True
self._auto = False
self._pid_control = PIDControl(105, 83, 28) # kp, ki, kd
self.ground = self.world.CreateBody(shapes=b2EdgeShape(
vertices=[(-25, 0), (25, 0)]))
self.carBody = self.world.CreateDynamicBody(
position=(0, 3),
fixtures=b2FixtureDef(shape=b2PolygonShape(box=(5, 1)), density=1))
self.carLwheel = self.world.CreateDynamicBody(
position=(-3, 1),
fixtures=b2FixtureDef(shape=b2CircleShape(radius=1),
density=2,
friction=1))
self.carRwheel = self.world.CreateDynamicBody(
position=(3, 1),
fixtures=b2FixtureDef(shape=b2CircleShape(radius=1),
density=2,
friction=1))
self.pendulum = self.world.CreateDynamicBody(
position=(0, 13),
fixtures=b2FixtureDef(shape=b2PolygonShape(box=(0.5, 10)),
density=1),
)
self.pendelumJoin = self.world.CreateRevoluteJoint(bodyA=self.carBody,
bodyB=self.pendulum,
anchor=(0, 3),
maxMotorTorque=1,
enableMotor=True)
self.pendelumRJoin = self.world.CreateRevoluteJoint(
bodyA=self.carBody,
bodyB=self.carRwheel,
anchor=(3, 1),
maxMotorTorque=1,
enableMotor=True,
#motorSpeed=10
)
self.pendelumLJoin = self.world.CreateRevoluteJoint(
bodyA=self.carBody,
bodyB=self.carLwheel,
anchor=(-3, 1),
maxMotorTorque=1,
enableMotor=True,
#motorSpeed=10
)
def destroyWorld(self):
self.world.DestroyBody(self.carBody)
self.world.DestroyBody(self.carLwheel)
self.world.DestroyBody(self.carRwheel)
self.world.DestroyBody(self.pendulum)
self._isLiving = False
def Keyboard(self, key):
if key == Keys.K_a:
if self._isLiving:
self._auto = True
elif key == Keys.K_m:
self._auto = False
if self._isLiving:
self.pendelumLJoin.motorSpeed = 0
self.pendelumLJoin.maxMotorTorque = 1
self.pendelumRJoin.motorSpeed = 0
self.pendelumRJoin.maxMotorTorque = 1
elif key == Keys.K_n:
if self._isLiving:
self.destroyWorld()
self.createWorld()
def Step(self, settings):
super(BodyPendulum, self).Step(settings)
w = 0
e = (w - self.pendulum.angle * -1)
y = self._pid_control.get_xa(e)
if self._auto and self._isLiving:
self.pendelumLJoin.maxMotorTorque = 1000
self.pendelumRJoin.maxMotorTorque = 1000
self.pendelumLJoin.motorSpeed = y
self.pendelumRJoin.motorSpeed = y