robotpark.co.kr robotpark.co.kr 로봇파크 쇼핑몰( ROBOTPARK.NET )

컨텐츠 바로가기


board


현재 위치

  1. 게시판
  2. 로봇쌤 교실

로봇쌤 교실

로봇쌤 교실입니다.

메탈파이터 예제-25 횟수만큼보행
제목 메탈파이터 예제-25 횟수만큼보행
작성자 관리자 (ip:)
  • 작성일 2013-04-28
  • 추천 추천 하기
  • 조회수 357
  • 평점 0점

'****************************************
'***** 메탈파이터 기초 프로그램 *******
'****************************************

'******* 변수선언 ***********************
DIM A AS BYTE
DIM A_old AS BYTE
DIM 보행횟수 AS BYTE
DIM 반복횟수 AS BYTE
DIM 보행순서 AS BYTE
'**** 기울기센서포트 설정

CONST 앞뒤기울기AD포트 = 2
CONST 좌우기울기AD포트 = 3

CONST MIN = 105  '앞으로넘어졌을때
CONST MAX = 145  '뒤로넘어졌을때

'*******모터방향설정*********************
DIR G6A,1,0,0,1,0,0 '왼쪽다리:모터0~5번
DIR G6D,0,1,1,0,1,0 '오른쪽다리:모터18~23번
DIR G6B,1,1,1,1,1,1 '왼쪽팔:모터6~11번
DIR G6C,0,0,0,0,0,0 '오른쪽팔:모터12~17번


'*******모터동시제어설정****************
PTP SETON   '단위그룹별 점대점동작 설정
PTP ALLON  '전체모터 점대점 동작 설정

'*******모터위치값피드백****************
GOSUB MOTOR_GET

'*******모터사용설정********************
GOSUB MOTOR_ON

'*******피에조소리내기******************
TEMPO 220
MUSIC "O23EAB7EA>3#C"
'***** 초기자세로 **********************
SPEED 5
GOSUB 기본자세

보행순서 = 0

'***** 메인 반복루틴 **************
MAIN:

    ERX 4800,A,MAIN
    A_old = A

    IF A = 1 THEN
        보행횟수 = 3
        GOTO 전진횟수보행50
    ELSE
        MUSIC "F" 
    ENDIF


    GOTO MAIN
    '*************************************

전진횟수보행50:
    반복횟수 = 0
    GOSUB Leg_motor_mode3
   
    IF 보행순서 = 0 THEN
        보행순서 = 1
        SPEED 3
        '오른쪽기울기
        MOVE G6A, 88,  71, 152,  91, 110
        MOVE G6D,108,  76, 146,  93,  94
        MOVE G6B,100,35
        MOVE G6C,100,35
        WAIT

        SPEED 10'보행속도
        '왼발들기
        MOVE G6A, 90, 100, 115, 105, 114
        MOVE G6D,113,  78, 146,  93,  94
        MOVE G6B,90
        MOVE G6C,110
        WAIT

        GOTO 전진횟수보행50_1 
    ELSE
        보행순서 = 0

        SPEED 3
        '왼쪽기울기
        MOVE G6D,  88,  71, 152,  91, 110
        MOVE G6A, 108,  76, 146,  93,  94
        MOVE G6C, 100,35
        MOVE G6B, 100,35
        WAIT

        SPEED 10'보행속도
        '오른발들기
        MOVE G6D, 90, 100, 115, 105, 114
        MOVE G6A,113,  78, 146,  93,  94
        MOVE G6C,90
        MOVE G6B,110
        WAIT

        GOTO 전진횟수보행50_2 

    ENDIF

 

전진횟수보행50_1:
    반복횟수 = 반복횟수 + 1
    SPEED 10
    '왼발뻣어착지
    MOVE G6A, 85,  44, 163, 113, 114
    MOVE G6D,110,  77, 146,  93,  94
    WAIT

    SPEED 4
    '왼발중심이동
    MOVE G6A,110,  76, 144, 100,  93
    MOVE G6D,85, 93, 155,  71, 112
    WAIT

    SPEED 10
    '오른발들기10
    MOVE G6A,111,  77, 146,  93, 94
    MOVE G6D,90, 100, 105, 110, 114
    MOVE G6B,110
    MOVE G6C,90
    WAIT

 

    IF 반복횟수 >= 보행횟수 THEN
        HIGHSPEED SETOFF
        SPEED 5
        '왼쪽기울기2
        MOVE G6A, 106,  76, 146,  93,  96  
        MOVE G6D,  88,  71, 152,  91, 106
        MOVE G6B, 100,35
        MOVE G6C, 100,35
        WAIT 

        SPEED 3
        GOSUB 기본자세
        GOSUB Leg_motor_mode1

        GOTO MAIN
    ENDIF
    '**********


