def run(self):
""" start the simulation
"""
dt=self.dt
clock = gui.clock()
frameRate=1.0/dt/self.slowMotionFactor
self.tick_count=0
self.world.start()
# the event loop also loops the animation code
while True:
self.frameskipcount -= 1
self.tick_count += 1
display= self.frameskipcount == 0 and self.frameskipfactor != 0
if display:
clock.tick(frameRate)
self.frameskipcount=self.frameskipfactor
if display or (self.tick_count%100)==0:
gui.grab_events()
if self.admin != None:
self.admin.process(self)
if gui.check_for_quit():
break
self.ticks += 1
self.world.step()
if display:
self.screen.fill((0,0,0))
if self.painter != None:
if self.painter.preDraw != None:
self.painter.preDraw(self.screen)
self.world.draw(self.screen)
if self.painter != None:
if self.painter.postDraw != None:
self.painter.postDraw(self.screen)
gui.blit(self.screen)
def run(self):
""" start the simulation
"""
dt = self.world.dt
clock = gui.clock()
frameRate = 1.0 / dt / self.slowMotionFactor
self.tick_count = 0
self.world.start()
# the event loop also loops the animation code
while True:
self.frameskipcount -= 1
self.tick_count += 1
display = self.frameskipcount == 0 and self.frameskipfactor != 0
if display:
clock.tick(frameRate)
self.frameskipcount = self.frameskipfactor
if display or (self.tick_count % 100) == 0:
gui.grab_events()
if self.admin != None:
self.admin.process(self)
if gui.check_for_quit():
break
self.ticks += 1
self.world.step()
if display:
self.screen.fill((0, 0, 0))
if self.painter != None:
if self.painter.preDraw != None:
self.painter.preDraw(self.screen)
self.world.draw(self.screen)
if self.painter != None:
if self.painter.postDraw != None:
self.painter.postDraw(self.screen)
gui.blit(self.screen)
def draw(screen, cart, force):
"""
Called by the simulation to display the current state of the cart.
"""
# clear screen
screen.fill((0, 0, 0))
# get the size of the window
wid = gui.dim_window[0]
hei = gui.dim_window[1]
# pendulum length
length = cart.l
scale = hei / length / 3.0
# map position onto the screen
x1 = wid / 2.0 + cart.getX() * scale
# if too big/small limit the position
if x1 > wid * .8:
x1 = wid * .8
if x1 < wid * .2:
x1 = wid * .2
# base line for the cart
y1 = hei * .6
# angle of pendulum
ang = cart.getAngle()
# x,y of the end of pendulum
x2 = x1 + scale * math.sin(ang) * length
y2 = y1 + scale * math.cos(ang) * length
# draw pendulum
col = (255, 0, 0)
thick = 3
gui.draw_line(screen, x1, y1, x2, y2, col, thick)
col = (0, 255, 0)
gui.fill_circle(screen, x2, y2, 12, col)
# draw cart
col = (0, 0, 255)
thick = 20
gui.draw_line(screen, x1 - 20, y1, x1 + 20, y1, col, thick)
# display the state of the cart
col = (0, 255, 255)
state = cart.state
str2 = ""
str2 += "Phi: %5.2f " % (state[0] - math.pi)
str2 += "dphidt: %5.2f " % state[1]
str2 += "x: %5.2f " % state[2]
str2 += "dxdt: %5.2f " % state[3]
str2 += " force: %5.2f " % force
gui.draw_string(screen, str2, (20, 10), col, 16)
# copy screen onto the display
gui.blit(screen)
def draw(screen,cart,force):
"""
Called by the simulation to display the current state of the cart.
"""
# clear screen
screen.fill((0,0,0))
# get the size of the window
wid=gui.dim_window[0]
hei=gui.dim_window[1]
# pendulum length
length=cart.l
scale=hei/length/3.0
# map position onto the screen
x1=wid/2.0+cart.getX()*scale
# if too big/small limit the position
if x1 > wid*.8:
x1 =wid*.8
if x1 < wid*.2:
x1 = wid*.2
# base line for the cart
y1=hei*.6
# angle of pendulum
ang=cart.getAngle()
# x,y of the end of pendulum
x2=x1+scale*math.sin(ang)*length
y2=y1+scale*math.cos(ang)*length
# draw pendulum
col=(255,0,0)
thick=3
gui.draw_line(screen,x1,y1,x2,y2,col,thick)
col=(0,255,0)
gui.fill_circle(screen,x2,y2,12,col)
# draw cart
col=(0,0,255)
thick=20
gui.draw_line(screen,x1-20,y1,x1+20,y1,col,thick)
# display the state of the cart
col=(0,255,255)
state=cart.state
str2=""
str2+= "Phi: %5.2f " % (state[0]-math.pi)
str2+= "dphidt: %5.2f " % state[1]
str2+= "x: %5.2f " % state[2]
str2+= "dxdt: %5.2f " %state[3]
str2+= " force: %5.2f " % force
gui.draw_string(screen,str2,(20,10),col,16)
# copy screen onto the display
gui.blit(screen)
def draw(screen,robot,pwmRatio):
"""
Called by the simulation to display the current state of the robot.
"""
#Get the robot state
state = robot.getState()
# Map robot angles to the screen coordinate system
angW = (-state[0]) + math.pi #Wheel angel
angB = (angW + state[2]) #Body angle
# Clear screen
screen.fill((0,0,0))
# Gget the size of the window
wid=gui.dim_window[0]
hei=gui.dim_window[1]
# Get robot dimensions
length=robot.L
scale = hei/length/3.0
robot_r = int(robot.wR*1000)
#robot_r = int(robot.wR*scale) #Use this value for robot_r to display robot to scale
# Robot position mapped to the screen coordinate system
x1 = screen.get_width()/2 #Robot reference X position on screen
xm1 = (robot_r*state[0])%screen.get_width() #Robot true X displacement on screen
y1=hei*.6 #Robot base line for wheel on screen
# X,Y Coordinate of the body's centre of gravity
x2=x1+scale*math.sin(angB)*length
y2=y1+scale*math.cos(angB)*length
# Draw robot's centre of gravity
col=(255,0,0)
thick=3
gui.draw_line(screen,x1,y1,x2,y2,col,thick)
col=(0,255,0)
gui.fill_circle(screen,x2,y2,6,col)
# Draw wheel
col=(0,0,255)
gui.fill_circle(screen,x1,y1,robot_r,col)
# Draw floor
col=(255,255,0)
thick=3
gui.draw_line(screen,0,y1+robot_r,screen.get_width(),y1+robot_r,col,thick)
# Draw floor reference lines
col=(255,0,50)
thick=8
gui.draw_line(screen,screen.get_width()-xm1+x1,y1+robot_r,screen.get_width()-xm1+x1,y1+robot_r+40,col,thick)
gui.draw_line(screen,screen.get_width()-xm1-x1,y1+robot_r,screen.get_width()-xm1-x1,y1+robot_r+40,col,thick)
thick=3
col=(255,255,255)
gui.draw_line(screen,0.*screen.get_width()-xm1,y1+robot_r,0*screen.get_width()-xm1,y1+robot_r+20,col,thick)
gui.draw_line(screen,0.25*screen.get_width()-xm1,y1+robot_r,0.25*screen.get_width()-xm1,y1+robot_r+20,col,thick)
gui.draw_line(screen,0.75*screen.get_width()-xm1,y1+robot_r,0.75*screen.get_width()-xm1,y1+robot_r+20,col,thick)
gui.draw_line(screen,1*screen.get_width()-xm1,y1+robot_r,1*screen.get_width()-xm1,y1+robot_r+20,col,thick)
gui.draw_line(screen,1.25*screen.get_width()-xm1,y1+robot_r,1.25*screen.get_width()-xm1,y1+robot_r+20,col,thick)
gui.draw_line(screen,1.75*screen.get_width()-xm1,y1+robot_r,1.75*screen.get_width()-xm1,y1+robot_r+20,col,thick)
gui.draw_line(screen,2*screen.get_width()-xm1,y1+robot_r,2*screen.get_width()-xm1,y1+robot_r+20,col,thick)
#Draw the wheel angle reference line
wx1 = robot_r*math.sin(angW)
wy1 = robot_r*math.cos(angW)
thick=3
col=(255,255,255)
gui.draw_line(screen,x1+wx1,y1+wy1,x1,y1,col,thick)
# Display the state of the wheel and robot body
str2=""
str2+= "O: %5.2f " % (state[0])
str2+= "dO/dt: %5.2f " % (state[1])
str2+= "beta: %5.2f " % (state[2])
str2+= "dbeta/dt: %5.2f " % (state[3])
str2+= " pwmRatio: %5.2f " % (pwmRatio)
gui.draw_string(screen,str2,(20,10),col,16)
# Copy the screen onto the display
gui.blit(screen)
def draw(screen, robot, pwmRatio):
"""
Called by the simulation to display the current state of the robot.
"""
#Get the robot state
state = robot.getState()
# Map robot angles to the screen coordinate system
angW = (-state[0]) + math.pi #Wheel angel
angB = (angW + state[2]) #Body angle
# Clear screen
screen.fill((0, 0, 0))
# Gget the size of the window
wid = gui.dim_window[0]
hei = gui.dim_window[1]
# Get robot dimensions
length = robot.L
scale = hei / length / 3.0
robot_r = int(robot.wR * 1000)
#robot_r = int(robot.wR*scale) #Use this value for robot_r to display robot to scale
# Robot position mapped to the screen coordinate system
x1 = screen.get_width() / 2 #Robot reference X position on screen
xm1 = (robot_r *
state[0]) % screen.get_width() #Robot true X displacement on screen
y1 = hei * .6 #Robot base line for wheel on screen
# X,Y Coordinate of the body's centre of gravity
x2 = x1 + scale * math.sin(angB) * length
y2 = y1 + scale * math.cos(angB) * length
# Draw robot's centre of gravity
col = (255, 0, 0)
thick = 3
gui.draw_line(screen, x1, y1, x2, y2, col, thick)
col = (0, 255, 0)
gui.fill_circle(screen, x2, y2, 6, col)
# Draw wheel
col = (0, 0, 255)
gui.fill_circle(screen, x1, y1, robot_r, col)
# Draw floor
col = (255, 255, 0)
thick = 3
gui.draw_line(screen, 0, y1 + robot_r, screen.get_width(), y1 + robot_r,
col, thick)
# Draw floor reference lines
col = (255, 0, 50)
thick = 8
gui.draw_line(screen,
screen.get_width() - xm1 + x1, y1 + robot_r,
screen.get_width() - xm1 + x1, y1 + robot_r + 40, col, thick)
gui.draw_line(screen,
screen.get_width() - xm1 - x1, y1 + robot_r,
screen.get_width() - xm1 - x1, y1 + robot_r + 40, col, thick)
thick = 3
col = (255, 255, 255)
gui.draw_line(screen, 0. * screen.get_width() - xm1, y1 + robot_r,
0 * screen.get_width() - xm1, y1 + robot_r + 20, col, thick)
gui.draw_line(screen, 0.25 * screen.get_width() - xm1, y1 + robot_r,
0.25 * screen.get_width() - xm1, y1 + robot_r + 20, col,
thick)
gui.draw_line(screen, 0.75 * screen.get_width() - xm1, y1 + robot_r,
0.75 * screen.get_width() - xm1, y1 + robot_r + 20, col,
thick)
gui.draw_line(screen, 1 * screen.get_width() - xm1, y1 + robot_r,
1 * screen.get_width() - xm1, y1 + robot_r + 20, col, thick)
gui.draw_line(screen, 1.25 * screen.get_width() - xm1, y1 + robot_r,
1.25 * screen.get_width() - xm1, y1 + robot_r + 20, col,
thick)
gui.draw_line(screen, 1.75 * screen.get_width() - xm1, y1 + robot_r,
1.75 * screen.get_width() - xm1, y1 + robot_r + 20, col,
thick)
gui.draw_line(screen, 2 * screen.get_width() - xm1, y1 + robot_r,
2 * screen.get_width() - xm1, y1 + robot_r + 20, col, thick)
#Draw the wheel angle reference line
wx1 = robot_r * math.sin(angW)
wy1 = robot_r * math.cos(angW)
thick = 3
col = (255, 255, 255)
gui.draw_line(screen, x1 + wx1, y1 + wy1, x1, y1, col, thick)
# Display the state of the wheel and robot body
str2 = ""
str2 += "O: %5.2f " % (state[0])
str2 += "dO/dt: %5.2f " % (state[1])
str2 += "beta: %5.2f " % (state[2])
str2 += "dbeta/dt: %5.2f " % (state[3])
str2 += " pwmRatio: %5.2f " % (pwmRatio)
gui.draw_string(screen, str2, (20, 10), col, 16)
# Copy the screen onto the display
gui.blit(screen)