def gyro(self, data, fast_mode, frametime):
## gyro ##
self.data["raw_gyro"] = data
gyro_speed = [visual.radians((data[i] - self.param["gyro_zero"][i]) / self.param["gyro_gain"]) for i in xrange(3)]
for i in xrange(3):
if fast_mode[i]:
gyro_speed[i] *= self.param["gyro_fast"]
cutoff = [i for i in xrange(3) if abs(gyro_speed[i]) > self.param["gyro_thresh"]]
if len(cutoff):
for i in xrange(3):
self.data["gyro_speed"][i].append(gyro_speed[i])
if len(self.data["gyro_speed"][0]) >= 3:
gyro_speed_average = [sum(self.data["gyro_speed"][i])/3 for i in xrange(3)]
gyro = [gyro_speed_average[i] * frametime for i in xrange(3)]
theta = [self.turning_angle(self.data["prev_gyro_orient"][i], gyro[i]) for i in xrange(3)]
if self.option["gyro"]:
print "gyro", [visual.degrees(gyro_speed_average[i]) for i in xrange(3)]
if self.option["gyro_orient"]:
print "gyro_orient", [visual.degrees(theta[i]) for i in xrange(3)]
for i in cutoff: # graph
self.data["graph_gyro"][i] = int((gyro_speed_average[i] * self.param["height_"] / self.param["maxG"] + self.param["height_"]) / 2)
self.data["graph_gyro"][i] = max(0, min(self.param["height_"] - 1, self.data["graph_gyro"][i]))
self.data["graph_gyro_orient"][i] = int((theta[i] * self.param["height_"] / self.param["maxO"] + self.param["height_"]) / 2)
self.data["graph_gyro_orient"][i] = max(0, min(self.param["height_"] - 1, self.data["graph_gyro_orient"][i]))
self.visual["wiiobj"][1].rotate(angle=-gyro[0], axis=(1,0,0))
self.visual["wiiobj"][1].rotate(angle=-gyro[1], axis=(0,1,0))
self.visual["wiiobj"][1].rotate(angle=gyro[2], axis=(0,0,1))
self.data["prev_gyro_orient"] = theta
self.data["gyro_speed"] = [self.data["gyro_speed"][i][1:] for i in xrange(3)]
return (-gyro[0], -gyro[1], gyro[2])
return None
def _set_plane_vectors(self):
cx, cy, cz = self.center.getPosition()
x, y, z = self.motors[0].getPosition()
self.v1 = norm(vector(x-cx, y-cy, z-cz))
x, y, z = self.motors[2].getPosition()
self.v2 = norm(vector(x-cx, y-cy, z-cz))
# Calculate pitch and roll
self.pitch = 90 - degrees(self.v1.diff_angle(vector(0, 1, 0)))
self.roll = degrees(self.v2.diff_angle(vector(0, 1, 0))) - 90
# calculate yaw, (this works if the quad is close to a horizontal position)
if self.v1.z <= 0:
self.yaw = degrees(self.v1.diff_angle(vector(1, 0, 0)))
else:
self.yaw = 360 - degrees(self.v1.diff_angle(vector(1, 0, 0)))
VSS.range = RANGE_X
elif MODE == 'v': # demonstrate altering scene.fov
CAM_DIST = CAM_DIST + (MOUSE_CHANGE.x + MOUSE_CHANGE.y + MOUSE_CHANGE.z)*4
if CAM_DIST <= 0:
CAM_DIST = 0.001 # allow only positive
RAY = (MOUSE_LAB.pos - (CAM_DIST, 0, 0)).norm()
redraw_lines()
CAM_BOX.pos = (CAM_DIST, 0, 0)
CAM_LAB.pos = (CAM_DIST, 0, 0)
FWR_ARROW.pos = (CAM_DIST, 0, 0)
MOUSE_ARROW.pos = (CAM_DIST, 0, 0)
MOUSE_ARROW.axis = RAY*2
redraw_tri() # redraw the camera triangle
FOV = 2*vs.arctan(RANGE_X / CAM_DIST)
MODE_LAB.SetLabel('scene.fov:\n{0:9.0f} deg'.format(vs.degrees(FOV)))
if not Q_PY:
VSS.fov = FOV
elif MODE == 'm': # demonstrate moving the mouse.
HIT = VSS.mouse.project(CAM_FRAME.axis, CAM_FRAME.pos)
M_POS = CAM_FRAME.world_to_frame(HIT)
if abs(M_POS.z) <= CENT_PLANE.width/2.0 and \
abs(M_POS.y) <= CENT_PLANE.height/2.0: # not "off the screen"
RAY = (M_POS - (CAM_DIST, 0, 0)).norm()
redraw_line(MOUSE_LINE, vs.vector(CAM_DIST, 0, 0), LINELEN, RAY)
MOUSE_LAB.pos = M_POS
MOUSE_ARROW.axis = 2*RAY
RAYOUT = CAM_FRAME.frame_to_world(RAY) - CAM_FRAME.frame_to_world((0, 0, 0))
MODE_LAB.SetLabel('scene.mouse.ray:\n' + str2(RAYOUT))
def accel_gyro(self, raccel, rgyro, rfast, time):
if not self.param["acc_zero"] or not self.param["gyro_zero"]:
return
frametime = time - self.data["prev_time"]
acc, acc_theta = self.accel(raccel, frametime)
gyro_rotate = self.gyro(rgyro, rfast, frametime)
if not acc or not gyro_rotate:
return
else:
# saber's sound
if self.option["sound"] and self.param["saber"]:
if (4 > abs(acc[0]) >= 1.5) or (4 > abs(
acc[1]) >= 2.5) or (4 > abs(acc[2]) >= 2.5):
self.play_sound("swing1", True)
elif (abs(acc[0]) >= 4) or (abs(acc[1]) >= 3) or (abs(acc[2])
>= 3):
self.play_sound("strike1", False, stop=True)
# free fall detection
if self.option["fall"]:
if abs(acc[0]) * 10 < 1 and abs(acc[1]) * 10 < 1 and abs(
acc[2]) * 10 < 1 and not pygame.mixer.get_busy():
print "Fall!!"
self.param["fall"] = True
if self.option["sound"]:
self.play_sound("fall")
## calc average routine ##
# select correct angle of acc_orient
for i in xrange(3):
if acc_theta[i] <= 180:
if abs(
self.turning_angle(self.data["prev_average_orient"][i],
gyro_rotate[i]) - acc_theta[i]
) < abs(
self.turning_angle(self.data["prev_average_orient"][i],
gyro_rotate[i]) -
(180 - acc_theta[i])):
acc_theta[i] = 180 - acc_theta[i]
print self.turning_angle(
self.data["prev_average_orient"][i], gyro_rotate[i]),
print abs(
self.turning_angle(self.data["prev_average_orient"][i],
gyro_rotate[i]) - acc_theta[i]),
print self.turning_angle(
self.data["prev_average_orient"][i], gyro_rotate[i]),
print abs(
self.turning_angle(self.data["prev_average_orient"][i],
gyro_rotate[i]) -
(180 - acc_theta[i]))
else:
if abs(
self.turning_angle(self.data["prev_average_orient"][i],
gyro_rotate[i]) - acc_theta[i]
) < abs(
self.turning_angle(self.data["prev_average_orient"][i],
gyro_rotate[i]) -
(540 - acc_theta[i])):
acc_theta[i] = 540 - acc_theta[i]
print "adjust"
if acc[0] + acc[1] + acc[2] <= 3 / math.sqrt(2):
#加速度の和が3/sqrt(2)G以下のとき (10:0)
self.param["cur_pattern"] = 0
rotate = [
acc_theta[i] - self.data["prev_average_orient"][i]
for i in xrange(2)
]
elif (acc[0] < 0 < self.data["prev_acc_average"][0]
or acc[0] > 0 > self.data["prev_acc_average"][0]) or (
acc[1] < 0 < self.data["prev_acc_average"][1]
or acc[1] > 0 > self.data["prev_acc_average"][1]) or (
acc[2] < 0 < self.data["prev_acc_average"][2]
or acc[2] > 0 > self.data["prev_acc_average"][2]):
#加速度の符号が逆になったとき
self.param["cur_pattern"] = 1
rotate = [gyro_rotate[i] for i in xrange(2)]
elif gyro_rotate[0] < (math.pi / 5) and gyro_rotate[1] < (
math.pi / 5) and gyro_rotate[2] < (math.pi / 5):
#ジャイロから算出した角度がそれぞれ36度以下のとき (加速度に外力が加わっている→並進時はジャイロのデ-タのみ)
self.param["cur_pattern"] = 2
rotate = [gyro_rotate[i] for i in xrange(2)]
elif acc_theta[0] + acc_theta[1] <= (
math.pi / 2) and acc_theta[1] + acc_theta[2] <= (
math.pi / 2) and acc_theta[2] + acc_theta[0] <= (math.pi /
2):
#加速度から算出した二つの角を合せて90度以下のとき (重力加速度以外の加速度が入っていない→静止時は加速度のデ-タのみ)
self.param["cur_pattern"] = 3
rotate = [
acc_theta[i] - self.data["prev_average_orient"][i]
for i in xrange(2)
]
elif acc[1] < 2 and acc[1] < 2 and acc[2] < 2:
#加速度が各軸2G以下のとき (1:9)
self.param["cur_pattern"] = 4
rotate = [
((acc_theta[i] - self.data["prev_average_orient"][i]) * 1 +
gyro_rotate[i] * 9) / 10 for i in xrange(2)
]
else:
#加速度が各軸2G以上のとき (0:10)
self.param["cur_pattern"] = 5
rotate = [
((acc_theta[i] - self.data["prev_average_orient"][i]) * 0 +
gyro_rotate[i] * 10) / 10 for i in xrange(2)
]
rotate.append(gyro_rotate[2])
## end rotine ##
# 3D
self.visual["wiiobj"][2].rotate(angle=-rotate[0], axis=(1, 0, 0))
self.visual["wiiobj"][2].rotate(angle=rotate[1], axis=(0, 1, 0))
self.visual["wiiobj"][2].rotate(angle=rotate[2], axis=(0, 0, 1))
theta = [
self.turning_angle(self.data["prev_average_orient"][i], rotate[i])
for i in xrange(3)
]
# graph
for i in xrange(3):
self.data["graph_average_orient"][i] = int(
(theta[i] * self.param["height_"] / self.param["maxO"] +
self.param["height_"]) / 2)
self.data["graph_average_orient"][i] = max(
0,
min(self.param["height_"] - 1,
self.data["graph_average_orient"][i]))
if self.option["average_orient"]:
print "average_orient:", [
visual.degrees(theta[i]) for i in xrange(3)
]
self.data["prev_average_orient"] = theta
self.data["prev_time"] = time
return
def accel_gyro(self, raccel, rgyro, rfast, time):
if not self.param["acc_zero"] or not self.param["gyro_zero"]:
return
frametime = time - self.data["prev_time"]
acc, acc_theta = self.accel(raccel, frametime)
gyro_rotate = self.gyro(rgyro, rfast, frametime)
if not acc or not gyro_rotate:
return
else:
# saber's sound
if self.option["sound"] and self.param["saber"]:
if (4 > abs(acc_average[0]) >= 1.5) or (4 > abs(
acc_average[1]) >= 2.5) or (4 > abs(acc_average[2]) >=
2.5):
self.play_sound("swing1", True)
elif (abs(acc_average[0]) >= 4) or (abs(
acc_average[1]) >= 3) or (abs(acc_average[2]) >= 3):
self.play_sound("strike1", False, stop=True)
# free fall detection
if self.option["fall"]:
if abs(within[0]) * 10 < 1 and abs(within[1]) * 10 < 1 and abs(
within[2]) * 10 < 1 and not pygame.mixer.get_busy():
print "Fall!"
if self.option["sound"]:
self.play_sound("fall")
## calc average routine ##
if acc_theta[0] + acc_theta[1] <= (
math.pi / 2) and acc_theta[1] + acc_theta[2] <= (
math.pi / 2) and acc_theta[2] + acc_theta[0] <= (math.pi /
2):
#加速度から算出した二つの角を合せて90度以下のとき (重力加速度以外の加速度が入っていない→静止時は加速度のデ-タのみ)
self.param[
"pattern"] = "if acc_theta[i]+acc_theta[i+1] < (math.pi/2)"
rotate = [
acc_theta[i] - self.data["prev_average_orient"][i]
for i in xrange(2)
]
elif gyro_rotate[0] < (math.pi / 5) and gyro_rotate[1] < (
math.pi / 5) and gyro_rotate[2] < (math.pi / 5):
#ジャイロから算出した角度がそれぞれ36度以下のとき (加速度に外力が加わっている→並進時はジャイロのデ-タのみ)
self.param["pattern"] = "if gyro_rotate[i] < (math.pi/9)"
rotate = [gyro_rotate[i] for i in xrange(2)]
elif acc[0] <= 1 and acc[1] <= 1 and acc[2] <= 1:
#加速度が各軸1G以下のとき ()
self.param["pattern"] = "if acc[i] <= 1"
rotate = [
((acc_theta[i] - self.data["prev_average_orient"][i]) * 3 +
gyro_rotate[i] * 7) / 10 for i in xrange(2)
]
elif acc[1] < 2 and acc[1] < 2 and acc[2] < 2:
#加速度が各軸2G以下のとき ()
self.param["pattern"] = "if 1 < acc[i] < 2"
rotate = [
((acc_theta[i] - self.data["prev_average_orient"][i]) * 2 +
gyro_rotate[i] * 8) / 10 for i in xrange(2)
]
else:
#加速度が各軸2G以上のとき ()
self.param["pattern"] = "if acc[i] >= 2"
rotate = [
((acc_theta[i] - self.data["prev_average_orient"][i]) * 1 +
gyro_rotate[i] * 9) / 10 for i in xrange(2)
]
rotate.append(gyro_rotate[2])
## end rotine ##
# 3D
self.visual["wiiobj"][2].rotate(angle=-rotate[0], axis=(1, 0, 0))
self.visual["wiiobj"][2].rotate(angle=rotate[1], axis=(0, 1, 0))
self.visual["wiiobj"][2].rotate(angle=rotate[2], axis=(0, 0, 1))
theta = [
self.turning_angle(self.data["prev_average_orient"][i], rotate[i])
for i in xrange(3)
]
# graph
for i in xrange(3):
self.data["graph_average_orient"][i] = int(
(theta[i] * self.param["height_"] / self.param["maxO"] +
self.param["height_"]) / 2)
self.data["graph_average_orient"][i] = max(
0,
min(self.param["height_"] - 1,
self.data["graph_average_orient"][i]))
if self.option["average_orient"]:
print "average_orient:", [
visual.degrees(theta[i]) for i in xrange(3)
]
self.data["prev_average_orient"] = theta
self.data["prev_time"] = time
return
mode_lab.SetLabel( 'scene.range:\n' + format( range_x, "9.3"))
if not qPy: vss.range = range_x
elif mode == 'v': # demonstrate altering scene.fov
cam_dist = cam_dist + (mouse_change.x + mouse_change.y + mouse_change.z)*4
if cam_dist <= 0: cam_dist = 0.001 # allow only positive
ray = (mouse_lab.pos - (cam_dist, 0,0)).norm()
reDrawLines()
cam_box.pos = (cam_dist,0,0)
cam_lab.pos = (cam_dist,0,0)
fwd_arrow.pos = (cam_dist,0,0)
mouse_arrow.pos = (cam_dist,0,0)
mouse_arrow.axis = ray*2
reDrawTri() # redraw the camera triangle
fov = 2*vs.arctan( range_x / cam_dist)
mode_lab.SetLabel( 'scene.fov:\n{0:9.0f} deg'.format( vs.degrees(fov)))
if not qPy: vss.fov = fov
elif mode == 'm': # demonstrate moving the mouse.
hit = vss.mouse.project( cam_frame.axis, cam_frame.pos)
m_pos = cam_frame.world_to_frame(hit)
if abs(m_pos.z) <= cent_plane.width/2.0 and \
abs(m_pos.y) <= cent_plane.height/2.0 : # not "off the screen"
ray = (m_pos - (cam_dist,0,0)).norm()
reDrawLine( mouse_line, vs.vector(cam_dist,0,0), linelen, ray)
mouse_lab.pos = m_pos
mouse_arrow.axis = 2*ray
rayout = cam_frame.frame_to_world(ray) - cam_frame.frame_to_world((0,0,0))
mode_lab.SetLabel( 'scene.mouse.ray:\n' + str2( rayout))
if not qPy: vss.range = range_x
elif mode == 'v': # demonstrate altering scene.fov
cam_dist = cam_dist + (mouse_change.x + mouse_change.y +
mouse_change.z) * 4
if cam_dist <= 0: cam_dist = 0.001 # allow only positive
ray = (mouse_lab.pos - (cam_dist, 0, 0)).norm()
reDrawLines()
cam_box.pos = (cam_dist, 0, 0)
cam_lab.pos = (cam_dist, 0, 0)
fwd_arrow.pos = (cam_dist, 0, 0)
mouse_arrow.pos = (cam_dist, 0, 0)
mouse_arrow.axis = ray * 2
reDrawTri() # redraw the camera triangle
fov = 2 * vs.arctan(range_x / cam_dist)
mode_lab.SetLabel('scene.fov:\n{0:9.0f} deg'.format(vs.degrees(fov)))
if not qPy: vss.fov = fov
elif mode == 'm': # demonstrate moving the mouse.
hit = vss.mouse.project(cam_frame.axis, cam_frame.pos)
m_pos = cam_frame.world_to_frame(hit)
if abs(m_pos.z) <= cent_plane.width/2.0 and \
abs(m_pos.y) <= cent_plane.height/2.0 : # not "off the screen"
ray = (m_pos - (cam_dist, 0, 0)).norm()
reDrawLine(mouse_line, vs.vector(cam_dist, 0, 0), linelen, ray)
mouse_lab.pos = m_pos
mouse_arrow.axis = 2 * ray
rayout = cam_frame.frame_to_world(ray) - cam_frame.frame_to_world(
(0, 0, 0))
mode_lab.SetLabel('scene.mouse.ray:\n' + str2(rayout))
def accel_gyro(self, raccel, rgyro, rfast, time):
if not self.param["acc_zero"] or not self.param["gyro_zero"]:
return
frametime = time - self.data["prev_time"]
acc, acc_theta = self.accel(raccel, frametime)
gyro_rotate = self.gyro(rgyro, rfast, frametime)
if not acc or not gyro_rotate:
return
else:
# saber's sound
if self.option["sound"] and self.param["saber"]:
if (4 > abs(acc[0]) >= 1.5) or (4 > abs(acc[1]) >= 2.5) or (4 > abs(acc[2]) >= 2.5):
self.play_sound("swing1", True)
elif (abs(acc[0]) >= 4) or (abs(acc[1]) >= 3) or (abs(acc[2]) >= 3):
self.play_sound("strike1", False, stop=True)
# free fall detection
if self.option["fall"]:
if abs(acc[0]) * 10 < 1 and abs(acc[1]) * 10 < 1 and abs(acc[2]) * 10 < 1 and not pygame.mixer.get_busy():
print "Fall!!"
self.param["fall"] = True
if self.option["sound"]:
self.play_sound("fall")
## calc average routine ##
if acc[0] <= 0.5 and acc[1] <= 0.5 and acc[2] <= 0.5:
#加速度が各軸0.5G以下のとき (10:0)
self.param["cur_pattern"] = 0
rotate = [acc_theta[i] - self.data["prev_average_orient"][i] for i in xrange(2)]
elif (acc[0] < 0 < self.data["prev_acc_average"][0] or acc[0] > 0 > self.data["prev_acc_average"][0]) or (acc[1] < 0 < self.data["prev_acc_average"][1] or acc[1] > 0 > self.data["prev_acc_average"][1]) or (acc[2] < 0 < self.data["prev_acc_average"][2] or acc[2] > 0 > self.data["prev_acc_average"][2]):
#加速度の符号が逆になったとき
self.param["cur_pattern"] = 1
rotate = [gyro_rotate[i] for i in xrange(2)]
elif gyro_rotate[0] < (math.pi/5) and gyro_rotate[1] < (math.pi/5) and gyro_rotate[2] < (math.pi/5):
#ジャイロから算出した角度がそれぞれ36度以下のとき (加速度に外力が加わっている→並進時はジャイロのデ-タのみ)
self.param["cur_pattern"] = 2
rotate = [gyro_rotate[i] for i in xrange(2)]
#elif acc_theta[0]+acc_theta[1] <= (math.pi/2) and acc_theta[1]+acc_theta[2] <= (math.pi/2) and acc_theta[2]+acc_theta[0] <= (math.pi/2):
#加速度から算出した二つの角を合せて90度以下のとき (重力加速度以外の加速度が入っていない→静止時は加速度のデ-タのみ)
# self.param["cur_pattern"] = 3
# rotate = [acc_theta[i] - self.data["prev_average_orient"][i] for i in xrange(2)]
elif acc[1] < 2 and acc[1] < 2 and acc[2] < 2:
#加速度が各軸2G以下のとき (1:9)
self.param["cur_pattern"] = 4
rotate = [((acc_theta[i] - self.data["prev_average_orient"][i])*1 + gyro_rotate[i]*9) / 10 for i in xrange(2)]
else:
#加速度が各軸2G以上のとき (0:10)
self.param["cur_pattern"] = 5
rotate = [((acc_theta[i] - self.data["prev_average_orient"][i])*0 + gyro_rotate[i]*10) / 10 for i in xrange(2)]
rotate.append(gyro_rotate[2])
## end rotine ##
# 3D
self.visual["wiiobj"][2].rotate(angle=rotate[0], axis=(1,0,0))
self.visual["wiiobj"][2].rotate(angle=rotate[1], axis=(0,1,0))
self.visual["wiiobj"][2].rotate(angle=rotate[2], axis=(0,0,1))
theta = [self.turning_angle(self.data["prev_average_orient"][i], rotate[i]) for i in xrange(3)]
# graph
for i in xrange(3):
self.data["graph_average_orient"][i] = int((theta[i] * self.param["height_"] / self.param["maxO"] + self.param["height_"]) / 2)
self.data["graph_average_orient"][i] = max(0, min(self.param["height_"] - 1, self.data["graph_average_orient"][i]))
if self.option["average_orient"]:
print "average_orient:", [visual.degrees(theta[i]) for i in xrange(3)]
self.data["prev_average_orient"] = theta
self.data["prev_time"] = time
return
def accel_gyro(self, raccel, rgyro, rfast, time):
if not self.param["acc_zero"] or not self.param["gyro_zero"]:
return
frametime = time - self.data["prev_time"]
acc, acc_theta = self.accel(raccel, frametime)
gyro_rotate = self.gyro(rgyro, rfast, frametime)
if not acc or not gyro_rotate:
return
else:
# saber's sound
if self.option["sound"] and self.param["saber"]:
if (
(4 > abs(acc_average[0]) >= 1.5)
or (4 > abs(acc_average[1]) >= 2.5)
or (4 > abs(acc_average[2]) >= 2.5)
):
self.play_sound("swing1", True)
elif (abs(acc_average[0]) >= 4) or (abs(acc_average[1]) >= 3) or (abs(acc_average[2]) >= 3):
self.play_sound("strike1", False, stop=True)
# free fall detection
if self.option["fall"]:
if (
abs(within[0]) * 10 < 1
and abs(within[1]) * 10 < 1
and abs(within[2]) * 10 < 1
and not pygame.mixer.get_busy()
):
print "Fall!"
if self.option["sound"]:
self.play_sound("fall")
## calc average routine ##
if (
acc_theta[0] + acc_theta[1] <= (math.pi / 2)
and acc_theta[1] + acc_theta[2] <= (math.pi / 2)
and acc_theta[2] + acc_theta[0] <= (math.pi / 2)
):
# 加速度から算出した二つの角を合せて90度以下のとき (重力加速度以外の加速度が入っていない→静止時は加速度のデ-タのみ)
self.param["pattern"] = "if acc_theta[i]+acc_theta[i+1] < (math.pi/2)"
rotate = [acc_theta[i] - self.data["prev_average_orient"][i] for i in xrange(2)]
elif gyro_rotate[0] < (math.pi / 6) and gyro_rotate[1] < (math.pi / 4) and gyro_rotate[2] < (math.pi / 4):
# ジャイロから算出した角度がそれぞれ30度以下のとき (加速度に外力が加わっている→並進時はジャイロのデ-タのみ)
self.param["pattern"] = "if gyro_rotate[i] < (math.pi/4)"
rotate = [gyro_rotate[i] for i in xrange(2)]
elif acc[0] <= 1 and acc[1] <= 1 and acc[2] <= 1:
# 加速度が各軸1G以下のとき ()
self.param["pattern"] = "if acc[i] <= 1"
rotate = [
((acc_theta[i] - self.data["prev_average_orient"][i]) * 3 + gyro_rotate[i] * 7) / 10 for i in xrange(2)
]
elif acc[1] < 2 and acc[1] < 2 and acc[2] < 2:
# 加速度が各軸2G以下のとき ()
self.param["pattern"] = "if 1 < acc[i] < 2"
rotate = [
((acc_theta[i] - self.data["prev_average_orient"][i]) * 2 + gyro_rotate[i] * 8) / 10 for i in xrange(2)
]
else:
# 加速度が各軸2G以上のとき ()
self.param["pattern"] = "if acc[i] >= 2"
rotate = [
((acc_theta[i] - self.data["prev_average_orient"][i]) * 1 + gyro_rotate[i] * 9) / 10 for i in xrange(2)
]
rotate.append(gyro_rotate[2])
## end rotine ##
# 3D
if self.option["3D"] == "average":
self.visual["wiiobj"].rotate(angle=-rotate[0], axis=(1, 0, 0))
self.visual["wiiobj"].rotate(angle=rotate[1], axis=(0, 1, 0))
self.visual["wiiobj"].rotate(angle=rotate[2], axis=(0, 0, 1))
theta = [self.turning_angle(self.data["prev_average_orient"][i], rotate[i]) for i in xrange(3)]
# graph
for i in xrange(3):
self.data["graph_average_orient"][i] = int(
(theta[i] * self.param["height_"] / self.param["maxO"] + self.param["height_"]) / 2
)
self.data["graph_average_orient"][i] = max(
0, min(self.param["height_"] - 1, self.data["graph_average_orient"][i])
)
if self.option["average_orient"]:
print "average_orient:", [visual.degrees(theta[i]) for i in xrange(3)]
self.data["prev_average_orient"] = theta
self.data["prev_time"] = time
return
