Codigo de Robot2

7/26/2019 Codigo de Robot2

1/21

//##################################################################################################################################

// #

LIBRERIAS#

//##################################################################################################################################

#include // Funciones para gestin de !e!oria din"!ica controlde procesos $ otras

#include // Funciones prede&nidas para traba'ar con ser%os

//##################################################################################################################################

// #(E)LARA)I*+ES

#

//

##################################################################################################################################

//#de&ne )ALIBRA)I*+ // ,ara calibrar el robot

// L*+-I( (E L*S BRA0*S (EL R*B* 1en !ili!etros2

#de&ne L3 43.5

#de&ne L5 67

#de&ne L4 38

// )*+E9I*+ES (E L*S SER:*S A L*S ,I+ES

#de&ne SER:*ELE:A(*R,I+ 8 // Ser%o de Ele%acion ;> ,in 8 del Arduino1cable +ARA+A del ser%o2


2/21

#de&ne SER:*I0=IER(*,I+ 6 // Ser%o I?uierdo ;> ,in 6 del Arduino1cable +ARA+A del ser%o2

#de&ne SER:*(ERE)@*,I+ // Ser%o (erecho ;> ,in del Arduino 1cable+ARA+A del ser%o2

// :AL*RES (E L*S SER:*S 1sados $ opti!iados en la ase de )ALIBRA)I*+2

#de&ne AC,LI(SER:*I0= 4D87

#de&ne AC,LI(SER:*(ER D77

#de&ne RA+-*SER:*S 677

// ,*SI)I*+ES (EL SER:* ELE:A(*R

#de&ne A)I:ARELE:A(*R D7 // )a!bio de letras#de&ne (ESA)I:ARELE:A(*R 477 // Escribiendo

#de&ne :EL*)I(A(ELE:A(*R 4877 // :elocidad con la ?ue le%anta!os oba'a!os

// ,+*S *RI-E+ SER:*S 1I/(2

#de&ne *49 55

#de&ne *4G ;58

#de&ne *59

#de&ne *5G ;58

%olatile Hoat coord9 3J // Li!ite !ini!o donde e!pea!os a escribir en ele'e 9

%olatile double ulti!aG .8J // Li!ite !aKi!o en el e'e G

%olatile double ulti!a9 8J // Li!ite !aKi!o en el e'e 9

Ser%o ser%oEle%adorJ // )rea un ob'eto ser%o para controlar el ser%o ELE:A(*R

Ser%o ser%oI?uierdoJ // )rea un ob'eto ser%o para controlar el ser%oI0=IER(*

Ser%o ser%o(erechoJ // )rea un ob'eto ser%o para controlar el ser%o (ERE)@*


3/21

String palabraJ // ,alabra ?ue el robot escribira

Hoat escala 7.8J // Escala de la uente con la ?ue escribe el robot

int posEle%ador 4877J // ,osicion inicial del ser%o ele%ador

//##################################################################################################################################

// #F+)I*+ES

#

//###############################################

###################################################################################

%oid setup12M

ser%oEle%ador.attach1SER:*ELE:A(*R,I+2J // Asocia el ser%o al pin 8

ser%oI?uierdo.attach1SER:*I0=IER(*,I+2J // Asocia el ser%o al pin 6

ser%o(erecho.attach1SER:*(ERE)@*,I+2J // Asocia el ser%o al pin

irNa18.5 2J

!o%erEle%ador142J

dela$147772J // Espera!os un poco

O

%oid loop12 M

i 1ser%oEle%ador.attached12 PP ser%oI?uierdo.attached12 PPser%o(erecho.attached122 M // )o!proba!os ?ue los SER:*S esten asociados alos ,I+ES

#ide )ALIBRA)I*+ calibrar12J // LLa!ada a la uncion de calibracion

#else

escribir,alabra12J // Lla!ada a la uncion ?ue escribe la palabra

#endi


4/21

O else M

print1QnERR*R Al !enos un ser%o!otor no esta conectado. Qn2J

O

// (esacti%a!os los ser%os

ser%oEle%ador.detach12J

ser%oI?uierdo.detach12J

ser%o(erecho.detach12J

O

%oid escribir,alabra12 M

int posicion)aracterJ // ,osicion ?ue ocupa el caracter dentro de la

palabra

int coordGJ // :aria dependiendo de la posicion de la letra debido a ?ue lasulti!as tienen una des%iacion ?ue ha$ ?ue corregir !ediante sotare

or 1posicion)aracter 7 J posicion)aracter < palabra.length12 Jposicion)aracterTT2 M

i1posicion)aracter


5/21

irNa1;3 5.52J

dela$18772J

irNa1.4 5D2J

dela$18772J

O

%oid escribir)aracter1Hoat coord9 Hoat coordG char letra Hoat escala2 M

