Exemple #1
0
 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
Exemple #2
0
 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)))
Exemple #3
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
Exemple #6
0
        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))
            
            
Exemple #7
0
        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