'Search by hand
DIM A AS BYTE
DIMSensorData AS INTEGER
DIMSensorData2 AS INTEGER
DIM ObjDistance AS INTEGER
DIM ArmNo AS INTEGER
PTP SETON
PTP ALLON
'== motor diretion setting ======================
DIR G6A,1,0,0,1,0,0
DIR G6B,1,1,1,1,1,1
DIR G6C,0,0,0,0,0,0
DIR G6D,0,1,1,0,1,0
'== motor start position read ===================
GETMOTORSET G6A,1,1,1,1,1,0
GETMOTORSET G6B,1,1,1,0,0,0
GETMOTORSET G6C,1,1,1,0,0,0
GETMOTORSET G6D,1,1,1,1,1,0
SPEED 5
'== motor power on =============================
MOTOR G24
GOSUB standard_pose
'================================================
MAIN:
'A = REMOCON(1)
'IF A <2>=5 AND SensorData<30>=5 AND SensorData<30 THEN
FOR i=5 TO SensorData
GOSUB forward_walk
NEXT i
GOSUB Punch_It
ENDIF
ENDIF
GOSUB standard_pose
WAIT
'Program For the Sensor
GOTO MAIN
Punch_It:
'Right arm
HIGHSPEED SETON
MOVE G8B,100, 100, 100, 100, 101, 20, 92, 100
MOVE G8B,100, 100, 100, 100, 101, 35, 92, 100
MOVE G8B,100, 100, 100, 100, 101, 53, 92, 100
MOVE G8B,100, 100, 100, 100, 101, 74, 92, 100
MOVE G8B,100, 100, 100, 100, 101, 98, 92, 100
HIGHSPEED SETOFF
RETURN
Detect_Object3: 'for the left arm infront
SensorData=0
SensorData=AD(1)
IF SensorData<4 THEN SensorData=4
SensorData=3000/SensorData
RETURN
ReadyForSearchInfrontByHand:
MOVE G8B, 80, 100, 100, 100, 122, 21, 93, 100
MOVE G8B, 80, 100, 100, 100, 159, 17, 93, 100
MOVE G8B, 80, 100, 100, 100, 162, 17, 67, 100
MOVE G8B, 80, 100, 100, 100, 180, 17, 43, 100
MOVE G8B, 80, 100, 100, 100, 184, 17, 12, 100
WAIT
RETURN
SearchDone:
MOVE G8B, 80, 100, 100, 100, 100, 100, 100, 100
MOVE G8B, 80, 100, 100, 100, 182, 107, 77, 100
MOVE G8B, 80, 100, 100, 100, 184, 22, 77, 100
WAIT
RETURN
SearchDone2: 'for the side'because the robot throw away the object with his hand.
MOVE G8B, 78, 100, 100, 100, 103, 42, 69, 100
Detect_Object4: 'for the left arm left Side
SensorData=0
SensorData=AD(1)
IF SensorData<4 THEN SensorData=4
SensorData=3000/SensorData
RETURN
SearchBySide:
MOVE G8B, 80, 100, 100, 100, 101, 76, 34, 100
WAIT
RETURN
'================================================
forward_walk:
SPEED 5
MOVE24 85, 71, 152, 91, 112, 60, 100, 40, 80, , , , 100, 40, 80, , , , 112, 76, 145, 93, 92, 60,
SPEED 14
'left up
MOVE24 90, 107, 105, 105, 114, 60, 90, 40, 80, , , , 100, 40, 80, , , , 114, 76, 145, 93, 90, 60,
'---------------------------------------
'left down
MOVE24 90, 56, 143, 122, 114, 60, 80, 40, 80, , , , 105, 40, 80, , , , 113, 80, 145, 90, 90, 60,
MOVE24 90, 46, 163, 112, 114, 60, 80, 40, 80, , , , 105, 40, 80, , , , 112, 80, 145, 90, 90, 60,
SPEED 10
'left center
MOVE24 100, 66, 141, 113, 100, 100, 90, 40, 80, , , , 100, 40, 80, , , , 100, 83, 156, 80, 100, 100,
MOVE24 113, 78, 142, 105, 90, 60, 100, 40, 80, , , , 100, 40, 80, , , , 90, 102, 136, 85, 114, 60,
SPEED 14
'right up
MOVE24 113, 76, 145, 93, 90, 60, 100, 40, 80, , , , 90, 40, 80, , , , 90, 107, 105, 105, 114, 60,
'right down
MOVE24 113, 80, 145, 90, 90, 60, 105, 40, 80, , , , 80, 40, 80, , , , 90, 56, 143, 122, 114, 60,
MOVE24 112, 80, 145, 90, 90, 60, 105, 40, 80, , , , 80, 40, 80, , , , 90, 46, 163, 112, 114, 60,
SPEED 10
'right center
MOVE24 100, 83, 156, 80, 100, 100, 100, 40, 80, , , , 90, 40, 80, , , , 100, 66, 141, 113, 100, 100,
MOVE24 90, 102, 136, 85, 114, 60, 100, 40, 80, , , , 100, 40, 80, , , , 113, 78, 142, 105, 90, 60,
SPEED 14
'left up
MOVE24 90, 107, 105, 105, 114, 60, 90, 40, 80, , , , 100, 40, 80, , , , 113, 76, 145, 93, 90, 60,
'---------------------------------------
SPEED 5
MOVE24 85, 71, 152, 91, 112, 60, 100, 40, 80, , , , 100, 40, 80, , , , 112, 76, 145, 93, 92, 60,
RETURN
'================================================
right_turn:
SPEED 6
MOVE G6A, 85, 71, 152, 91, 112, 60
MOVE G6D, 112, 76, 145, 93, 92, 60
MOVE G6B, 100, 40, 80, , , ,
MOVE G6C, 100, 40, 80, , , ,
WAIT
SPEED 9
MOVE G6D, 113, 75, 145, 97, 93, 60
MOVE G6A, 90, 50, 157, 115, 112, 60
MOVE G6C, 105, 40, 70, , , ,
MOVE G6B, 90, 40, 70, , , ,
WAIT
MOVE G6D, 108, 78, 145, 110, 93, 60
MOVE G6A, 95, 43, 169, 110, 110, 60
MOVE G6C, 105, 40, 70, , , ,
MOVE G6B, 80, 40, 70, , , ,
WAIT
RETURN
'================================================
MAIN_RIGHT:
GOSUB right_turn
GOSUB standard_pose
DELAY 1000
GOSUB right_turn
GOSUB standard_pose
DELAY 1000
GOSUB right_turn
GOSUB standard_pose
DELAY 1000
GOSUB right_turn
GOSUB standard_pose
DELAY 1000
GOSUB right_turn
GOSUB standard_pose
DELAY 1000
'================================================
standard_pose:
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
'================================================
Please correct me in anything that is wrong, thank you so much!!!!!!!!!!!Statistics: Posted by miaja1 — Mon Jan 16, 2012 2:03 pm
]]>