전진횟수보행50_2:
    반복횟수 = 반복횟수 + 1
    SPEED 10
    '오른발뻣어착지
    MOVE G6D,85,  44, 163, 113, 114
    MOVE G6A,110,  77, 146,  93,  94
    WAIT

    SPEED 4
    '오른발중심이동
    MOVE G6D,110,  76, 144, 100,  93
    MOVE G6A, 85, 93, 155,  71, 112
    WAIT

    SPEED 10
    '왼발들기10
    MOVE G6A, 90, 100, 105, 110, 114
    MOVE G6D,111,  77, 146,  93,  94
    MOVE G6B, 90
    MOVE G6C,110
    WAIT

    IF 반복횟수 >= 보행횟수 THEN
        HIGHSPEED SETOFF
        SPEED 5
        '오른쪽기울기2
        MOVE G6D, 106,  76, 146,  93,  96  
        MOVE G6A,  88,  71, 152,  91, 106
        MOVE G6C, 100,35
        MOVE G6B, 100,35
        WAIT

        SPEED 3
        GOSUB 기본자세
        GOSUB Leg_motor_mode1
        GOTO MAIN
    ENDIF


    GOTO 전진횟수보행50_1
    '************************************************
    '******************************************
    '***************************************
MOTOR_ON: '전포트서보모터사용설정
    MOTOR G24
    RETURN

    '***********************************
MOTOR_OFF: '전포트서보모터설정해제
    MOTOROFF G6B
    MOTOROFF G6C
    MOTOROFF G6A
    MOTOROFF G6D
    RETURN
    '***********************************
MOTOR_GET: '위치값피드백
    GETMOTORSET G6A,1,1,1,1,1,0
    GETMOTORSET G6B,1,1,1,0,0,1
    GETMOTORSET G6C,1,1,1,0,0,0
    GETMOTORSET G6D,1,1,1,1,1,0
    RETURN


    '*******기본자세관련*****************

기본자세:
    MOVE G6A,100,  76, 145,  93, 100, 100
    MOVE G6D,100,  76, 145,  93, 100, 100
    MOVE G6B,100,  30,  80, 100, 100, 100
    MOVE G6C,100,  30,  80, 100, 100, 100
    WAIT
    RETURN
    '************************************* 
차렷자세:
    MOVE G6A,100, 56, 182, 76, 100, 100
    MOVE G6D,100, 56, 182, 76, 100, 100
    MOVE G6B,100, 20, 90, 100, 100, 100
    MOVE G6C,100, 20, 90, 100, 100, 100
    WAIT
    RETURN
    '**************************************
앉은자세:

    MOVE G6A,100, 143,  28, 142, 100, 100
    MOVE G6D,100, 143,  28, 142, 100, 100
    MOVE G6B,100,  30,  80
    MOVE G6C,100,  30,  80
    WAIT
    RETURN
    '***************************************

    '************************************************
All_motor_Reset:

    MOTORMODE G6A,1,1,1,1,1
    MOTORMODE G6D,1,1,1,1,1
    MOTORMODE G6B,1,1,1, , ,1
    MOTORMODE G6C,1,1,1

    RETURN
    '************************************************
All_motor_mode2:

    MOTORMODE G6A,2,2,2,2,2
    MOTORMODE G6D,2,2,2,2,2
    MOTORMODE G6B,2,2,2, , ,2
    MOTORMODE G6C,2,2,2

    RETURN
    '************************************************
All_motor_mode3:

    MOTORMODE G6A,3,3,3,3,3
    MOTORMODE G6D,3,3,3,3,3
    MOTORMODE G6B,3,3,3, , ,3
    MOTORMODE G6C,3,3,3

    RETURN
    '************************************************
Leg_motor_mode1:
    MOTORMODE G6A,1,1,1,1,1
    MOTORMODE G6D,1,1,1,1,1
    RETURN
    '************************************************
Leg_motor_mode2:
    MOTORMODE G6A,2,2,2,2,2
    MOTORMODE G6D,2,2,2,2,2
    RETURN

    '************************************************
Leg_motor_mode3:
    MOTORMODE G6A,3,3,3,3,3
    MOTORMODE G6D,3,3,3,3,3
    RETURN
    '************************************************
Leg_motor_mode4:
    MOTORMODE G6A,3,2,2,1,3
    MOTORMODE G6D,3,2,2,1,3
    RETURN
    '************************************************
Leg_motor_mode5:
    MOTORMODE G6A,3,2,2,1,2
    MOTORMODE G6D,3,2,2,1,2
    RETURN
    '************************************************
    '************************************************
Arm_motor_mode1:
    MOTORMODE G6B,1,1,1
    MOTORMODE G6C,1,1,1
    RETURN
    '************************************************
Arm_motor_mode2:
    MOTORMODE G6B,2,2,2
    MOTORMODE G6C,2,2,2
    RETURN
    '************************************************
Arm_motor_mode3:
    MOTORMODE G6B,3,3,3
    MOTORMODE G6C,3,3,3
    RETURN

    '************************************************

첨부파일 19[1].MF횟수만큼보행.bas
비밀번호 삭제하려면 비밀번호를 입력하세요.

목록

삭제 수정 답변

댓글 수정

비밀번호

수정 취소

/ byte