Clase 15 Dic 2012

Control

En una matriz de 3x3 se colocan objetos en diferentes puntos, el brazo robotico debe realizar movimientos para leer la posición de los objetos dentro de la matriz.

Desarrollo: 

1-. Para soltar los servos y decidir la posición de lectura

inicio:
SERVO 0 = @
SERVO 1 = @

LET A= $SERVO(0)
LET B= $SERVO(1)
PRINT "A: ", A, "; B: ", B

LET K=$KIR
PRINT "K: ", K
IF K<0 THEN
  GOTO inicio
ENDIF

2-. Realizar el movimiento del servo en las medidas, dividida en tres estaciones.

MOVE @{2,15,128},10,1000
WAIT 50
MOVE @{2,48,128},10,1000
WAIT 50
MOVE @{2,76,128},10,1000
WAIT 50

3-. Leer en cada una de las estaciones antes definidas.

inicio:

MOVE @{2,15,128},10,1000
WAIT 50
LET L=$PSD
PRINT "a: ", L

MOVE @{2,48,128},10,1000
WAIT 50
LET M=$PSD
PRINT "b: ", M

MOVE @{2,76,128},10,1000
WAIT 50
LET N=$PSD
PRINT "c: ", N

LET K=$KIR
IF K<0 THEN
  GOTO inicio
ENDIF

4-.Recorre, Lee y Define el cuadrante de la matriz

inicio:

'primer movimiento
MOVE @{2,15,128},10,1000
WAIT 20
LET L=$PSD
PRINT "a: ", L


'segundo movimiento
MOVE @{2,48,128},10,1000
WAIT 20
LET M=$PSD
PRINT "b: ", M


'tercer movimiento
MOVE @{2,76,128},10,1000
WAIT 20
LET N=$PSD
PRINT "c: ", N

LET A = 0
LET B = 0
LET C = 0
LET D = 0
LET E = 0
LET F = 0
LET G = 0
LET H = 0
LET I = 0

PRINT "| ",A," | ",B," | ",C," |"
PRINT "| ",D," | ",E," | ",F," |"
PRINT "| ",G," | ",H," | ",I," |"

IF L<=30 & L>25 THEN
A=1
ENDIF

IF L<=25 & L>20 THEN
D=1
ENDIF

IF L<=20 & L>=16 THEN
G=1
ENDIF

IF M<=30 & M>25 THEN
B=1
ENDIF
IF M<=25 & M>20 THEN
E=1
ENDIF
IF M<=20 & M>=16 THEN
H=1
ENDIF

IF N<=30 & N>25 THEN
C=1
ENDIF
IF N<=25 & N>20 THEN
F=1
ENDIF
IF N<=20 & N>=16 THEN
I=1
ENDIF

PRINT "------------"
PRINT "| ",A," | ",B," | ",C," |"
PRINT "| ",D," | ",E," | ",F," |"
PRINT "| ",G," | ",H," | ",I," |"