How can I do this?

[miaja1]

Hello!!! I have a question, I'm doing a project about robonova, i want to know how I can calculate the distance or degree if I have to turn right the robot to the object and then punch it. Here goes my template:

'Search by hand

DIM A AS BYTE
DIM SensorData AS INTEGER
DIM SensorData2 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!!!!!!!!!!!