// El e'e en el ?ue dibu'a!os cada letra ?uedaria asi eKpresadogra&ca!ente

/W

EE G

CA9 57 X

X

X

X

X

48 X

X

X

X

X

47 X

X

X

X

X

8 X

X


6/21

X

X

X

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; EE 9

1772 5 6 45 CA9

W/

// Las letras se e!piean en el 5 por?ue sino al dibu'arlas se solapararian

sitch 1letra2 M

case YAY

irNa1coord9 T 5 W escala coordG T 7 W escala2J

!o%erEle%ador172J

irNa1coord9 T W escala coordG T 57 W escala2J


!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case YBY


!o%erEle%ador172J






!o%erEle%ador142J


7/21

breaZJ

case Y)Y


!o%erEle%ador172J






!o%erEle%ador142J

breaZJ

case Y(Y


!o%erEle%ador172J


!o%erEle%ador142J

!o%erEle%ador172J


irNa1coord9 T 47 W escala coordG T D W escala2J


!o%erEle%ador142J

breaZJ

case YEY


!o%erEle%ador172J


!o%erEle%ador142J


8/21

!o%erEle%ador172J


!o%erEle%ador142J

!o%erEle%ador172J


!o%erEle%ador142J


!o%erEle%ador172J

irNa1coord9 T D W escala coordG T 47 W escala2J

!o%erEle%ador142J

breaZJ

case YFY


!o%erEle%ador172J


!o%erEle%ador142J

!o%erEle%ador172J


!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case Y-Y


!o%erEle%ador172J



9/21





irNa1coord9 T W escala coordG T D W escala2J

!o%erEle%ador142J

breaZJ

case Y@Y


!o%erEle%ador172J


!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case YIY


!o%erEle%ador172J


!o%erEle%ador142J


!o%erEle%ador172J


10/21


!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case YY


!o%erEle%ador172J




!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case Y[Y

irNa1coord9 T 47T escala coordG T 4D W escala2J

!o%erEle%ador172J



!o%erEle%ador142J


!o%erEle%ador172J



11/21

!o%erEle%ador142J

breaZJ

case YLY


!o%erEle%ador172J


!o%erEle%ador142J

!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case YCY


!o%erEle%ador172J



!o%erEle%ador142J

!o%erEle%ador172J



!o%erEle%ador142J

breaZJ

case Y+Y


!o%erEle%ador172J


!o%erEle%ador142J


12/21

!o%erEle%ador172J


!o%erEle%ador142J

!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case Y*Y


!o%erEle%ador172J









!o%erEle%ador142J

breaZJ

case Y,Y


!o%erEle%ador172J


!o%erEle%ador142J

!o%erEle%ador172J




13/21


!o%erEle%ador142J

breaZJ

case Y=Y


!o%erEle%ador172J




irNa1coord9 T 45 W escala coordG T 4D W escala2J



irNa1coord9 T 5 W escala coordG T 4D W escala2J


!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case YRY


!o%erEle%ador172J


!o%erEle%ador142J

!o%erEle%ador172J




14/21


!o%erEle%ador142J

!o%erEle%ador172J

irNa1coord9 T 45Wescala coordG T 7Wescala2J

!o%erEle%ador142J

breaZJ

case YSY


!o%erEle%ador172J






!o%erEle%ador142J

breaZJ

case YY


!o%erEle%ador172J


!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case YY


15/21


!o%erEle%ador172J






!o%erEle%ador142J

breaZJ

case Y:Y


!o%erEle%ador172J


!o%erEle%ador142J

!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case Y\Y


!o%erEle%ador172J



!o%erEle%ador142J

!o%erEle%ador172J



!o%erEle%ador142J


16/21

breaZJ

case Y9Y


!o%erEle%ador172J


!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case YGY


!o%erEle%ador172J


!o%erEle%ador142J

!o%erEle%ador172J


!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

case Y0Y


!o%erEle%ador172J


17/21




!o%erEle%ador142J


!o%erEle%ador172J


!o%erEle%ador142J

breaZJ

O

O

%oid !o%erEle%ador1int senial2 M

// E!plea!os las siguientes unciones de la libreria Ser%o.h

// riteCicroseconds1int2 ;;> Canda un pulso de un ancho de tantos!icrosegundos co!o le especi&?ue!os en el par]ntesis de esta or!a elser%o se colocar" en un deter!inado punto. 4877;)entro 4777;I? $ 5777;(er

// dela$Cicroseconds1int2 ;;> @ace una pausa en el progra!a con la cantidadde tie!po pasada en !icrosegundos

sitch 1senial2 M

case 7 // Escribir

i 1posEle%ador > (ESA)I:ARELE:A(*R2 M

hile 1posEle%ador > (ESA)I:ARELE:A(*R2 M

posEle%ador;;J

ser%oEle%ador.riteCicroseconds1posEle%ador2J

dela$Cicroseconds1:EL*)I(A(ELE:A(*R2J

O

O else M

hile 1posEle%ador


18/21

posEle%adorTTJ



O

O

breaZJ

case 4 // Co%i!iento entre letras

i 1posEle%ador > A)I:ARELE:A(*R2 M

hile 1posEle%ador > A)I:ARELE:A(*R2 M

posEle%ador;;J



O

O else M

hile 1posEle%ador


19/21

i 1r < 42

r 4J

int iJ

or 1i 7J i


20/21

sa!os la or!ula !ate!atica de la longitud de arco o recti&cacin deuna cur%a ?ue es la !edida de la distancia a lo largo de una cur%a odi!ensin lineal.

BRE:E E*RIA

Suponiendo ?ue se tiene una cur%a recti&cable cual?uieradeter!inada por una uncin $ suponiendo ?ue se ?uiere aproKi!ar lalongitud del arco de cur%a s ?ue %a desde un punto a a uno b.

)on la or!ula de la longitud del arco es posible dise^ar unaserie de tri"ngulos rect"ngulos cu$as hipotenusas concatenadas cubran elarco de cur%a elegido.

,ara calcular un pe?ue^o seg!ento de la cur%a pode!osaproKi!ar la or!ula !encionada ba'o condiciones especiales con la or!uladel eore!a de ,itagoras ?ue en nuestro codigo seria s rai)uadrada1distancia9_5 T distanciaG_52

W/

// ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;> ,ARA EL BRA0* I0=IER(* Candaun pulso de un ancho de tantos !icrosegundos co!o le especi&?ue!os en elpar]ntesis de esta or!a el ser%o se colocar" en un deter!inado punto.

4877;)entro 4777;I? $ 5777;(er

// ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;> ,ARA EL BRA0* (ERE)@*


21/21

h9 K T L3 W cos11angulo4 ; angulo5 T 7.6542 T CN,I2J // 368` ;>7.654

hG $ T L3 W sin11angulo4 ; angulo5 T 7.6542 T CN,I2J

distancia9 h9 ; *59J

distanciaG hG ; *5GJ

// ,ara calcular el triangulo rectangulo !encionado en la teoriacalcula!os el triangulo entre el ser%o(er el brao $ el rotulador

s s?rt1distancia9 W distancia9 T distanciaG W distanciaG2J

angulo4 atan51distanciaG distancia92J

angulo5 calcularAngulo1L4 1L5 ; L32 s2J

// Co%e!os el ser%o a donde ?uere!os

ser%o(erecho.riteCicroseconds1Hoor111angulo4 ; angulo52 WRA+-*SER:*S2 T AC,LI(SER:*(ER22J // riteCicroseconds1int2 ;;>Canda un pulso de un ancho de tantos !icrosegundos co!o le especi&?ue!osen el par]ntesis de esta or!a el ser%o se colocar" en un deter!inadopunto. 4877;)entro 4777;I? $ 5777;(er

O

Codigo de Robot2

Documents

Transcript of Codigo de Robot2