'****************************************
'***** 메탈파이터 기초 프로그램 *******
'****************************************
'******* 변수선언 ***********************
DIM A AS BYTE
DIM A_old AS BYTE
DIM X AS BYTE
DIM Y 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 기본자세
보행순서 = 1
'***** 메인 반복루틴 **************
MAIN:
ERX 4800,A,MAIN
A_old = A
IF A = 1 THEN
GOTO 전진달리기50
ELSE
MUSIC "F"
ENDIF
GOTO MAIN
'************************************************
'******************************************
전진달리기50:
SPEED 12
HIGHSPEED SETON
GOSUB Leg_motor_mode4
IF 보행순서 = 0 THEN
보행순서 = 1
MOVE G6A,95, 76, 145, 93, 101
MOVE G6D,101, 78, 145, 93, 98
WAIT
GOTO 전진달리기50_1
ELSE
보행순서 = 0
MOVE G6D,95, 76, 145, 93, 101
MOVE G6A,101, 78, 145, 93, 98
WAIT
GOTO 전진달리기50_4
ENDIF
'**********************
전진달리기50_1:
MOVE G6A,95, 95, 100, 120, 104
MOVE G6D,104, 78, 146, 91, 102
MOVE G6B, 80
MOVE G6C,120
WAIT
전진달리기50_2:
MOVE G6A,95, 75, 122, 120, 104
MOVE G6D,104, 80, 146, 89, 100
WAIT
전진달리기50_3:
MOVE G6A,103, 70, 145, 103, 100
MOVE G6D, 95, 88, 160, 68, 102
WAIT
ERX 4800,A, 전진달리기50_4
IF A <> A_old THEN GOTO 전진달리기50_멈춤
'*********************************
전진달리기50_4:
MOVE G6D,95, 95, 100, 120, 104
MOVE G6A,104, 78, 146, 91, 102
MOVE G6C, 80
MOVE G6B,120
WAIT
전진달리기50_5:
MOVE G6D,95, 75, 122, 120, 104
MOVE G6A,104, 80, 146, 89, 100
WAIT
전진달리기50_6:
MOVE G6D,103, 70, 145, 103, 100
MOVE G6A, 95, 88, 160, 68, 102
WAIT
ERX 4800,A, 전진달리기50_1
IF A <> A_old THEN GOTO 전진달리기50_멈춤
GOTO 전진달리기50_1
'*********************************
전진달리기50_멈춤:
HIGHSPEED SETOFF
SPEED 15
GOSUB 안정화자세
SPEED 5
GOSUB 기본자세
DELAY 400
GOSUB Leg_motor_mode1
GOTO RX_EXIT
'******************************************
안정화자세:
MOVE G6A,98, 76, 145, 93, 101, 100
MOVE G6D,98, 76, 145, 93, 101, 100
MOVE G6B,100, 30, 80, 100, 100, 100
MOVE G6C,100, 30, 80, 100, 100, 100
WAIT
RETURN
'************************************************
'************************************************
'******************************************
'***************************************
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
'************************************************
RX_EXIT: '수신값을 버리는루틴
ERX 4800, A, MAIN
GOTO RX_EXIT
'************************************************