def __init__(self):
mass = .5
ball_radius = 25
#~ moment = pymunk.moment_for_circle(mass, 0, radius, (0,0))
self.body = pymunk.Body(mass, 10)
self.shape =pymunk.Circle(self.body, 20)
self.shape.color=pygame.Color(255, 255, 0)
mp=from_pygame( (pygame.mouse.get_pos()), screen )
self.body.position=(mp[0],mp[1])
self.shape.ignore_draw =True
def do_event(self, event):
"""Do object selection."""
if event.type == MOUSEBUTTONDOWN:
self.selecting = False
self.sel_rect.topleft = event.pos
self.sel_rect.size = 0, 0
p = from_pygame(event.pos, self.screen)
self.make_active_shape(p)
elif event.type == MOUSEMOTION:
if self.active_shape != None:
b = self.active_shape.body
b.position = from_pygame(event.pos, self.screen)
if event.buttons[0] == 1:
self.selecting = True
self.sel_rect.width += event.rel[0]
self.sel_rect.height += event.rel[1]
elif event.type == MOUSEBUTTONUP:
self.selecting = False
def set_walls(self, lines):
self.lines = [pymunk.Segment(self.static_body, from_pygame(x, y), from_pygame(x2, y2), 0)
for x, y, x2, y2 in lines]
self.space.add(self.lines)
def main():
### PyGame init
pygame.init()
screen = pygame.display.set_mode((width,height))
clock = pygame.time.Clock()
running = True
font = pygame.font.SysFont("Arial", 16)
### Physics stuff
space = pymunk.Space()
space.gravity = 0,-1000
# walls - the left-top-right walls
static= [pymunk.Segment(space.static_body, (50, 50), (50, 550), 5)
,pymunk.Segment(space.static_body, (50, 550), (650, 550), 5)
,pymunk.Segment(space.static_body, (650, 550), (650, 50), 5)
,pymunk.Segment(space.static_body, (50, 50), (650, 50), 5)
]
b2 = pymunk.Body()
static.append(pymunk.Circle(b2, 30))
b2.position = 300,400
for s in static:
s.friction = 1.
s.group = 1
space.add(static)
# "Cannon" that can fire arrows
cannon_body = pymunk.Body(pymunk.inf, pymunk.inf)
cannon_shape = pymunk.Circle(cannon_body, 25)
cannon_shape.sensor = True
cannon_body.position = 100,100
space.add(cannon_shape)
arrow_body,arrow_shape = create_arrow()
space.add(arrow_shape)
space.add_collision_handler(0, 1, post_solve=post_solve_arrow_hit)
flying_arrows = []
while running:
for event in pygame.event.get():
if event.type == QUIT or \
event.type == KEYDOWN and (event.key in [K_ESCAPE, K_q]):
running = False
elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
start_time = pygame.time.get_ticks()
elif event.type == KEYDOWN and event.key == K_p:
pygame.image.save(screen, "arrows.png")
elif event.type == pygame.MOUSEBUTTONUP and event.button == 1:
end_time = pygame.time.get_ticks()
diff = end_time - start_time
power = max(min(diff, 1000), 10) * 1.5
impulse = power * Vec2d(1,0)
arrow_body.apply_impulse(impulse.rotated(arrow_body.angle))
space.add(arrow_body)
flying_arrows.append(arrow_body)
arrow_body, arrow_shape = create_arrow()
space.add(arrow_shape)
keys = pygame.key.get_pressed()
speed = 2.5
if (keys[K_UP]):
cannon_body.position += Vec2d(0,1) * speed
if (keys[K_DOWN]):
cannon_body.position += Vec2d(0,-1) * speed
if (keys[K_LEFT]):
cannon_body.position += Vec2d(-1,0) * speed
if (keys[K_RIGHT]):
cannon_body.position += Vec2d(1,0) * speed
mouse_position = from_pygame( Vec2d(pygame.mouse.get_pos()), screen )
cannon_body.angle = (mouse_position - cannon_body.position).angle
# move the unfired arrow together with the cannon
arrow_body.position = cannon_body.position + Vec2d(cannon_shape.radius + 40, 0).rotated(cannon_body.angle)
arrow_body.angle = cannon_body.angle
for flying_arrow in flying_arrows:
drag_constant = 0.0002
pointing_direction = Vec2d(1,0).rotated(flying_arrow.angle)
flight_direction = Vec2d(flying_arrow.velocity)
flight_speed = flight_direction.normalize_return_length()
dot = flight_direction.dot(pointing_direction)
drag_force_magnitude = (1-abs(dot)) * flight_speed **2 * drag_constant * flying_arrow.mass
arrow_tail_position = Vec2d(-50, 0).rotated(flying_arrow.angle)
flying_arrow.apply_impulse(drag_force_magnitude * -flight_direction, arrow_tail_position)
flying_arrow.angular_velocity *= 0.9
### Clear screen
screen.fill(pygame.color.THECOLORS["black"])
### Draw stuff
draw_space(screen, space)
# Power meter
if pygame.mouse.get_pressed()[0]:
current_time = pygame.time.get_ticks()
diff = current_time - start_time
power = max(min(diff, 1000), 10)
h = power / 2
pygame.draw.line(screen, pygame.color.THECOLORS["red"], (30,550), (30,550-h), 10)
# Info and flip screen
screen.blit(font.render("fps: " + str(clock.get_fps()), 1, THECOLORS["white"]), (0,0))
screen.blit(font.render("Aim with mouse, hold LMB to powerup, release to fire", 1, THECOLORS["darkgrey"]), (5,height - 35))
screen.blit(font.render("Press R to reset, ESC or Q to quit", 1, THECOLORS["darkgrey"]), (5,height - 20))
pygame.display.flip()
### Update physics
fps = 60
dt = 1./fps
space.step(dt)
clock.tick(fps)
def main():
### PyGame init
pygame.init()
screen = pygame.display.set_mode((width,height))
clock = pygame.time.Clock()
running = True
font = pygame.font.SysFont("Arial", 16)
### Physics stuff
space = pymunk.Space()
space.gravity = 0,-1000
# walls - the left-top-right walls
static= [pymunk.Segment(space.static_body, (50, 50), (50, 550), 5)
,pymunk.Segment(space.static_body, (50, 550), (650, 550), 5)
,pymunk.Segment(space.static_body, (650, 550), (650, 50), 5)
,pymunk.Segment(space.static_body, (50, 50), (650, 50), 5)
]
b2 = pymunk.Body()
static.append(pymunk.Circle(b2, 30))
b2.position = 300,400
for s in static:
s.friction = 1.
s.group = 1
space.add(static)
# "Cannon" that can fire arrows
cannon_body = pymunk.Body(pymunk.inf, pymunk.inf)
cannon_shape = pymunk.Circle(cannon_body, 25)
cannon_shape.sensor = True
cannon_body.position = 100,100
space.add(cannon_shape)
arrow_body,arrow_shape = create_arrow()
space.add(arrow_shape)
space.add_collision_handler(0, 1, post_solve=post_solve_arrow_hit)
flying_arrows = []
while running:
for event in pygame.event.get():
if event.type == QUIT or \
event.type == KEYDOWN and (event.key in [K_ESCAPE, K_q]):
running = False
elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
start_time = pygame.time.get_ticks()
elif event.type == KEYDOWN and event.key == K_p:
pygame.image.save(screen, "arrows.png")
elif event.type == pygame.MOUSEBUTTONUP and event.button == 1:
end_time = pygame.time.get_ticks()
diff = end_time - start_time
power = max(min(diff, 1000), 10) * 1.5
impulse = power * Vec2d(1,0)
arrow_body.apply_impulse(impulse.rotated(arrow_body.angle))
space.add(arrow_body)
flying_arrows.append(arrow_body)
arrow_body, arrow_shape = create_arrow()
space.add(arrow_shape)
keys = pygame.key.get_pressed()
speed = 2.5
if (keys[K_UP]):
cannon_body.position += Vec2d(0,1) * speed
if (keys[K_DOWN]):
cannon_body.position += Vec2d(0,-1) * speed
if (keys[K_LEFT]):
cannon_body.position += Vec2d(-1,0) * speed
if (keys[K_RIGHT]):
cannon_body.position += Vec2d(1,0) * speed
mouse_position = from_pygame( Vec2d(pygame.mouse.get_pos()), screen )
cannon_body.angle = (mouse_position - cannon_body.position).angle
# move the unfired arrow together with the cannon
arrow_body.position = cannon_body.position + Vec2d(cannon_shape.radius + 40, 0).rotated(cannon_body.angle)
arrow_body.angle = cannon_body.angle
for flying_arrow in flying_arrows:
drag_constant = 0.0002
pointing_direction = Vec2d(1,0).rotated(flying_arrow.angle)
flight_direction = Vec2d(flying_arrow.velocity)
flight_speed = flight_direction.normalize_return_length()
dot = flight_direction.dot(pointing_direction)
# (1-abs(dot)) can be replaced with (1-dot) to make arrows turn around even when fired straight up.
# Might not be as accurate, but maybe look better.
drag_force_magnitude = (1-abs(dot)) * flight_speed **2 * drag_constant * flying_arrow.mass
arrow_tail_position = Vec2d(-50, 0).rotated(flying_arrow.angle)
flying_arrow.apply_impulse(drag_force_magnitude * -flight_direction, arrow_tail_position)
flying_arrow.angular_velocity *= 0.9
### Clear screen
screen.fill(pygame.color.THECOLORS["black"])
### Draw stuff
draw(screen, space)
# Power meter
if pygame.mouse.get_pressed()[0]:
current_time = pygame.time.get_ticks()
diff = current_time - start_time
power = max(min(diff, 1000), 10)
h = power / 2
pygame.draw.line(screen, pygame.color.THECOLORS["red"], (30,550), (30,550-h), 10)
# Info and flip screen
screen.blit(font.render("fps: " + str(clock.get_fps()), 1, THECOLORS["white"]), (0,0))
screen.blit(font.render("Aim with mouse, hold LMB to powerup, release to fire", 1, THECOLORS["darkgrey"]), (5,height - 35))
screen.blit(font.render("Press R to reset, ESC or Q to quit", 1, THECOLORS["darkgrey"]), (5,height - 20))
pygame.display.flip()
### Update physics
fps = 60
dt = 1./fps
space.step(dt)
clock.tick(fps)
def keyControl(self):
isMinJerk = False
isPointToPoint = False
while (self.running):
""" angle calculated from mouse cursor position"""
mouse_position = from_pygame( Vec2d(self.pygame.mouse.get_pos()), self.screen )
forced_angle = (mouse_position-self.gForearm_body.position).angle # calculate angle with mouse cursor loc.
# move the unfired arrow together with the cannon
# arrow_body.position = cannon_body.position + Vec2d(cannon_shape.radius + 40, 0).rotated(cannon_body.angle)
# arrow_body.angle = cannon_body.angle
# self.gForearm_body.torque = -0.1
# rest-length change
if (isMinJerk):
d = 10 # 40 , speed
jmax = 0.8 # 1.0 , joint maxzz
t = jmax * (10*(self.timeMinJerk/d)**3 - 15*(self.timeMinJerk/d)**4 + 6*(self.timeMinJerk/d)**5)
if t > jmax:
self.jointMin = jmax
xem_cortical_bic.SendButton(False, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
xem_cortical_tri.SendButton(False, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
print "t > jmax"
else:
self.jointMin = t
if self.timeMinJerk == 0.0: # enter only once
print "time 0"
xem_cortical_bic.SendButton(True, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
xem_cortical_tri.SendButton(True, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
# xem_cortical_bic.SendPara(bitVal = 5000, trigEvent = 8) # up (TONIC ON triceps)
elif self.timeMinJerk == 7.0:
# xem_cortical_tri.SendPara(bitVal = 5000, trigEvent = 8) # down (TONIC ON biceps)
print "time 7"
self.timeMinJerk+= 1.0 # time step
# point to point trigger in for only one cycle (non-repetitive)
if (isPointToPoint):
if self.p2ptime > 500.0: # 50.0
xem_cortical_bic.SendButton(False, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
xem_cortical_tri.SendButton(False, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
self.p2ptime += 1.0
print self.p2ptime
""" key control """
for event in self.pygame.event.get():
if event.type == QUIT:
self.running = False
elif event.type == KEYDOWN and event.key == K_ESCAPE:
self.plotData(self.data_bic, self.data_tri)
self.running = False
elif event.type == KEYDOWN and event.key == K_p: # Point-to-point
# isMinJerk = True
bitVal = convertType(0.0, fromType = 'f', toType = 'I')
xem_cortical_bic.SendPara(bitVal = 0, trigEvent = 8)
xem_cortical_tri.SendPara(bitVal = 0, trigEvent = 8)
# xem_cortical_bic.SendButton(True, BUTTON_RESET_SIM) #
# xem_cortical_tri.SendButton(True, BUTTON_RESET_SIM) #
#
# xem_cortical_tri.SendButton(False, BUTTON_RESET_SIM) #
# xem_cortical_bic.SendButton(False, BUTTON_RESET_SIM) #
self.point2pointForce(True) # point-to-point movement
# bitVal = convertType(200.0, fromType = 'f', toType = 'I')
# xem_cortical_bic.SendPara(bitVal = 5000, trigEvent = 8)
# xem_cortical_tri.SendPara(bitVal = 5000, trigEvent = 8)
elif event.type == KEYDOWN and event.key == K_j:
self.gForearm_body.torque -= 14.0
elif event.type == KEYDOWN and event.key == K_f:
#self.gForearm_body.apply_force(Vec2d.unit() * -40000, (-100,0))
self.gForearm_body.torque += 14.0
elif event.type == KEYDOWN and event.key == K_t: # tonic on
# bitVal = convertType(6000.0, fromType = 'f', toType = 'I')
xem_cortical_bic.SendPara(bitVal = 18000, trigEvent = 8)
xem_cortical_tri.SendPara(bitVal = 12000, trigEvent = 8)
elif event.type == KEYDOWN and event.key == K_y: # tonic off
bitVal = convertType(0.0, fromType = 'f', toType = 'I')
xem_cortical_bic.SendPara(bitVal = 0, trigEvent = 8)
xem_cortical_tri.SendPara(bitVal = 0, trigEvent = 8)
elif event.type == KEYDOWN and event.key == K_z:
# self.gRest_joint_angle = self.angle
self.gForearm_body.angle = 0.0
elif event.type == KEYDOWN and event.key == K_r:
self.gForearm_body.apply_impulse(Vec2d.unit()*0.3, (4, 0))
elif event.type == KEYDOWN and event.key == K_u:
self.gForearm_body.apply_impulse(Vec2d.unit()*0.3, (-4, 0))
elif event.type == KEYDOWN and event.key == K_s: #reset-sim boards
xem_cortical_bic.SendButton(True, BUTTON_RESET_SIM) #
xem_cortical_tri.SendButton(True, BUTTON_RESET_SIM) #
# xem_cortical_tri.SendButton(False, BUTTON_RESET_SIM) #
# xem_cortical_bic.SendButton(False, BUTTON_RESET_SIM) #
elif event.type == KEYDOWN and event.key == K_d: #reset-sim boards
xem_cortical_bic.SendButton(False, BUTTON_RESET_SIM) #
xem_cortical_tri.SendButton(False, BUTTON_RESET_SIM) #
# elif event.type == KEYDOWN and event.key == K_d: #reset-sim boards
elif event.type == KEYDOWN and event.key == K_e: # trigger off
xem_cortical_bic.SendButton(False, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
xem_cortical_tri.SendButton(False, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
elif event.type == KEYDOWN and event.key == K_w: # trigger on
isPointToPoint = True
xem_cortical_bic.SendButton(True, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
xem_cortical_tri.SendButton(True, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
# elif event.type == KEYDOWN and event.key == K_o: # CN syn gain 50
# bitVal50 = convertType(50.0, fromType = 'f', toType = 'I')
# xem_muscle_bic.SendPara(bitVal = bitVal50, trigEvent = 10)
# xem_muscle_tri.SendPara(bitVal = bitVal50, trigEvent = 10)
# elif event.type == KEYDOWN and event.key == K_p: # CN syn gain 100
# bitVal100 = convertType(100.0, fromType = 'f', toType = 'I')
# xem_muscle_bic.SendPara(bitVal = bitVal100, trigEvent = 10)
# xem_muscle_tri.SendPara(bitVal = bitVal100, trigEvent = 10)
elif event.type == KEYDOWN and event.key == K_l: # forced movement, follow the mouse
if (self.record == True):
self.record = False
if (self.record == False):
self.record = True
k = 1
""" Minos - A simple angle servo """
# servo_torque = 3.0 * (30.0/180.0 * 3.141592 - self.gForearm_body.angle)
# self.gForearm_body.torque += servo_torque
""" mouse cursor controlled forced (passive) movement"""
if (self.record == True):
#self.gForearm_body.angle =forced_angle
if k == len(self.j2List)-1:
self.gForearm_body.angle = 0.0
else:
self.gForearm_body.angle = (self.j2List[k])*3.141592/180 # in radian
k = k + 1
#print self.j2List[k]
""" Clear screen """
self.screen.fill(THECOLORS["white"]) # ~1ms
""" Draw stuff """
for f in [self.gForearm_shape,]:
ps = f.get_points()
ps.append(ps[0])
ps = map(self.to_pygame, ps)
color = THECOLORS["black"]
self.pygame.draw.lines(self.screen, color, False, ps, 2)
#if abs(flipper_body.angle) < 0.001: flipper_body.angle = 0
"""draw circle """
# pygame.draw.circle(self.screen, THECOLORS["black"], (300, 300), int(42), 0)
# pygame.draw.circle(self.screen, THECOLORS["white"], (300, 300), int(40), 0)
# pygame.draw.circle(self.screen, THECOLORS["black"], (300, 300), int(3), 0)
# pygame.draw.line(self.screen, THECOLORS["black"], [300, 300-42], [500, 300-42], 2)
# pygame.draw.line(self.screen, THECOLORS["black"], [300, 300+40], [500, 300+40], 2)
""" Update physics """
fps = 30.0 #was 30.0
step = 1
dt = 1.0/fps/step
for x in range(step):
# self.gSpace.step(dt)
self.gSpace.step(0.001*8*2)
""" text message"""
myfont = self.pygame.font.SysFont("monospace", 15)
label1 = myfont.render("j:torque down, f: torque up" , 1, THECOLORS["black"])
label2 = myfont.render("l: mouse-controlled movement, esc:out" , 1, THECOLORS["black"])
self.screen.blit(label1, (10, 10))
self.screen.blit(label2, (10, 40))
""" Flip screen (big delay from here!) """
self.pygame.display.flip() # ~1ms
self.gClock.tick(fps) # target fps
# self.gClock.tick(80) # oscillate
self.pygame.display.set_caption("fps: " + str(self.gClock.get_fps()))
def keyControl(self):
while (self.running):
""" angle calculated from mouse cursor position"""
mouse_position = from_pygame( Vec2d(self.pygame.mouse.get_pos()), self.screen )
forced_angle = (mouse_position-self.gForearm_body.position).angle # calculate angle with mouse cursor loc.
# move the unfired arrow together with the cannon
# arrow_body.position = cannon_body.position + Vec2d(cannon_shape.radius + 40, 0).rotated(cannon_body.angle)
# arrow_body.angle = cannon_body.angle
# self.gForearm_body.torque = -0.1
""" key control """
for event in self.pygame.event.get():
if event.type == QUIT:
self.running = False
elif event.type == KEYDOWN and event.key == K_ESCAPE:
self.plotData(self.data_index_flexor, self.data_index_extensor, self.data_middle_flexor, self.data_middle_extensor)
self.running = False
elif event.type == KEYDOWN and event.key == K_j:
self.gForearm_body.torque -= 14.0
elif event.type == KEYDOWN and event.key == K_f:
#self.gForearm_body.apply_force(Vec2d.unit() * -40000, (-100,0))
self.gForearm_body.torque += 14.0
#
elif event.type == KEYDOWN and event.key == K_z:
self.gForearm_body.angle = 0.0
elif event.type == KEYDOWN and event.key == K_x:
self.gForearm_body_middle.angle = 0.0
elif event.type == KEYDOWN and event.key == K_r:
self.gForearm_body_middle.torque += 14.0
elif event.type == KEYDOWN and event.key == K_u:
self.gForearm_body_middle.torque -= 14.0
##
# elif event.type == KEYDOWN and event.key == K_o: # CN syn gain 50
# bitVal50 = convertType(50.0, fromType = 'f', toType = 'I')
# xem_muscle_index.SendPara(bitVal = bitVal50, trigEvent = 10)
# xem_muscle.SendPara(bitVal = bitVal50, trigEvent = 10)
# elif event.type == KEYDOWN and event.key == K_p: # CN syn gain 100
# bitVal100 = convertType(100.0, fromType = 'f', toType = 'I')
# xem_muscle_index.SendPara(bitVal = bitVal100, trigEvent = 10)
# xem_muscle.SendPara(bitVal = bitVal100, trigEvent = 10)
elif event.type == KEYDOWN and event.key == K_l: # forced movement, follow the mouse
if (self.record == True):
self.record = False
if (self.record == False):
self.record = True
k = 1
""" mouse cursor controlled forced (passive) movement"""
if (self.record == True):
self.gForearm_body.angle =forced_angle
# if k == len(self.j2List)-1:
# self.gForearm_body.angle = 0.0
# else:
# self.gForearm_body.angle = (self.j2List[k])*3.141592/180 # in radian
# k = k + 1
# #print self.j2List[k]
""" Clear screen """
self.screen.fill(THECOLORS["white"]) # ~1ms
""" Draw stuff """
for f in [self.gForearm_shape,]:
ps = f.get_points()
ps.append(ps[0])
ps = map(self.to_pygame, ps)
color = THECOLORS["black"]
self.pygame.draw.lines(self.screen, color, False, ps, 2)
#if abs(flipper_body.angle) < 0.001: flipper_body.angle = 0
for f in [self.gForearm_shape_middle,]:
ps = f.get_points()
ps.append(ps[0])
ps = map(self.to_pygame, ps)
color = THECOLORS["black"]
self.pygame.draw.lines(self.screen, color, False, ps, 2)
"""draw circle """
# pygame.draw.circle(self.screen, THECOLORS["black"], (300, 300), int(42), 0)
# pygame.draw.circle(self.screen, THECOLORS["white"], (300, 300), int(40), 0)
# pygame.draw.circle(self.screen, THECOLORS["black"], (300, 300), int(3), 0)
# pygame.draw.line(self.screen, THECOLORS["black"], [300, 300-42], [500, 300-42], 2)
# pygame.draw.line(self.screen, THECOLORS["black"], [300, 300+40], [500, 300+40], 2)
""" Update physics """
fps = 30.0 #was 30.0
step = 1
dt = 1.0/fps/step
for x in range(step):
self.gSpace.step(dt) # dt
""" text message"""
myfont = self.pygame.font.SysFont("monospace", 15)
label1 = myfont.render("j:torque down, f: torque up" , 1, THECOLORS["black"])
label2 = myfont.render("l: mouse-controlled movement, esc:out" , 1, THECOLORS["black"])
self.screen.blit(label1, (10, 10))
self.screen.blit(label2, (10, 40))
""" Flip screen (big delay from here!) """
self.pygame.display.flip() # ~1ms
self.gClock.tick(fps) # target fps
# self.gClock.tick(80) # oscillate
self.pygame.display.set_caption("fps: " + str(self.gClock.get_fps()))
def keyControl(self):
while (self.running):
""" angle calculated from mouse cursor position"""
mouse_position = from_pygame( Vec2d(self.pygame.mouse.get_pos()), self.screen )
forced_angle = (mouse_position-self.gForearm_body.position).angle # calculate angle with mouse cursor loc.
# xem_cortical_bic.SendButton(True, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
# xem_cortical_tri.SendButton(True, BUTTON_INPUT_FROM_TRIGGER) # BUTTON_INPUT_FROM_TRIGGER = 1
#
# xem_cortical_bic.SendButton(True, BUTTON_RESET_SIM) #
# xem_cortical_tri.SendButton(True, BUTTON_RESET_SIM) #
# move the unfired arrow together with the cannon
# arrow_body.position = cannon_body.position + Vec2d(cannon_shape.radius + 40, 0).rotated(cannon_body.angle)
# arrow_body.angle = cannon_body.angle
# self.gForearm_body.torque = -0.1
""" key control """
for event in self.pygame.event.get():
if event.type == QUIT:
self.running = False
elif event.type == KEYDOWN and event.key == K_ESCAPE:
self.escape()
elif event.type == KEYDOWN and event.key == K_p:
self.point2pointForce(True)
elif event.type == KEYDOWN and event.key == K_j:
self.cTorque() # clock-wise torque
elif event.type == KEYDOWN and event.key == K_0:
self.softReset()
elif event.type == KEYDOWN and event.key == K_f:
self.ccTorque() # counter-clockwise torque
elif event.type == KEYDOWN and event.key == K_r: # selective tonic on
self.tonicDrive(200.0)
elif event.type == KEYDOWN and event.key == K_y: # tonic off
self.tonicDrive(0.0)
elif event.type == KEYDOWN and event.key == K_z:
self.gForearm_body.angle = 0.0
elif event.type == KEYDOWN and event.key == K_d:
self.currGammaDyn = 0.0
self.softReset()
self.setGammaDyn()
elif event.type == KEYDOWN and event.key == K_e:
self.currGammaDyn += self.GAMMA_INC
self.setGammaDyn()
elif event.type == KEYDOWN and event.key == K_s:
self.currGammaSta = 0.0
self.setGammaSta()
elif event.type == KEYDOWN and event.key == K_w:
self.currGammaSta += self.GAMMA_INC
self.softReset()
self.setGammaSta()
# elif event.type == KEYDOWN and event.key == K_r:
# self.gForearm_body.apply_impulse(Vec2d.unit()*0.1, (4, 0))
# elif event.type == KEYDOWN and event.key == K_u:
# self.gForearm_body.apply_impulse(Vec2d.unit()*0.1, (-4, 0))
elif event.type == KEYDOWN and event.key == K_o: # set CN syn gain= 50
self.corticalGain(50.0)
elif event.type == KEYDOWN and event.key == K_m: # forced movement, follow the mouse
self.mouseOn = True
elif event.type == KEYDOWN and event.key == K_l: # doornik replay
if (self.record == True):
self.record = False
if (self.record == False):
self.record = True
k = 1
""" mouse cursor controlled forced (passive) movement"""
if (self.mouseOn == True):
self.gForearm_body.angle =forced_angle
""" doornik replay """
if (self.record == True):
#self.gForearm_body.angle =forced_angle
if k == len(self.j2List)-1:
self.gForearm_body.angle = 0.0
else:
self.gForearm_body.angle = (self.j2List[k])*3.141592/180 # in radian
k = k + 1
#print self.j2List[k]
""" Clear screen """
self.screen.fill(THECOLORS["white"]) # ~1ms
""" Draw stuff """
for f in [self.gForearm_shape,]:
ps = f.get_points()
ps.append(ps[0])
ps = map(self.toPygame, ps)
color = THECOLORS["black"]
self.pygame.draw.lines(self.screen, color, False, ps, 2)
#if abs(flipper_body.angle) < 0.001: flipper_body.angle = 0
"""draw circle """
# pygame.draw.circle(self.screen, THECOLORS["black"], (300, 300), int(42), 0)
# pygame.draw.circle(self.screen, THECOLORS["white"], (300, 300), int(40), 0)
# pygame.draw.circle(self.screen, THECOLORS["black"], (300, 300), int(3), 0)
# pygame.draw.line(self.screen, THECOLORS["black"], [300, 300-42], [500, 300-42], 2)
# pygame.draw.line(self.screen, THECOLORS["black"], [300, 300+40], [500, 300+40], 2)
""" Update physics """
fps = 60.0 #was 30.0
step = 1
dt = 1.0/fps/step
for x in range(step):
self.gSpace.step(dt) #(0.001*8) matters, matched with control loop update speed..
self.ind += 1
""" text message"""
myfont = self.pygame.font.SysFont("monospace", 15)
label1 = myfont.render("j:torque c, f: torque cc" , 1, THECOLORS["black"])
label2 = myfont.render("l: mouse-controlled movement, esc:out" , 1, THECOLORS["black"])
label3 = myfont.render("Trial %d/%d, GammaDynBic %.1f, GammaStaBic %.1f" % \
(self.currTrial, self.TOTAL_TRIALS, \
self.currGammaDyn, self.currGammaSta), 1, THECOLORS["black"])
label5 = myfont.render("LceBic %.1f, LceTri %.1f" % \
(self.lce_bic, self.lce_tri), 1, THECOLORS["black"])
label6 = myfont.render("ForceBic %.1f, ForceTri %.1f" % \
(self.force_bic, self.force_tri), 1, THECOLORS["black"])
self.screen.blit(label1, (10, 10))
self.screen.blit(label2, (10, 30))
self.screen.blit(label3, (10, 50))
# self.screen.blit(label4, (10, 70))
self.screen.blit(label5, (10, 90))
self.screen.blit(label6, (10, 110))
""" Flip screen (big delay from here!) """
self.pygame.display.flip() # ~1ms
self.gClock.tick(fps) # target fps
# self.gClock.tick(80) # oscillate
self.pygame.display.set_caption("fps: " + str(self.gClock.get_fps()))
def main():
### PyGame init
pygame.init()
clock = pygame.time.Clock()
running = True
### Physics stuff
space.gravity = 0,-980
static= [pymunk.Segment(space.static_body, (10, 50), (10, 590), 5),
pymunk.Segment(space.static_body, (500, 200), (900,200), 5),
pymunk.Segment(space.static_body, (500, 100), (900,100), 5),
pymunk.Segment(space.static_body, (500, 300), (900,300), 5),
pymunk.Segment(space.static_body, (500, 100), (450,150), 5),
pymunk.Segment(space.static_body, (10, 590), (890, 590), 5)
]
for s in static:
s.friction = 1.
s.group = 1
space.add(static)
# "Cannon" that can fire arrows
cannon_body = pymunk.Body(10, 10)
cannon_shape = pymunk.Circle(cannon_body, .1)
cannon_shape.ignore_draw=True
#~ cannon_shape.sensor = True
cannon_body.position = 100,150
#~ space.add(cannon_shape)
ball_body,ball_shape = create_ball()
pig_body,pig_shape = create_pig()
#~ ball_shape.ignore_draw=True
while running:
start=Vec2d (100,300)
for event in pygame.event.get():
mp=from_pygame( (pygame.mouse.get_pos()), screen )
if event.type == QUIT or \
event.type == KEYDOWN and (event.key in [K_ESCAPE, K_q]):
running = False
elif pygame.mouse.get_pressed()[1]:
pig_body.position=mp
space.add(pig_body)
space.add(pig_shape)
pig_body, pig_shape = create_pig()
print len(space.shapes)
break
elif event.type == pygame.MOUSEMOTION and pygame.mouse.get_pressed()[0]:
cannon_body.position =Vec2d (mp[0],mp[1])
print cannon_body.position
elif event.type == pygame.MOUSEBUTTONUP and pygame.mouse.get_pressed()[1]==False:
diff = -1*(Vec2d(mp) - start)
print("abs="+str(diff))
cannon_body.position =start
impulse = diff* 75
ball_body.apply_impulse(impulse.rotated(ball_body.angle))
space.add(ball_body)
#~ ball_shape.ignore_draw=True
ball_body, ball_shape = create_ball()
space.add(ball_shape)
mouse_position = from_pygame( Vec2d(pygame.mouse.get_pos()), screen )
cannon_body.angle = (mouse_position - cannon_body.position).angle
# move the unfired ball together with the cannon
ball_body.position = cannon_body.position
ball_body.angle = cannon_body.angle
### Clear screen
screen.fill(pygame.color.THECOLORS["white"])
### Draw stuff
background = os.path.join("bgsl.png")
background_surface = pygame.image.load(background)
screen.blit(background_surface, (0,0))
#~ animation_offset = 32*0
col=[]
pigs=[]
for x in space.shapes:
if x.body.mass>1:
col.append(x)
else:
pigs.append(x)
#~ print len(col)
for x in col:
tem=x.body.position
tem=list(tem)
pos=tem[0]-20,tem[1]+20
#~ position=5
#~ print (s)
screen.blit(img, to_pygame(pos, screen))
for x in pigs:
tem=x.body.position
tem=list(tem)
pos=tem[0]-30,tem[1]+20
#~ position=5
#~ print (s)
screen.blit(pigimg, to_pygame(pos, screen))
draw_space(screen, space)
pygame.display.flip()
### Update physics
fps = 60
dt = 1./fps
space.step(dt)
clock.tick(fps)
def get_mouse_pos(self, is_pygame = True): if is_pygame: return pygame.mouse.get_pos() return from_pygame(pygame.mouse.get_pos(), self.screen)
